Sequential multi-pose planning in PlannerServer

grab2_msgs/action/ComputePlanThroughPoses.action

@@ -1,9 +1,10 @@
-#Goal Definition
+# ----- Goal Definition -----
 std_msgs/Header header
 geometry_msgs/PoseStamped[] goals
-float64 interpolation_dt  0.1 # Optional (Seconds): used while forming trajectory msg
+float64 interpolation_dt  0.1   # Optional (seconds), used when forming the trajectory message
+
 ---
-#Result Definition: modified copy of control_msgs/action/FollowJointTrajectory
+# ----- Result Definition -----
 trajectory_msgs/JointTrajectory trajectory
 
 int32 error_code
@@ -12,13 +13,20 @@ int32 INVALID_GOAL = -1
 int32 INVALID_JOINTS = -2
 int32 OLD_HEADER_TIMESTAMP = -3
 
-# Human readable description of the error code. Contains complementary
-# information that is especially useful when execution fails, for instance:
-# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested
-#   trajectory is in the past).
-# - INVALID_JOINTS: The mismatch between the expected controller joints
-#   and those provided in the goal.
+# Human-readable explanation of the error
+# Examples:
+#  - INVALID_GOAL: The reason for the invalid goal (e.g., no goals provided or unreachable pose)
+#  - INVALID_JOINTS: The mismatch between expected and provided joints
+#  - OLD_HEADER_TIMESTAMP: Timestamp in the past
 string error_string
 
+float64 total_duration        # Total time of the combined trajectory (seconds)
+int32 successful_count        # Number of poses planned successfully
+int32 failed_count            # Number of poses that failed
+int32[] failed_indices        # Indices of poses that failed to plan
+
 ---
-#Feedback Definition
+
+# ----- Feedback Definition -----
+float32 progress_percentage    # (0–100) Planning progress feedback
+string current_status          # Optional short text (e.g., "Planning pose 3/5")

grab2_planner/CMakeLists.txt

@@ -24,9 +24,11 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(moveit_msgs REQUIRED)
-find_package(moveit_visual_tools REQUIRED)
+# find_package(moveit_visual_tools REQUIRED) for now
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_core REQUIRED)
+
 
 set(executable_name planner_server)
 set(library_name ${executable_name}_core)

grab2_planner/include/grab2_planner/planner_server.hpp

@@ -5,6 +5,9 @@
 
 #include <string>
 #include <memory>
+#include <future>
+#include <mutex>
+#include <thread>
 
 #include "rclcpp/version.h"
 #include "rclcpp/rclcpp.hpp"
@@ -55,6 +58,8 @@ class PlannerServer : public grab2::Node
   rclcpp_action::Server<ActionToPose>::SharedPtr action_server_pose_;
   rclcpp_action::Server<ActionThroughPoses>::SharedPtr action_server_poses_;
   moveit::planning_interface::MoveGroupInterfaceUniquePtr move_group_interface_;
+  std::mutex planning_mutex_;
+  rclcpp::Node::SharedPtr node_;
 };
 
 }  // namespace grab2_planner

grab2_planner/package.xml

@@ -14,7 +14,8 @@
   <depend>rclcpp_components</depend>
   <depend>tf2_ros</depend>
   <depend>std_msgs</depend>
-  <depend>control_msgs</depend>
+  <depend condition="$ROS_DISTRO == 'humble'">control_msgs</depend>
+  <depend condition="$ROS_DISTRO == 'jazzy'">ros2_control_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>grab2_msgs</depend>
@@ -23,10 +24,11 @@
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_visual_tools</depend>
-  <depend>moveit_configs_utils</depend>
-  <depend>moveit_resources_panda_moveit_config</depend>
+  <!-- <depend>moveit_visual_tools</depend> -->
+  <!-- <depend>moveit_configs_utils</depend> -->
+  <depend condition="$ROS_DISTRO == 'humble'">moveit_resources_panda_moveit_config</depend>
   <depend>grab2_ros_common</depend>
+  <depend>moveit_core</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

grab2_planner/src/planner_server.cpp

@@ -88,18 +88,97 @@ void PlannerServer::computePlanThroughPoses(
   const auto goal = goal_handle->get_goal();
   auto result = std::make_shared<ActionThroughPoses::Result>();
 
-  // Check if we need to initialize
   if (!initialized_) {
     initialize();
   }
 
-  // TODO(ElSayed): Implement your planning logic here
-  // Example:
-  // - Use move_group_interface_ to plan through goal->target_poses
-  // - Fill result->trajectory
+  if (goal->goals.empty()) {
+    result->error_code = result->INVALID_GOAL;
+    result->error_string = "No goals provided";
+    goal_handle->abort(result);
+    return;
+  }
+
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
+  moveit_msgs::msg::RobotTrajectory combined_trajectory;
+  std::vector<size_t> successful_indices;
+  std::vector<size_t> failed_indices;
+
+  moveit::core::RobotStatePtr current_state = move_group_interface_->getCurrentState();
+  move_group_interface_->setStartState(*current_state);
+
+  double total_duration = 0.0;
+
+  for (size_t i = 0; i < goal->goals.size(); ++i) {
+    const auto & pose = goal->goals[i].pose;
+    move_group_interface_->setPoseTarget(pose);
+
+    RCLCPP_INFO(this->get_logger(), "Planning for pose %zu...", i);
+    bool success = static_cast<bool>(move_group_interface_->plan(plan));
+
+    if (!success) {
+      RCLCPP_WARN(this->get_logger(), "Failed to plan for pose index %zu", i);
+      failed_indices.push_back(i);
+      continue;
+    }
+
+    // For Jazzy and Later Support
+    #if RCLCPP_VERSION_GTE(28, 0, 0)
+    const auto & traj = plan.trajectory.joint_trajectory;
+    #else
+    const auto & traj = plan.trajectory_.joint_trajectory;
+    #endif
+
+    // Append trajectory points
+    if (combined_trajectory.joint_trajectory.joint_names.empty()) {
+      combined_trajectory.joint_trajectory.joint_names = traj.joint_names;
+    }
+    combined_trajectory.joint_trajectory.points.insert(
+      combined_trajectory.joint_trajectory.points.end(),
+      traj.points.begin(),
+      traj.points.end()
+    );
+
+    // Add duration (time_from_start of last point)
+    if (!traj.points.empty()) {
+      total_duration += traj.points.back().time_from_start.sec +
+        traj.points.back().time_from_start.nanosec * 1e-9;
+    }
+
+    successful_indices.push_back(i);
+
+    // Update start state
+    moveit::core::RobotState next_state(*current_state);
+    next_state.setVariablePositions(traj.joint_names, traj.points.back().positions);
+    move_group_interface_->setStartState(next_state);
+    *current_state = next_state;
+  }
+
+  if (successful_indices.empty()) {
+    result->error_code = result->INVALID_GOAL;
+    result->error_string = "Failed to plan any valid trajectories.";
+    goal_handle->abort(result);
+    return;
+  }
+
+  result->trajectory = combined_trajectory.joint_trajectory;
+  result->total_duration = total_duration;
+  result->successful_count = successful_indices.size();
+  result->failed_count = failed_indices.size();
+
+  // for (auto idx : failed_indices) {
+  //   result->failed_indices.push_back((idx));
+  // }
+
+  result->error_code = result->SUCCESSFUL;
+  result->error_string = "Planning completed successfully.";
+
+  goal_handle->succeed(result);
 
-  // For now, just abort
-  goal_handle->abort(result);
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Planning finished: %zu successful, %zu failed, total duration %.2f sec",
+    successful_indices.size(), failed_indices.size(), total_duration);
 }
 
 }  // namespace grab2_planner