diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b9e125..732dcd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(experimental_behaviors CXX) find_package(moveit_studio_common REQUIRED) moveit_studio_package() -set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs moveit_studio_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected) +set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs moveit_studio_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected control_msgs) foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${package} REQUIRED) endforeach() @@ -13,6 +13,7 @@ add_library( experimental_behaviors SHARED src/get_pose_stamped_from_topic.cpp + src/execute_tracked_trajectory.cpp src/register_behaviors.cpp) target_include_directories( experimental_behaviors diff --git a/include/experimental_behaviors/execute_tracked_trajectory.hpp b/include/experimental_behaviors/execute_tracked_trajectory.hpp new file mode 100644 index 0000000..3fde536 --- /dev/null +++ b/include/experimental_behaviors/execute_tracked_trajectory.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; + +namespace moveit_studio::behaviors +{ + +class ExecuteAndTrackJointTrajectory : public AsyncBehaviorBase +{ +public: + ExecuteAndTrackJointTrajectory(const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources); + ~ExecuteAndTrackJointTrajectory() override; + + static BT::PortsList providedPorts(); + + std::shared_future>& getFuture() override; + +protected: + tl::expected doWork() override; + tl::expected doHalt() override; + +private: + void handleTrigger(const std::shared_ptr res); + void processFeedback(const std::shared_ptr feedback); + trajectory_msgs::msg::JointTrajectory computeRemainderTrajectory(); + + rclcpp::Service::SharedPtr trigger_service_; + rclcpp_action::Client::SharedPtr action_client_; + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + trajectory_msgs::msg::JointTrajectory full_trajectory_; + double last_feedback_time_{ 0.0 }; + std::atomic triggered_{ false }; + std::shared_future> future_; +}; + +} // namespace moveit_studio::behaviors diff --git a/src/execute_tracked_trajectory.cpp b/src/execute_tracked_trajectory.cpp new file mode 100644 index 0000000..ab97a9f --- /dev/null +++ b/src/execute_tracked_trajectory.cpp @@ -0,0 +1,228 @@ +#include "experimental_behaviors/execute_tracked_trajectory.hpp" +namespace moveit_studio::behaviors +{ + +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; + +ExecuteAndTrackJointTrajectory::ExecuteAndTrackJointTrajectory(const std::string& name, + const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources) + : AsyncBehaviorBase(name, config, shared_resources) +{ +} + +ExecuteAndTrackJointTrajectory::~ExecuteAndTrackJointTrajectory() = default; + +BT::PortsList ExecuteAndTrackJointTrajectory::providedPorts() +{ + return { + BT::InputPort("action_name", "/joint_trajectory_controller/follow_joint_trajectory", + "Name of the FollowJointTrajectory action server"), + BT::InputPort("service_name", "/pause_trajectory", + "Name of the Trigger service for pausing trajectories"), + BT::InputPort("joint_trajectory_msg", "", + "JointTrajectory message to execute"), + BT::InputPort("goal_position_tolerance", 0.0, "Tolerance for position error (radians)"), + BT::InputPort("goal_time_tolerance", 0.0, "Tolerance for time error (seconds)"), + BT::InputPort("goal_duration_tolerance", -1.0, + "Time (in seconds) to wait before canceling the goal. Negative means wait indefinitely"), + BT::InputPort("goal_accept_tolerance", 3.0, "Time (in seconds) to wait for the goal to be accepted"), + BT::OutputPort("trajectory_remainder", "{joint_trajectory_remainder}", + "Remaining trajectory after cancellation"), + BT::OutputPort("time_from_start", "{time_from_start}", "Latest feedback time_from_start in seconds") + }; +} + +std::shared_future>& ExecuteAndTrackJointTrajectory::getFuture() +{ + return future_; +} + +tl::expected ExecuteAndTrackJointTrajectory::doWork() +{ + notifyCanHalt(); + + auto ports = + getRequiredInputs(getInput("action_name"), getInput("service_name"), + getInput("joint_trajectory_msg"), + getInput("goal_position_tolerance"), getInput("goal_time_tolerance"), + getInput("goal_accept_tolerance"), getInput("goal_duration_tolerance")); + if (!ports) + { + return tl::make_unexpected(std::string("Failed to get required input: ") + ports.error()); + } + auto [action_name, service_name, joint_trajectory, position_tolerance, time_tolerance, acceptance_tolerance, + duration_tolerance] = ports.value(); + full_trajectory_ = joint_trajectory; + + // Advertise cancellation trigger service + trigger_service_ = shared_resources_->node->create_service( + service_name, + [this](const std::shared_ptr, const std::shared_ptr, + std::shared_ptr res) { handleTrigger(res); }); + + action_client_ = rclcpp_action::create_client(shared_resources_->node, action_name); + if (!action_client_->wait_for_action_server(std::chrono::duration(acceptance_tolerance))) + { + trigger_service_.reset(); + return tl::make_unexpected("FollowJointTrajectory action server not available."); + } + + if (full_trajectory_.points.empty()) + { + return tl::make_unexpected("Empty JointTrajectory Message received, nothing to execute."); + } + if (position_tolerance < 0.0 || time_tolerance < 0.0) + { + return tl::make_unexpected("Position and time tolerances must be positive or zero."); + } + + // Build goal + FollowJointTrajectory::Goal goal; + goal.trajectory = full_trajectory_; + for (const auto& jn : goal.trajectory.joint_names) + { + control_msgs::msg::JointTolerance tol; + tol.name = jn; + tol.position = position_tolerance; + goal.goal_tolerance.push_back(tol); + } + goal.goal_time_tolerance = rclcpp::Duration::from_seconds(time_tolerance); + + // Send goal with feedback callback + typename rclcpp_action::Client::SendGoalOptions options; + options.feedback_callback = [this](auto, std::shared_ptr fb) { + processFeedback(fb); + }; + auto future_goal_handle = action_client_->async_send_goal(goal, options); + if (future_goal_handle.wait_for(std::chrono::duration(acceptance_tolerance)) == std::future_status::timeout) + { + trigger_service_.reset(); + return tl::make_unexpected("Timed out waiting for goal to be accepted."); + } + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) + { + trigger_service_.reset(); + return tl::make_unexpected("Goal request was rejected by the server."); + } + + // Wait for result (with optional timeout) + auto future_result = action_client_->async_get_result(goal_handle_); + if (duration_tolerance >= 0.0) + { + if (future_result.wait_for(std::chrono::duration(duration_tolerance)) == std::future_status::timeout) + { + action_client_->async_cancel_goal(goal_handle_); + trigger_service_.reset(); + return tl::make_unexpected("Timed out waiting for result."); + } + } + auto wrapped_result = future_result.get(); + + // Handle completion or cancellation + switch (wrapped_result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + trigger_service_.reset(); + return true; + + case rclcpp_action::ResultCode::CANCELED: + trigger_service_.reset(); + if (triggered_) + { + auto rem = computeRemainderTrajectory(); + setOutput("trajectory_remainder", rem); + return tl::make_unexpected("Trajectory halted"); + } + else + { + return tl::make_unexpected("Trajectory canceled unexpectedly"); + } + + case rclcpp_action::ResultCode::ABORTED: + trigger_service_.reset(); + return tl::make_unexpected("Action aborted: " + wrapped_result.result->error_string); + + default: + trigger_service_.reset(); + return tl::make_unexpected("Unknown action result code"); + } +} + +tl::expected ExecuteAndTrackJointTrajectory::doHalt() +{ + triggered_ = true; + if (goal_handle_) + { + try + { + action_client_->async_cancel_goal(goal_handle_); + } + catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) + { + RCLCPP_WARN(rclcpp::get_logger("ExecuteAndTrackJointTrajectory"), "doHalt cancel failed: %s", e.what()); + } + goal_handle_.reset(); + } + trigger_service_.reset(); + return {}; +} + +void ExecuteAndTrackJointTrajectory::handleTrigger(const std::shared_ptr res) +{ + triggered_ = true; + if (goal_handle_) + { + try + { + action_client_->async_cancel_goal(goal_handle_); + } + catch (const rclcpp_action::exceptions::UnknownGoalHandleError& e) + { + RCLCPP_WARN(rclcpp::get_logger("ExecuteAndTrackJointTrajectory"), "Failed to cancel goal: %s", e.what()); + } + goal_handle_.reset(); + } + res->success = true; + res->message = "Trajectory cancellation triggered"; +} + +void ExecuteAndTrackJointTrajectory::processFeedback( + const std::shared_ptr feedback) +{ + last_feedback_time_ = feedback->desired.time_from_start.sec + feedback->desired.time_from_start.nanosec * 1e-9; + setOutput("time_from_start", last_feedback_time_); +} + +trajectory_msgs::msg::JointTrajectory ExecuteAndTrackJointTrajectory::computeRemainderTrajectory() +{ + trajectory_msgs::msg::JointTrajectory new_traj; + new_traj.joint_names = full_trajectory_.joint_names; + + size_t start_index = 0; + for (; start_index < full_trajectory_.points.size(); ++start_index) + { + double t = full_trajectory_.points[start_index].time_from_start.sec + + full_trajectory_.points[start_index].time_from_start.nanosec * 1e-9; + if (t >= last_feedback_time_) + break; + } + if (start_index >= full_trajectory_.points.size()) + return new_traj; + + double offset = full_trajectory_.points[start_index].time_from_start.sec + + full_trajectory_.points[start_index].time_from_start.nanosec * 1e-9; + for (size_t i = start_index; i < full_trajectory_.points.size(); ++i) + { + auto pt = full_trajectory_.points[i]; + double orig = pt.time_from_start.sec + pt.time_from_start.nanosec * 1e-9; + double adjusted = orig - offset; + pt.time_from_start.sec = static_cast(adjusted); + pt.time_from_start.nanosec = static_cast((adjusted - pt.time_from_start.sec) * 1e9); + new_traj.points.push_back(pt); + } + return new_traj; +} + +} // namespace moveit_studio::behaviors diff --git a/src/register_behaviors.cpp b/src/register_behaviors.cpp index 6084482..c015ab0 100644 --- a/src/register_behaviors.cpp +++ b/src/register_behaviors.cpp @@ -2,6 +2,7 @@ #include #include +#include "experimental_behaviors/execute_tracked_trajectory.hpp" #include "experimental_behaviors/get_pose_stamped_from_topic.hpp" #include @@ -17,6 +18,8 @@ class ExperimentalBehaviorsLoader : public moveit_studio::behaviors::SharedResou { moveit_studio::behaviors::registerBehavior(factory, "GetPoseStampedFromTopic", shared_resources); + moveit_studio::behaviors::registerBehavior( + factory, "ExecuteAndTrackJointTrajectory", shared_resources); } }; } // namespace experimental_behaviors