Improve Error Handling, Add Named Pose Service, Optimize Motion Planning, and Enhance Feedback Mechanisms

- Set success flags on results when actions complete successfully in both `mtp_jeet_cart.cpp` and `mtp_jtc.cpp`, providing proper feedback to clients.
- Increased trajectory planning granularity by adjusting the divisor from 0.01 to 0.001 in `mtp_jtc_cart.cpp`, resulting in smoother robot movements.
- Introduced a new service `GetNamedPose` defined in `GetNamedPose.srv`, allowing nodes to request specific poses by name, enhancing modularity and reusability.
- Optimized motion planning in `mtp_jtc.cpp` by immediately succeeding when no movement is needed, reducing unnecessary computations.
- Updated `CMakeLists.txt` to include the new service

rbs_bt_executor/src/MoveToJointStates.cpp

@@ -18,17 +18,19 @@ public:
 
   static PortsList providedPorts() {
     return providedBasicPorts({InputPort<std::string>("robot_name"),
-                               InputPort<std::vector<double>>("JointState")});
+                               InputPort<std::vector<double>>("joint_state"),
+                               InputPort<double>("duration")});
   }
 
   bool setGoal(RosActionNode::Goal &goal) override {
     getInput("robot_name", goal.robot_name);
-    getInput("JointState", goal.joint_values);
+    getInput("joint_state", goal.joint_values);
+    getInput("duration", goal.duration);
     return true;
   }
 
   NodeStatus onResultReceived(const WrappedResult &wr) override {
-
+    RCLCPP_INFO(this->logger(), "Starting get response %s with status %b", this->action_name_.c_str(), wr.result->success);
     if (!wr.result->success) {
       return NodeStatus::FAILURE;
     }

rbs_bt_executor/src/MoveToPose.cpp

@@ -34,7 +34,7 @@ public:
 
   NodeStatus onResultReceived(const WrappedResult &wr) override {
 
-    RCLCPP_INFO(this->logger(), "Starting get response");
+    RCLCPP_INFO(this->logger(), "Starting get response %s with status %b", this->action_name_.c_str(), wr.result->success);
     if (!wr.result->success) {
       return NodeStatus::FAILURE;
     }

rbs_skill_servers/src/mtjs_jtc.cpp

@@ -83,6 +83,7 @@ protected:
                    &wrapped_result) {
           if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
             RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+            m_current_result->success = true;
             m_current_goal_handle->succeed(m_current_result);
           } else {
             RCLCPP_ERROR(this->get_logger(), "Goal failed");

rbs_skill_servers/src/mtp_cart.cpp

@@ -72,7 +72,7 @@ private:
       m_current_pose_subscriber;
   geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose;
   std::string m_base_link;
-  double m_threshold_distance = 0.02;
+  double m_threshold_distance = 0.01;
 
   std::vector<geometry_msgs::msg::Pose> m_trajectory;
   size_t m_current_step;
@@ -92,7 +92,14 @@ private:
         RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
         m_trajectory_timer->cancel();
       } else {
-        publish_pose(m_trajectory.back());
+        if (distance > m_threshold_distance) {
+          publish_pose(m_trajectory.back());
+        } else {
+          m_current_result->success = true;
+          m_current_goal_handle->succeed(m_current_result);
+          RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
+          m_trajectory_timer->cancel();
+        }
       }
     }
   }
@@ -139,7 +146,7 @@ private:
         normalize_orientation(end_pose.orientation);
 
     double distance = (end_position - start_position).norm();
-    int num_steps = static_cast<int>(distance / 0.01);
+    int num_steps = static_cast<int>(distance / 0.001);
 
     for (int i = 0; i <= num_steps; ++i) {
       double t = static_cast<double>(i) / num_steps;

rbs_skill_servers/src/mtp_jtc_cart.cpp

@@ -75,9 +75,11 @@ public:
     auto base_link = this->get_parameter("base_link").as_string();
     auto robot_ee_link = this->get_parameter("ee_link").as_string();
     if (base_link.empty() or robot_ee_link.empty()) {
-      RCLCPP_ERROR(this->get_logger(),
-                   "Describe robot end-effector link and base link to continue");
-      throw std::runtime_error("Describe robot end-effector link and base link to continue");
+      RCLCPP_ERROR(
+          this->get_logger(),
+          "Describe robot end-effector link and base link to continue");
+      throw std::runtime_error(
+          "Describe robot end-effector link and base link to continue");
     }
 
     if (!kdl_tree.getChain(base_link, robot_ee_link, m_kdl_chain)) {
@@ -135,6 +137,7 @@ protected:
                    &wrapped_result) {
           if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
             RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+            m_current_result->success = true;
             m_current_goal_handle->succeed(m_current_result);
           } else {
             RCLCPP_ERROR(this->get_logger(), "Goal failed");

rbs_utils/rbs_utils_interfaces/CMakeLists.txt

@@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "srv/GetGraspPose.srv"
     "srv/GetPlacePose.srv"
     "srv/GetWorkspace.srv"
+    "srv/GetNamedPose.srv"
     DEPENDENCIES std_msgs geometry_msgs
 )
 

rbs_utils/rbs_utils_interfaces/srv/GetNamedPose.srv

@@ -0,0 +1,4 @@
+string pose_name
+---
+rbs_utils_interfaces/NamedPose named_pose
+bool ok