Compile Pilz planner package

moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf

@@ -694,7 +694,7 @@ PlanningContextLoaderCIRC
 --
 +getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
 const planning_interface::MotionPlanRequest& req,
-moveit_msgs::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr
+moveit_msgs::msg::MoveItErrorCodes& error_code):planning_interface::PlanningContextPtr
 --
 </panel_attributes>
     <additional_attributes/>

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_limits_aggregator.h

@@ -34,6 +34,7 @@
 
 #pragma once
 
+#include <rclcpp/rclcpp.hpp>
 #include "pilz_industrial_motion_planner/cartesian_limit.h"
 
 namespace pilz_industrial_motion_planner
@@ -56,10 +57,11 @@ class CartesianLimitsAggregator
    * - "max_rot_vel", the maximum rotational velocity [rad/s]
    * - "max_rot_acc", the maximum rotational acceleration [rad/s^2]
    * - "max_rot_dec", the maximum rotational deceleration (<= 0)[rad/s^2]
-   * @param nh node handle to access the parameters
+   * @param node node to access the parameters
+   * @param param_namespace the parameter name to access the parameters
    * @return the obtained cartesian limits
    */
-  static CartesianLimit getAggregatedLimits(const ros::NodeHandle& nh);
+  static CartesianLimit getAggregatedLimits(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace);
 };
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h

@@ -34,20 +34,20 @@
 
 #pragma once
 
-#include <ros/duration.h>
+#include <rclcpp/duration.hpp>
 
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist.hpp>
 
 namespace pilz_industrial_motion_planner
 {
 struct CartesianTrajectoryPoint
 {
-  geometry_msgs::Pose pose;
-  geometry_msgs::Twist velocity;
-  geometry_msgs::Twist acceleartion;
-  ros::Duration time_from_start;
+  geometry_msgs::msg::Pose pose;
+  geometry_msgs::msg::Twist velocity;
+  geometry_msgs::msg::Twist acceleartion;
+  rclcpp::Duration time_from_start{ 0, 0 };
 };
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h

@@ -40,9 +40,9 @@
 
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
-#include <moveit_msgs/MotionPlanResponse.h>
+#include <moveit_msgs/msg/motion_plan_response.hpp>
+#include <moveit_msgs/msg/motion_sequence_request.hpp>
 
-#include "moveit_msgs/MotionSequenceRequest.h"
 #include "pilz_industrial_motion_planner/plan_components_builder.h"
 #include "pilz_industrial_motion_planner/trajectory_blender.h"
 #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h"
@@ -52,11 +52,14 @@ namespace pilz_industrial_motion_planner
 using RobotTrajCont = std::vector<robot_trajectory::RobotTrajectoryPtr>;
 
 // List of exceptions which can be thrown by the CommandListManager class.
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveItErrorCodes::FAILURE);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException,
+                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException,
+                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException,
+                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::msg::MoveItErrorCodes::FAILURE);
 
 /**
  * @brief This class orchestrates the planning of single commands and
@@ -65,7 +68,7 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveI
 class CommandListManager
 {
 public:
-  CommandListManager(const ros::NodeHandle& nh, const robot_model::RobotModelConstPtr& model);
+  CommandListManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model);
 
   /**
    * @brief Generates trajectories for the specified list of motion commands.
@@ -100,11 +103,11 @@ class CommandListManager
    */
   RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                       const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
-                      const moveit_msgs::MotionSequenceRequest& req_list);
+                      const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
 private:
   using MotionResponseCont = std::vector<planning_interface::MotionPlanResponse>;
-  using RobotState_OptRef = boost::optional<const robot_state::RobotState&>;
+  using RobotState_OptRef = boost::optional<const moveit::core::RobotState&>;
   using RadiiCont = std::vector<double>;
   using GroupNamesCont = std::vector<std::string>;
 
@@ -129,7 +132,7 @@ class CommandListManager
    */
   MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                         const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
-                                        const moveit_msgs::MotionSequenceRequest& req_list) const;
+                                        const moveit_msgs::msg::MotionSequenceRequest& req_list) const;
 
   /**
    * @return TRUE if the blending radii of specified trajectories overlap,
@@ -152,7 +155,7 @@ class CommandListManager
    * from group.
    */
   static void setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name,
-                            moveit_msgs::RobotState& start_state);
+                            moveit_msgs::msg::RobotState& start_state);
 
   /**
    * @return Container of radii extracted from the specified request list.
@@ -163,48 +166,50 @@ class CommandListManager
    * - blend raddi between different groups.
    */
   static RadiiCont extractBlendRadii(const moveit::core::RobotModel& model,
-                                     const moveit_msgs::MotionSequenceRequest& req_list);
+                                     const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
   /**
    * @return True in case of an invalid blend radii between specified
    * command A and B, otherwise False. Invalid blend radii are:
    * - blend radii between end-effectors and
    * - blend raddi between different groups.
    */
-  static bool isInvalidBlendRadii(const moveit::core::RobotModel& model, const moveit_msgs::MotionSequenceItem& item_A,
-                                  const moveit_msgs::MotionSequenceItem& item_B);
+  static bool isInvalidBlendRadii(const moveit::core::RobotModel& model,
+                                  const moveit_msgs::msg::MotionSequenceItem& item_A,
+                                  const moveit_msgs::msg::MotionSequenceItem& item_B);
 
   /**
    * @brief Checks that all blend radii are greater or equal to zero.
    */
-  static void checkForNegativeRadii(const moveit_msgs::MotionSequenceRequest& req_list);
+  static void checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
   /**
    * @brief Checks that last blend radius is zero.
    */
-  static void checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list);
+  static void checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
   /**
    * @brief Checks that only the first request of the specified group has
    * a start state in the specified request list.
    */
-  static void checkStartStatesOfGroup(const moveit_msgs::MotionSequenceRequest& req_list, const std::string& group_name);
+  static void checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list,
+                                      const std::string& group_name);
 
   /**
    * @brief Checks that each group in the specified request list has only
    * one start state.
    */
-  static void checkStartStates(const moveit_msgs::MotionSequenceRequest& req_list);
+  static void checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
   /**
    * @return Returns all group names which are present in the specified
    * request.
    */
-  static GroupNamesCont getGroupNames(const moveit_msgs::MotionSequenceRequest& req_list);
+  static GroupNamesCont getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list);
 
 private:
-  //! Node handle
-  ros::NodeHandle nh_;
+  //! Node
+  const rclcpp::Node::SharedPtr node_;
 
   //! Robot model
   moveit::core::RobotModelConstPtr model_;
@@ -214,7 +219,7 @@ class CommandListManager
   PlanComponentsBuilder plan_comp_builder_;
 };
 
-inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::MotionSequenceRequest& req_list)
+inline void CommandListManager::checkLastBlendRadiusZero(const moveit_msgs::msg::MotionSequenceRequest& req_list)
 {
   if (req_list.items.back().blend_radius != 0.0)
   {

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_aggregator.h

@@ -37,7 +37,7 @@
 #include "pilz_industrial_motion_planner/joint_limits_container.h"
 #include "pilz_industrial_motion_planner/joint_limits_extension.h"
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_interface/planning_response.h>
@@ -72,12 +72,13 @@ class JointLimitsAggregator
    * @note The acceleration/deceleration can only be set via the parameter
    * server since they are not supported
    * in the urdf so far.
-   * @param nh Node handle in whose namespace the joint limit parameters are
-   * expected.
+   * @param node Node to use for accessing joint limit parameters
+   * @param param_namespace Namespace to use for looking up node parameters
    * @param joint_models The joint models
    * @return Container containing the limits
    */
-  static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle& nh,
+  static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr& node,
+                                                  const std::string& param_namespace,
                                                   const std::vector<const moveit::core::JointModel*>& joint_models);
 
 protected:

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_extension.h

@@ -35,7 +35,62 @@
 #ifndef JOINT_LIMITS_EXTENSION_H
 #define JOINT_LIMITS_EXTENSION_H
 
-#include <joint_limits_interface/joint_limits.h>
+// TODO(henning): Re-include when this is available, until then the headers content is copied below
+// #include <joint_limits_interface/joint_limits.h>
+/////////////////////////////////////////////////////
+// start of <joint_limits_interface/joint_limits.hpp>
+// //////////////////////////////////////////////////
+namespace joint_limits_interface
+{
+struct JointLimits
+{
+  JointLimits()
+    : min_position(0.0)
+    , max_position(0.0)
+    , max_velocity(0.0)
+    , max_acceleration(0.0)
+    , max_jerk(0.0)
+    , max_effort(0.0)
+    , has_position_limits(false)
+    , has_velocity_limits(false)
+    , has_acceleration_limits(false)
+    , has_jerk_limits(false)
+    , has_effort_limits(false)
+    , angle_wraparound(false)
+  {
+  }
+
+  double min_position;
+  double max_position;
+  double max_velocity;
+  double max_acceleration;
+  double max_jerk;
+  double max_effort;
+
+  bool has_position_limits;
+  bool has_velocity_limits;
+  bool has_acceleration_limits;
+  bool has_jerk_limits;
+  bool has_effort_limits;
+  bool angle_wraparound;
+};
+
+struct SoftJointLimits
+{
+  SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0)
+  {
+  }
+
+  double min_position;
+  double max_position;
+  double k_position;
+  double k_velocity;
+};
+}  // namespace joint_limits_interface
+///////////////////////////////////////////////////
+// end of <joint_limits_interface/joint_limits.hpp>
+// ////////////////////////////////////////////////
+
 #include <map>
 #include <string>