Code cleanup & MoveIt best practices (moveit#351)

* Export missing plugins
* Use std::chrono_literals
* Construct smart pointers with std::make_* instead of 'new'
* Fixup const-ref in function signatures, reduce copies
* Improve planning scene locking, robot state processing, controller logic

moveit_ros/hybrid_planning/global_planner/include/moveit/global_planner/global_planner_component.h

@@ -102,7 +102,7 @@ class GlobalPlannerComponent : public rclcpp::Node
   bool init();
 
   // Plan global trajectory
-  moveit_msgs::msg::MotionPlanResponse plan(moveit_msgs::msg::MotionPlanRequest planning_problem);
+  moveit_msgs::msg::MotionPlanResponse plan(const moveit_msgs::msg::MotionPlanRequest& planning_problem);
 };
 
 }  // namespace moveit_hybrid_planning

moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp

@@ -45,6 +45,7 @@
 
 namespace moveit_hybrid_planning
 {
+using namespace std::chrono_literals;
 const rclcpp::Logger LOGGER = rclcpp::get_logger("global_planner_component");
 constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin";
 
@@ -79,7 +80,7 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option
   global_trajectory_pub_ = this->create_publisher<moveit_msgs::msg::MotionPlanResponse>("global_trajectory", 1);
 
   // Initialize global planner after construction
-  timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
+  timer_ = this->create_wall_timer(1ms, [this]() {
     if (initialized_)
     {
       timer_->cancel();
@@ -100,18 +101,14 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option
 bool GlobalPlannerComponent::init()
 {
   auto node_ptr = shared_from_this();
+
   // Configure planning scene monitor
-  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(
-      node_ptr, "robot_description", tf_buffer_, "global_planner/planning_scene_monitor"));
+  planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
+      node_ptr, "robot_description", tf_buffer_, "global_planner/planning_scene_monitor");
+
   // Allows us to sycronize to Rviz and also publish collision objects to ourselves
   RCLCPP_DEBUG(LOGGER, "Configuring Planning Scene Monitor");
-  if (planning_scene_monitor_->getPlanningScene())
-  {
-    // Start state and scene monitors
-    RCLCPP_INFO(LOGGER, "Listening to '/publish_planning_scene' for planning scene updates");
-    planning_scene_monitor_->startSceneMonitor();
-  }
-  else
+  if (!planning_scene_monitor_->getPlanningScene())
   {
     const std::string error = "Unable to configure planning scene monitor";
     RCLCPP_FATAL(LOGGER, error);
@@ -127,6 +124,12 @@ bool GlobalPlannerComponent::init()
     throw std::runtime_error(error);
   }
 
+  // Start state and scene monitors
+  RCLCPP_INFO(LOGGER, "Starting planning scene monitors");
+  planning_scene_monitor_->startSceneMonitor();
+  planning_scene_monitor_->startWorldGeometryMonitor();
+  planning_scene_monitor_->startStateMonitor();
+
   // Load planning pipelines
   // TODO(sjahr) Use refactored MoveItCpp instance (load only planning pipeline and planning scene monitor) to reduce redundancy
   for (const auto& planning_pipeline_name : config_.pipeline_names)
@@ -137,9 +140,8 @@ bool GlobalPlannerComponent::init()
       continue;
     }
     RCLCPP_INFO(LOGGER, "Loading planning pipeline '%s'", planning_pipeline_name.c_str());
-    planning_pipeline::PlanningPipelinePtr pipeline;
-    pipeline.reset(
-        new planning_pipeline::PlanningPipeline(robot_model_, node_ptr, planning_pipeline_name, PLANNING_PLUGIN_PARAM));
+    auto pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(
+        robot_model_, node_ptr, planning_pipeline_name, PLANNING_PLUGIN_PARAM);
     if (!pipeline->getPlannerManager())
     {
       RCLCPP_ERROR(LOGGER, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
@@ -176,37 +178,41 @@ void GlobalPlannerComponent::globalPlanningRequestCallback(
   last_global_solution_ = planning_solution;  // TODO(sjahr) Add Service to expose this
 };
 
-moveit_msgs::msg::MotionPlanResponse GlobalPlannerComponent::plan(moveit_msgs::msg::MotionPlanRequest planning_problem)
+moveit_msgs::msg::MotionPlanResponse
+GlobalPlannerComponent::plan(const moveit_msgs::msg::MotionPlanRequest& planning_problem)
 {
-  // Clone current planning scene
+  // Result
+  moveit_msgs::msg::MotionPlanResponse planning_solution;
+
+  // Update planning scene and lock for planning
+  planning_scene_monitor_->getStateMonitor()->waitForCurrentState();
   planning_scene_monitor_->updateFrameTransforms();
   planning_scene_monitor_->lockSceneRead();  // LOCK planning scene
-  ::planning_scene::PlanningScenePtr planning_scene =
-      ::planning_scene::PlanningScene::clone(planning_scene_monitor_->getPlanningScene());
-  planning_scene_monitor_->unlockSceneRead();  // UNLOCK planning scene
 
-  // TODO(sjahr) Review start/current robot state considerations
-  moveit::core::robotStateToRobotStateMsg(planning_scene->getCurrentState(), planning_problem.start_state);
-
-  moveit_msgs::msg::MotionPlanResponse planning_solution;
-
-  // Set goal constraints
-  ::planning_interface::MotionPlanResponse response;
-  if (planning_problem.goal_constraints.empty())
+  // Validate MotionPlanRequest
+  // Assume, there is only one planning pipeline TODO(sjahr) expand Request
+  const auto& pipeline = planning_pipelines_.at(config_.pipeline_names[0]);
+  if (!pipeline->getPlannerManager()->canServiceRequest(planning_problem))
   {
-    RCLCPP_ERROR(LOGGER, "No goal constraints set for planning request");
+    RCLCPP_ERROR(LOGGER, "Planner can't service MotionPlanRequest");
     planning_solution.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
     return planning_solution;
   }
 
-  // Run planning attempt
-  const planning_pipeline::PlanningPipelinePtr pipeline = planning_pipelines_.at(
-      config_.pipeline_names[0]);  // Assume, there is only one planning pipeline TODO(sjahr) expand Request
-  pipeline->generatePlan(planning_scene, planning_problem, response);
+  // Run planning attempt with current state as start state, unlock scene right after
+  const planning_scene::PlanningSceneConstPtr scene = planning_scene_monitor_->getPlanningScene();
+  // TODO(sjahr): do we need to initialize start_state? by default the current state of the scene should be used
+  auto motion_plan_request = planning_problem;
+  moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), motion_plan_request.start_state);
+  ::planning_interface::MotionPlanResponse response;
+  pipeline->generatePlan(scene, motion_plan_request, response);
+  planning_scene_monitor_->unlockSceneRead();  // UNLOCK planning scene
+
   if (response.error_code_.val != response.error_code_.SUCCESS)
   {
-    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
+    RCLCPP_ERROR(LOGGER, "Failed to compute global trajectory");
   }
+
   response.getMessage(planning_solution);
   return planning_solution;
 }

moveit_ros/hybrid_planning/hybrid_planning_manager/include/moveit/hybrid_planning_manager/planner_logic_interface.h

@@ -56,10 +56,11 @@ class PlannerLogicInterface
 public:
   /**
    * Initialize the planner logic
-   * @param hybrid_planner_handle Handle to access the hybrid planning manager's member functions and communication interfaces.
+   * @param hybrid_planning_manager The hybrid planning manager instance to initialize this logic with.
    * @return true if initialization was successful
    */
-  virtual bool initialize(std::shared_ptr<moveit_hybrid_planning::HybridPlanningManager> hybrid_planner_handle) = 0;
+  virtual bool
+  initialize(const std::shared_ptr<moveit_hybrid_planning::HybridPlanningManager>& hybrid_planning_manager) = 0;
 
   /**
    * React to event defined in BasicHybridPlanningEvent enum
@@ -73,14 +74,14 @@ class PlannerLogicInterface
    * @param event Encoded as string
    * @return true if reaction was successful
    */
-  virtual bool react(std::string event) = 0;
+  virtual bool react(const std::string& event) = 0;
   virtual ~PlannerLogicInterface(){};
 
 protected:
   /** \brief Constructor */
   PlannerLogicInterface(){};
 
-  // Handle to access communication interfaces and member functions of hybrid planning manager instance that uses the planner plugin
-  std::shared_ptr<moveit_hybrid_planning::HybridPlanningManager> hybrid_planner_handle_ = nullptr;
+  // The hybrid planning manager instance that runs this logic plugin
+  std::shared_ptr<moveit_hybrid_planning::HybridPlanningManager> hybrid_planning_manager_ = nullptr;
 };
 }  // namespace moveit_hybrid_planning

moveit_ros/hybrid_planning/hybrid_planning_manager/src/hybrid_planning_manager.cpp

@@ -40,14 +40,15 @@
 
 namespace moveit_hybrid_planning
 {
+using namespace std::chrono_literals;
 const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager");
 
 HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options)
   : Node("hybrid_planning_manager", options)
 {
   initialized_ = false;
   // Initialize hybrid planning component after after construction
-  timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
+  timer_ = this->create_wall_timer(1ms, [this]() {
     if (initialized_)
     {
       timer_->cancel();
@@ -70,8 +71,9 @@ bool HybridPlanningManager::initialize()
   // Load planning logic plugin
   try
   {
-    planner_logic_plugin_loader_.reset(new pluginlib::ClassLoader<moveit_hybrid_planning::PlannerLogicInterface>(
-        "moveit_hybrid_planning", "moveit_hybrid_planning::PlannerLogicInterface"));
+    planner_logic_plugin_loader_ =
+        std::make_unique<pluginlib::ClassLoader<moveit_hybrid_planning::PlannerLogicInterface>>(
+            "moveit_hybrid_planning", "moveit_hybrid_planning::PlannerLogicInterface");
   }
   catch (pluginlib::PluginlibException& ex)
   {
@@ -105,7 +107,7 @@ bool HybridPlanningManager::initialize()
   // Initialize local planning action client
   local_planner_action_client_ =
       rclcpp_action::create_client<moveit_msgs::action::LocalPlanner>(this, "local_planning_action");
-  if (!local_planner_action_client_->wait_for_action_server(std::chrono::seconds(2)))
+  if (!local_planner_action_client_->wait_for_action_server(2s))
   {
     const std::string error = "Local planner action server not available after waiting";
     RCLCPP_FATAL(LOGGER, error);
@@ -115,7 +117,7 @@ bool HybridPlanningManager::initialize()
   // Initialize global planning action client
   global_planner_action_client_ =
       rclcpp_action::create_client<moveit_msgs::action::GlobalPlanner>(this, "global_planning_action");
-  if (!global_planner_action_client_->wait_for_action_server(std::chrono::seconds(2)))
+  if (!global_planner_action_client_->wait_for_action_server(2s))
   {
     const std::string error = "Global planner action server not available after waiting";
     RCLCPP_FATAL(LOGGER, error);

moveit_ros/hybrid_planning/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/decelerate_before_collision.h

@@ -60,18 +60,19 @@ class DecelerateBeforeCollision : public LocalConstraintSolverInterface
   DecelerateBeforeCollision();
   ~DecelerateBeforeCollision() override{};
   bool initialize(const rclcpp::Node::SharedPtr& node,
-                  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor) override;
+                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                  const std::string& group_name) override;
 
-  trajectory_msgs::msg::JointTrajectory
-  solve(robot_trajectory::RobotTrajectory local_trajectory,
-        std::vector<moveit_msgs::msg::Constraints> local_constraints,
-        std::shared_ptr<moveit_msgs::action::LocalPlanner::Feedback> feedback) override;
+  moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+                                                    const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+                                                    trajectory_msgs::msg::JointTrajectory& local_solution) override;
 
 private:
   rclcpp::Node::SharedPtr node_handle_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
   bool path_invalidation_event_send_;  // Send path invalidation event only once
-  std::vector<control_toolbox::Pid> joint_position_pids_;
+  // Map from joint name to controller
+  std::map<std::string, control_toolbox::Pid> joint_position_pids_;
   PIDConfig pid_config_;
   rclcpp::Rate loop_rate_;
 };

moveit_ros/hybrid_planning/local_constraint_solver_plugins/src/decelerate_before_collision.cpp

@@ -47,28 +47,32 @@ const double CYLCE_TIME = 0.01;  // TODO(sjahr) Add param and proper time handli
 DecelerateBeforeCollision::DecelerateBeforeCollision() : loop_rate_(1 / CYLCE_TIME){};
 
 bool DecelerateBeforeCollision::initialize(const rclcpp::Node::SharedPtr& node,
-                                           planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor)
+                                           const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                           const std::string& group_name)
 {
   planning_scene_monitor_ = planning_scene_monitor;
   node_handle_ = node;
   path_invalidation_event_send_ = false;
 
   // Initialize PID
-  auto joint_names = (planning_scene_monitor_->getRobotModel())->getJointModelNames();
-  for (std::size_t i = 0; i < joint_names.size(); i++)
+  const auto jmg = planning_scene_monitor_->getRobotModel()->getJointModelGroup(group_name);
+  for (const auto& joint_name : jmg->getActiveJointModelNames())
   {
-    joint_position_pids_.push_back(control_toolbox::Pid(pid_config_.k_p, pid_config_.k_i, pid_config_.k_d,
-                                                        pid_config_.windup_limit, -pid_config_.windup_limit,
-                                                        true));  // TODO(sjahr) Add ROS2 param
+    joint_position_pids_[joint_name] = control_toolbox::Pid(pid_config_.k_p, pid_config_.k_i, pid_config_.k_d,
+                                                            pid_config_.windup_limit, -pid_config_.windup_limit,
+                                                            true);  // TODO(sjahr) Add ROS2 param
   }
   return true;
 }
 
-trajectory_msgs::msg::JointTrajectory
-DecelerateBeforeCollision::solve(robot_trajectory::RobotTrajectory local_trajectory,
-                                 std::vector<moveit_msgs::msg::Constraints> local_constraints,
-                                 std::shared_ptr<moveit_msgs::action::LocalPlanner::Feedback> feedback)
+moveit_msgs::action::LocalPlanner::Feedback
+DecelerateBeforeCollision::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+                                 const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+                                 trajectory_msgs::msg::JointTrajectory& local_solution)
 {
+  // Feedback result
+  moveit_msgs::action::LocalPlanner::Feedback feedback_result;
+
   // Get current planning scene
   planning_scene_monitor_->updateFrameTransforms();
 
@@ -78,7 +82,7 @@ DecelerateBeforeCollision::solve(robot_trajectory::RobotTrajectory local_traject
   std::vector<std::size_t>* invalid_index = nullptr;
 
   // Get Current State
-  moveit::core::RobotState current_state = locked_planning_scene->getCurrentState();
+  const moveit::core::RobotState& current_state = locked_planning_scene->getCurrentState();
 
   // Check if path is valid
   if (locked_planning_scene->isPathValid(local_trajectory, local_trajectory.getGroupName(), false, invalid_index))
@@ -95,47 +99,50 @@ DecelerateBeforeCollision::solve(robot_trajectory::RobotTrajectory local_traject
   {
     if (!path_invalidation_event_send_)
     {  // Send feedback only once
-      feedback->feedback = "collision_ahead";
+      feedback_result.feedback = "collision_ahead";
       path_invalidation_event_send_ = true;  // Set feedback flag
     }
 
     // Keep current position
     robot_command.addSuffixWayPoint(current_state, 0.0);
   }
 
-  // Transform into joint_trajectory
+  // Transform robot trajectory into joint_trajectory message
   moveit_msgs::msg::RobotTrajectory robot_command_msg;
   robot_command.getRobotTrajectoryMsg(robot_command_msg);
-
-  trajectory_msgs::msg::JointTrajectory joint_trajectory = robot_command_msg.joint_trajectory;
+  const auto& joint_trajectory = robot_command_msg.joint_trajectory;
 
   // Transform current state into msg
   moveit_msgs::msg::RobotState current_state_msg;
   robotStateToRobotStateMsg(current_state, current_state_msg);
 
-  // Calculate PID command
-  std::vector<double> delta_theta;
-  for (std::size_t i = 0; i < joint_trajectory.points[0].positions.size(); i++)
-  {
-    double error = joint_trajectory.points[0].positions[i] - current_state_msg.joint_state.position[i];
-    delta_theta.push_back(joint_position_pids_[i].computeCommand(error, loop_rate_.period().count()));
-  }
-
-  // Apply PID command
+  // Initialize command goal
   trajectory_msgs::msg::JointTrajectoryPoint command_goal_point;
   command_goal_point.time_from_start = rclcpp::Duration(loop_rate_.period().count());
-  for (std::size_t i = 0; i < joint_trajectory.points[0].positions.size(); i++)
+
+  // Calculate PID command
+  const auto& target_state = joint_trajectory.points[0];
+  for (std::size_t i = 0; i < target_state.positions.size(); i++)
   {
-    // Increment joint
-    command_goal_point.positions.push_back(current_state_msg.joint_state.position[i] += delta_theta[i]);
+    // Skip position if joint is not part of active group
+    const auto& joint_name = joint_trajectory.joint_names[i];
+    if (joint_position_pids_.find(joint_name) == joint_position_pids_.end())
+      continue;
+
+    // Calculate PID command
+    const double error = target_state.positions[i] - current_state_msg.joint_state.position[i];
+    const double delta_theta = joint_position_pids_[joint_name].computeCommand(error, loop_rate_.period().count());
+
+    // Apply delta to current state to compute goal command
+    command_goal_point.positions.push_back(current_state_msg.joint_state.position[i] += delta_theta);
   }
 
-  // Add new command point to local solution joint trajectory
-  joint_trajectory.points.clear();
-  joint_trajectory.points.push_back(command_goal_point);
+  // Replace local trajectory with goal command
+  local_solution.joint_names = joint_trajectory.joint_names;
+  local_solution.points = { command_goal_point };
 
-  // Return only joint trajectory
-  return joint_trajectory;
+  // Return feedback_result
+  return feedback_result;
 }
 }  // namespace moveit_hybrid_planning
 

moveit_ros/hybrid_planning/local_planner/include/moveit/local_planner/local_constraint_solver_interface.h

@@ -64,20 +64,20 @@ class LocalConstraintSolverInterface
    * @return True if initialization was successful
    */
   virtual bool initialize(const rclcpp::Node::SharedPtr& node,
-                          planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor) = 0;
+                          const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                          const std::string& group_name) = 0;
 
   /**
    * Solve local planning problem for the current loop run
    * @param local_trajectory The local trajectory to pursue
    * @param local_constraints Local goal constraints
-   * @param planning_scene The planning scene to use for local planning
-   * @param feedback Feedback event string from the current solver i.e. "Collision detected"
-   * @return Local planning solution in joint space
+   * @param local_solution solution plan in joint space
+   * @return Feedback event from the current solver call i.e. "Collision detected"
    */
-  virtual trajectory_msgs::msg::JointTrajectory
-  solve(robot_trajectory::RobotTrajectory local_trajectory,
-        std::vector<moveit_msgs::msg::Constraints> local_constraints,
-        std::shared_ptr<moveit_msgs::action::LocalPlanner::Feedback> feedback) = 0;
+  virtual moveit_msgs::action::LocalPlanner::Feedback
+  solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+        const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+        trajectory_msgs::msg::JointTrajectory& local_solution) = 0;
   virtual ~LocalConstraintSolverInterface(){};
 
 protected: