Merge branch 'ros-planning:main' into retime_trajectory

moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp

@@ -239,7 +239,11 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP
     setStartState(motion_plan_responses, req.group_name, req.start_state);
 
     planning_interface::MotionPlanResponse res;
-    planning_pipeline->generatePlan(planning_scene, req, res);
+    if (!planning_pipeline->generatePlan(planning_scene, req, res))
+    {
+      RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed.");
+      res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
+    }
     if (res.error_code.val != res.error_code.SUCCESS)
     {
       std::ostringstream os;

moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

@@ -119,10 +119,6 @@ BenchmarkExecutor::~BenchmarkExecutor()
       RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str());
       continue;
     }
-
-    // Disable visualizations
-    pipeline->displayComputedMotionPlans(false);
-    pipeline->checkSolutionPaths(false);
   }
 
   // Error check

moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

@@ -46,8 +46,13 @@
 
 namespace move_group
 {
-static const rclcpp::Logger LOGGER =
-    rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability");
+
+namespace
+{
+const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability");
+constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true;
+constexpr bool CHECK_SOLUTION_PATHS = true;
+}  // namespace
 
 MoveGroupMoveAction::MoveGroupMoveAction()
   : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false }
@@ -69,8 +74,9 @@ void MoveGroupMoveAction::initialize()
         preemptMoveCallback();
         return rclcpp_action::CancelResponse::ACCEPT;
       },
-      [this](std::shared_ptr<MGActionGoal> goal) {
-        std::thread{ std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1), goal }.detach();
+      [this](const std::shared_ptr<MGActionGoal>& goal) {
+        std::thread{ [this](const std::shared_ptr<move_group::MGActionGoal>& goal) { executeMoveCallback(goal); }, goal }
+            .detach();
       });
 }
 
@@ -211,7 +217,12 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 
   try
   {
-    planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res);
+    if (!planning_pipeline->generatePlan(the_scene, goal->get_goal()->request, res, context_->debug_,
+                                         CHECK_SOLUTION_PATHS, DISPLAY_COMPUTED_MOTION_PLANS))
+    {
+      RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed.");
+      res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
+    }
   }
   catch (std::exception& ex)
   {
@@ -243,7 +254,8 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo
   planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor);
   try
   {
-    solved = planning_pipeline->generatePlan(plan.planning_scene, req, res);
+    solved = planning_pipeline->generatePlan(plan.planning_scene, req, res, context_->debug_, CHECK_SOLUTION_PATHS,
+                                             DISPLAY_COMPUTED_MOTION_PLANS);
   }
   catch (std::exception& ex)
   {

moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp

@@ -42,8 +42,12 @@
 
 namespace move_group
 {
-static const rclcpp::Logger LOGGER =
-    rclcpp::get_logger("moveit_move_group_default_capabilities.plan_service_capability");
+namespace
+{
+const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.plan_service_capability");
+constexpr bool DISPLAY_COMPUTED_MOTION_PLANS = true;
+constexpr bool CHECK_SOLUTION_PATHS = true;
+}  // namespace
 
 MoveGroupPlanService::MoveGroupPlanService() : MoveGroupCapability("MotionPlanService")
 {
@@ -82,7 +86,12 @@ bool MoveGroupPlanService::computePlanService(const std::shared_ptr<rmw_request_
   try
   {
     planning_interface::MotionPlanResponse mp_res;
-    planning_pipeline->generatePlan(ps, req->motion_plan_request, mp_res);
+    if (!planning_pipeline->generatePlan(ps, req->motion_plan_request, mp_res, context_->debug_, CHECK_SOLUTION_PATHS,
+                                         DISPLAY_COMPUTED_MOTION_PLANS))
+    {
+      RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed.");
+      mp_res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
+    }
     mp_res.getMessage(res->motion_plan_response);
   }
   catch (std::exception& ex)

moveit_ros/move_group/src/move_group_context.cpp

@@ -56,13 +56,6 @@ move_group::MoveGroupContext::MoveGroupContext(const moveit_cpp::MoveItCppPtr& m
   if (default_pipeline_it != pipelines.end())
   {
     planning_pipeline_ = default_pipeline_it->second;
-
-    // configure the planning pipeline
-    planning_pipeline_->displayComputedMotionPlans(true);
-    planning_pipeline_->checkSolutionPaths(true);
-
-    if (debug_)
-      planning_pipeline_->publishReceivedRequests(true);
   }
   else
   {

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -46,8 +46,14 @@ servo:
                   must be passed to the node during launch time."
   }
 
-############################# INCOMING COMMAND SETTINGS ########################
+  active_subgroup: {
+    type: string,
+    default_value: "",
+    description: "This parameter can be used to switch online to actuating a subgroup of the move group. \
+    If it is empty, the full move group is actuated."
+  }
 
+############################# INCOMING COMMAND SETTINGS ########################
   pose_command_in_topic: {
     type: string,
     read_only: true,

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -214,6 +214,9 @@ class Servo
   std::unique_ptr<CollisionMonitor> collision_monitor_;
 
   pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_ = nullptr;
+
+  // Map between joint subgroup names and corresponding joint name - move group indices map
+  std::unordered_map<std::string, JointNameToMoveGroupIndexMap> joint_name_to_index_maps_;
 };
 
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp

@@ -55,39 +55,47 @@ namespace moveit_servo
  * @param command The joint jog command.
  * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
  * @param servo_params The servo parameters.
+ * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position.
  * @return The status and joint position change required (delta).
  */
 JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state,
-                                        const servo::Params& servo_params);
+                                        const servo::Params& servo_params,
+                                        const JointNameToMoveGroupIndexMap& joint_name_group_index_map);
 
 /**
  * \brief Compute the change in joint position for the given twist command.
  * @param command The twist command.
  * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
  * @param servo_params The servo parameters.
+ * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position.
  * @return The status and joint position change required (delta).
  */
 JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
-                                     const servo::Params& servo_params);
+                                     const servo::Params& servo_params,
+                                     const JointNameToMoveGroupIndexMap& joint_name_group_index_map);
 
 /**
  * \brief Compute the change in joint position for the given pose command.
  * @param command The pose command.
  * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
  * @param servo_params The servo parameters.
+ * @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position
  * @return The status and joint position change required (delta).
  */
 JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state,
-                                    const servo::Params& servo_params);
+                                    const servo::Params& servo_params,
+                                    const JointNameToMoveGroupIndexMap& joint_name_group_index_map);
 
 /**
  * \brief Computes the required change in joint angles for given Cartesian change, using the robot's IK solver.
  * @param cartesian_position_delta The change in Cartesian position.
  * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
  * @param servo_params The servo parameters.
+ * @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position.
  * @return The status and joint position change required (delta).
  */
 JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta,
-                                  const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params);
+                                  const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params,
+                                  const JointNameToMoveGroupIndexMap& joint_name_group_index_map);
 
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp

@@ -129,4 +129,7 @@ struct KinematicState
   }
 };
 
+// Mapping joint names and their position in the move group vector
+typedef std::unordered_map<std::string, std::size_t> JointNameToMoveGroupIndexMap;
+
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo.cpp

@@ -63,8 +63,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
 {
   servo_params_ = servo_param_listener_->get_params();
 
-  const bool params_valid = validateParams(servo_params_);
-  if (!params_valid)
+  if (!validateParams(servo_params_))
   {
     RCLCPP_ERROR_STREAM(LOGGER, "Got invalid parameters, exiting.");
     std::exit(EXIT_FAILURE);
@@ -117,8 +116,38 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
     collision_monitor_->start();
 
     servo_status_ = StatusCode::NO_WARNING;
-    RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully");
   }
+
+  const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel()
+                                           ->getJointModelGroup(servo_params_.move_group_name)
+                                           ->getActiveJointModelNames();
+  // Create subgroup map
+  for (const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames())
+  {
+    // Skip move group
+    if (sub_group_name == servo_params_.move_group_name)
+    {
+      continue;
+    }
+    const auto& subgroup_joint_names =
+        planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames();
+
+    JointNameToMoveGroupIndexMap new_map;
+    // For each joint name of the subgroup calculate the index in the move group joint vector
+    for (const auto& joint_name : subgroup_joint_names)
+    {
+      // Find subgroup joint name in move group joint names
+      const auto move_group_iterator =
+          std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name);
+      // Calculate position and add a new mapping of joint name to move group joint vector position
+      new_map.insert(std::make_pair<std::string, std::size_t>(
+          std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator)));
+    }
+    // Add new joint name to index map to existing maps
+    joint_name_to_index_maps_.insert(
+        std::make_pair<std::string, JointNameToMoveGroupIndexMap>(std::string(sub_group_name), std::move(new_map)));
+  }
+  RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully");
 }
 
 Servo::~Servo()
@@ -161,7 +190,6 @@ void Servo::setCollisionChecking(const bool check_collision)
 bool Servo::validateParams(const servo::Params& servo_params) const
 {
   bool params_valid = true;
-
   auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
   if (joint_model_group == nullptr)
@@ -205,30 +233,32 @@ bool Servo::validateParams(const servo::Params& servo_params) const
     params_valid = false;
   }
 
+  if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name &&
+      !joint_model_group->isSubgroup(servo_params.active_subgroup))
+  {
+    RCLCPP_ERROR(LOGGER,
+                 "The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.",
+                 servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str());
+    params_valid = false;
+  }
+
   return params_valid;
 }
 
 bool Servo::updateParams()
 {
   bool params_updated = false;
-
   if (servo_param_listener_->is_old(servo_params_))
   {
-    auto params = servo_param_listener_->get_params();
+    const auto params = servo_param_listener_->get_params();
 
-    const bool params_valid = validateParams(params);
-    if (params_valid)
+    if (validateParams(params))
     {
       if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor)
       {
         RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : "
                                        << std::to_string(params.override_velocity_scaling_factor));
       }
-      if (params.move_group_name != servo_params_.move_group_name)
-      {
-        RCLCPP_INFO_STREAM(LOGGER, "Move group changed from " << servo_params_.move_group_name << " to "
-                                                              << params.move_group_name);
-      }
 
       servo_params_ = params;
       params_updated = true;
@@ -310,6 +340,12 @@ KinematicState Servo::haltJoints(const std::vector<int>& joints_to_halt, const K
 
 Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state)
 {
+  // Determine joint_name_group_index_map, if no subgroup is active, the map is empty
+  const auto& joint_name_group_index_map =
+      (!servo_params_.active_subgroup.empty() && servo_params_.active_subgroup != servo_params_.move_group_name) ?
+          joint_name_to_index_maps_.at(servo_params_.active_subgroup) :
+          JointNameToMoveGroupIndexMap();
+
   const int num_joints =
       robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
   Eigen::VectorXd joint_position_deltas(num_joints);
@@ -322,15 +358,17 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
   {
     if (expected_type == CommandType::JOINT_JOG)
     {
-      delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_);
+      delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_,
+                                            joint_name_group_index_map);
       servo_status_ = delta_result.first;
     }
     else if (expected_type == CommandType::TWIST)
     {
       try
       {
         const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(command));
-        delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_);
+        delta_result =
+            jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
         servo_status_ = delta_result.first;
       }
       catch (tf2::TransformException& ex)
@@ -344,7 +382,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
       try
       {
         const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(command));
-        delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_);
+        delta_result =
+            jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
         servo_status_ = delta_result.first;
       }
       catch (tf2::TransformException& ex)