Create motion plan request with a cost function

moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h

@@ -75,9 +75,6 @@ struct PlannerConfigurationSettings
 /** \brief Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings */
 typedef std::map<std::string, PlannerConfigurationSettings> PlannerConfigurationMap;
 
-/** \brief Definition for a cost function to measure the cost of a single state during motion planning */
-using StateCostFn = std::function<double(const Eigen::VectorXd& state_vector)>;
-
 MOVEIT_CLASS_FORWARD(PlanningContext);  // Defines PlanningContextPtr, ConstPtr, WeakPtr... etc
 
 /** \brief Representation of a particular planning context -- the planning scene and the request are known,

moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h

@@ -36,12 +36,23 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <moveit_msgs/msg/motion_plan_request.hpp>
 
 namespace planning_interface
 {
-// for now this is just a typedef
 
-typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest;
+/** \brief Definition for a cost function to measure the cost of a single state during motion planning */
+using StateCostFn = std::function<double(const Eigen::VectorXd& state_vector)>;
+struct MotionPlanRequest
+{
+  MotionPlanRequest() = default;
+  MotionPlanRequest(const moveit_msgs::msg::MotionPlanRequest& request_msg) : data(request_msg)
+  {
+  }
+
+  moveit_msgs::msg::MotionPlanRequest data;
+  planning_interface::StateCostFn state_cost_function = nullptr;
+};
 
 }  // namespace planning_interface

moveit_core/planning_interface/src/planning_interface.cpp

@@ -92,18 +92,18 @@ const StateCostFn& PlanningContext::getStateCostFunction() const
 void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request)
 {
   request_ = request;
-  if (request_.allowed_planning_time <= 0.0)
+  if (request_.data.allowed_planning_time <= 0.0)
   {
     RCLCPP_INFO(LOGGER, "The timeout for planning must be positive (%lf specified). Assuming one second instead.",
-                request_.allowed_planning_time);
-    request_.allowed_planning_time = 1.0;
+                request_.data.allowed_planning_time);
+    request_.data.allowed_planning_time = 1.0;
   }
-  if (request_.num_planning_attempts < 0)
+  if (request_.data.num_planning_attempts < 0)
   {
     RCLCPP_ERROR(LOGGER, "The number of desired planning attempts should be positive. "
                          "Assuming one attempt.");
   }
-  request_.num_planning_attempts = std::max(1, request_.num_planning_attempts);
+  request_.data.num_planning_attempts = std::max(1, request_.data.num_planning_attempts);
 }
 
 bool PlannerManager::initialize(const moveit::core::RobotModelConstPtr& /*unused*/,

moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp

@@ -69,7 +69,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager
   {
     error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
 
-    if (req.group_name.empty())
+    if (req.data.group_name.empty())
     {
       RCLCPP_ERROR(LOGGER, "No group specified to plan for");
       error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME;
@@ -88,7 +88,7 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager
     ps->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorHybrid::create());
 
     // retrieve and configure existing context
-    const CHOMPPlanningContextPtr& context = planning_contexts_.at(req.group_name);
+    const CHOMPPlanningContextPtr& context = planning_contexts_.at(req.data.group_name);
     context->setPlanningScene(ps);
     context->setMotionPlanRequest(req);
     error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;

moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp

@@ -62,7 +62,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
 
   // get the specified start state
   moveit::core::RobotState start_state = planning_scene->getCurrentState();
-  moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
+  moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.data.start_state, start_state);
 
   if (!start_state.satisfiesBounds())
   {
@@ -71,18 +71,19 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
     return false;
   }
 
-  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);
-  robotStateToArray(start_state, req.group_name, trajectory.getTrajectoryPoint(0));
+  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.data.group_name);
+  robotStateToArray(start_state, req.data.group_name, trajectory.getTrajectoryPoint(0));
 
-  if (req.goal_constraints.size() != 1)
+  if (req.data.goal_constraints.size() != 1)
   {
-    RCLCPP_ERROR(LOGGER, "Expecting exactly one goal constraint, got: %zd", req.goal_constraints.size());
+    RCLCPP_ERROR(LOGGER, "Expecting exactly one goal constraint, got: %zd", req.data.goal_constraints.size());
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
     return false;
   }
 
-  if (req.goal_constraints[0].joint_constraints.empty() || !req.goal_constraints[0].position_constraints.empty() ||
-      !req.goal_constraints[0].orientation_constraints.empty())
+  if (req.data.goal_constraints[0].joint_constraints.empty() ||
+      !req.data.goal_constraints[0].position_constraints.empty() ||
+      !req.data.goal_constraints[0].orientation_constraints.empty())
   {
     RCLCPP_ERROR(LOGGER, "Only joint-space goals are supported");
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
@@ -91,18 +92,18 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
 
   const size_t goal_index = trajectory.getNumPoints() - 1;
   moveit::core::RobotState goal_state(start_state);
-  for (const moveit_msgs::msg::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
+  for (const moveit_msgs::msg::JointConstraint& joint_constraint : req.data.goal_constraints[0].joint_constraints)
     goal_state.setVariablePosition(joint_constraint.joint_name, joint_constraint.position);
   if (!goal_state.satisfiesBounds())
   {
     RCLCPP_ERROR(LOGGER, "Goal state violates joint limits");
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE;
     return false;
   }
-  robotStateToArray(goal_state, req.group_name, trajectory.getTrajectoryPoint(goal_index));
+  robotStateToArray(goal_state, req.data.group_name, trajectory.getTrajectoryPoint(goal_index));
 
   const moveit::core::JointModelGroup* model_group =
-      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
+      planning_scene->getRobotModel()->getJointModelGroup(req.data.group_name);
   // fix the goal to move the shortest angular distance for wrap-around joints:
   for (size_t i = 0; i < model_group->getActiveJointModels().size(); ++i)
   {
@@ -190,8 +191,8 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
 
     // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
     // case of a recovery behaviour
-    optimizer =
-        std::make_unique<ChompOptimizer>(&trajectory, planning_scene, req.group_name, &params_nonconst, start_state);
+    optimizer = std::make_unique<ChompOptimizer>(&trajectory, planning_scene, req.data.group_name, &params_nonconst,
+                                                 start_state);
     if (!optimizer->isInitialized())
     {
       RCLCPP_ERROR(LOGGER, "Could not initialize optimizer");
@@ -239,7 +240,8 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
 
   RCLCPP_DEBUG(LOGGER, "Output trajectory has %zd joints", trajectory.getNumJoints());
 
-  auto result = std::make_shared<robot_trajectory::RobotTrajectory>(planning_scene->getRobotModel(), req.group_name);
+  auto result =
+      std::make_shared<robot_trajectory::RobotTrajectory>(planning_scene->getRobotModel(), req.data.group_name);
   // fill in the entire trajectory
   for (size_t i = 0; i < trajectory.getNumPoints(); ++i)
   {
@@ -278,7 +280,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
   // check that final state is within goal tolerances
   kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
   const moveit::core::RobotState& last_state = result->getLastWayPoint();
-  for (const moveit_msgs::msg::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
+  for (const moveit_msgs::msg::JointConstraint& constraint : req.data.goal_constraints[0].joint_constraints)
   {
     if (!jc.configure(constraint) || !jc.decide(last_state).satisfied)
     {

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/planning_interface_objective.hpp

@@ -49,7 +49,7 @@ class PlanningInterfaceObjective : public ompl::base::OptimizationObjective
 {
 public:
   PlanningInterfaceObjective(const ompl::base::SpaceInformationPtr& si,
-                             const planning_interface::StateCostFn& state_cost_function);
+                             const ::planning_interface::StateCostFn& state_cost_function);
 
   /** \brief Returns for a state calculated with the state_cost_function */
   ompl::base::Cost stateCost(const ompl::base::State* state) const override;
@@ -79,7 +79,7 @@ class PlanningInterfaceObjective : public ompl::base::OptimizationObjective
   ompl::base::Cost motionCostBestEstimate(const ompl::base::State* state_1, const ompl::base::State* state_2) const;
 
 protected:
-  const planning_interface::StateCostFn state_cost_function_;
+  const ::planning_interface::StateCostFn state_cost_function_;
 
   /** \brief Adapted from ompl::base::StateCostIntegralObjective: Helper method which uses the trapezoidal rule
       to approximate the integral of the cost between two

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h

@@ -84,7 +84,7 @@ struct ModelBasedPlanningContextSpecification
   ob::ConstrainedStateSpacePtr constrained_state_space_;
 };
 
-class ModelBasedPlanningContext : public planning_interface::PlanningContext
+class ModelBasedPlanningContext : public ::planning_interface::PlanningContext
 {
 public:
   ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec);

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h

@@ -64,26 +64,26 @@ class OMPLInterface
       planner configurations are used as specified in \e pconfig instead of reading them from ROS parameters
      */
   OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model,
-                const planning_interface::PlannerConfigurationMap& pconfig, const rclcpp::Node::SharedPtr& node,
+                const ::planning_interface::PlannerConfigurationMap& pconfig, const rclcpp::Node::SharedPtr& node,
                 const std::string& parameter_namespace);
 
   virtual ~OMPLInterface();
 
   /** @brief Specify configurations for the planners.
       @param pconfig Configurations for the different planners */
-  void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig);
+  void setPlannerConfigurations(const ::planning_interface::PlannerConfigurationMap& pconfig);
 
   /** @brief Get the configurations for the planners that are already loaded
       @param pconfig Configurations for the different planners */
-  const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const
+  const ::planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const
   {
     return context_manager_.getPlannerConfigurations();
   }
 
   ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                                  const planning_interface::MotionPlanRequest& req) const;
+                                                  const ::planning_interface::MotionPlanRequest& req) const;
   ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                                  const planning_interface::MotionPlanRequest& req,
+                                                  const ::planning_interface::MotionPlanRequest& req,
                                                   moveit_msgs::msg::MoveItErrorCodes& error_code) const;
 
   const PlanningContextManager& getPlanningContextManager() const
@@ -123,7 +123,7 @@ class OMPLInterface
   /** @brief Load planner configurations for specified group into planner_config */
   bool loadPlannerConfiguration(const std::string& group_name, const std::string& planner_id,
                                 const std::map<std::string, std::string>& group_params,
-                                planning_interface::PlannerConfigurationSettings& planner_config);
+                                ::planning_interface::PlannerConfigurationSettings& planner_config);
 
   /** @brief Configure the planners*/
   void loadPlannerConfigurations();
@@ -132,7 +132,7 @@ class OMPLInterface
   void loadConstraintSamplers();
 
   /** \brief Configure the OMPL planning context for a new planning request */
-  ModelBasedPlanningContextPtr prepareForSolve(const planning_interface::MotionPlanRequest& req,
+  ModelBasedPlanningContextPtr prepareForSolve(const ::planning_interface::MotionPlanRequest& req,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                moveit_msgs::msg::MoveItErrorCodes* error_code, unsigned int* attempts,
                                                double* timeout) const;

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h

@@ -58,7 +58,7 @@ class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory
    * For more details on this state space selection process, see:
    * https://github.com/JeroenDM/moveit/pull/2
    * **/
-  int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req,
+  int canRepresentProblem(const std::string& group, const ::planning_interface::MotionPlanRequest& req,
                           const moveit::core::RobotModelConstPtr& robot_model) const override;
 
 protected:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h

@@ -45,7 +45,7 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory
 public:
   JointModelStateSpaceFactory();
 
-  int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req,
+  int canRepresentProblem(const std::string& group, const ::planning_interface::MotionPlanRequest& req,
                           const moveit::core::RobotModelConstPtr& robot_model) const override;
 
 protected:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <ompl/base/StateSpace.h>
+#include <moveit/planning_interface/planning_request.h>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/kinematic_constraints/kinematic_constraint.h>

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h

@@ -66,7 +66,7 @@ class ModelBasedStateSpaceFactory
      the user
       request \e req for group \e group. The group \e group must always be specified and takes precedence over \e
      req.group_name, which may be different */
-  virtual int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req,
+  virtual int canRepresentProblem(const std::string& group, const ::planning_interface::MotionPlanRequest& req,
                                   const moveit::core::RobotModelConstPtr& robot_model) const = 0;
 
 protected:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h

@@ -45,7 +45,7 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory
 public:
   PoseModelStateSpaceFactory();
 
-  int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req,
+  int canRepresentProblem(const std::string& group, const ::planning_interface::MotionPlanRequest& req,
                           const moveit::core::RobotModelConstPtr& robot_model) const override;
 
 protected:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h

@@ -85,10 +85,10 @@ class PlanningContextManager
 
   /** @brief Specify configurations for the planners.
       @param pconfig Configurations for the different planners */
-  void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig);
+  void setPlannerConfigurations(const ::planning_interface::PlannerConfigurationMap& pconfig);
 
   /** @brief Return the previously set planner configurations */
-  const planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const
+  const ::planning_interface::PlannerConfigurationMap& getPlannerConfigurations() const
   {
     return planner_configs_;
   }
@@ -179,7 +179,7 @@ class PlanningContextManager
    *
    * */
   ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                                  const planning_interface::MotionPlanRequest& req,
+                                                  const ::planning_interface::MotionPlanRequest& req,
                                                   moveit_msgs::msg::MoveItErrorCodes& error_code,
                                                   const rclcpp::Node::SharedPtr& node,
                                                   bool use_constraints_approximations) const;
@@ -216,13 +216,13 @@ class PlanningContextManager
   void registerPlannerAllocatorHelper(const std::string& planner_id);
 
   /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */
-  ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config,
+  ModelBasedPlanningContextPtr getPlanningContext(const ::planning_interface::PlannerConfigurationSettings& config,
                                                   const ModelBasedStateSpaceFactoryPtr& factory,
-                                                  const moveit_msgs::msg::MotionPlanRequest& req) const;
+                                                  const ::planning_interface::MotionPlanRequest& req) const;
 
   const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& factory_type) const;
   const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& group_name,
-                                                             const moveit_msgs::msg::MotionPlanRequest& req) const;
+                                                             const ::planning_interface::MotionPlanRequest& req) const;
 
   /** \brief The kinematic model for which motion plans are computed */
   moveit::core::RobotModelConstPtr robot_model_;
@@ -237,7 +237,7 @@ class PlanningContextManager
       be of the form "group_name[config_name]" if there are
       particular configurations specified for a group, or of the
       form "group_name" if default settings are to be used. */
-  planning_interface::PlannerConfigurationMap planner_configs_;
+  ::planning_interface::PlannerConfigurationMap planner_configs_;
 
   /// maximum number of states to sample in the goal region for any planning request (when such sampling is possible)
   unsigned int max_goal_samples_;

moveit_planners/ompl/ompl_interface/scripts/generate_state_database.cpp

@@ -142,11 +142,11 @@ void computeDB(const rclcpp::Node::SharedPtr& node, const planning_scene::Planni
   scene->getCurrentStateNonConst().update();
 
   ompl_interface::OMPLInterface ompl_interface(scene->getRobotModel(), node, "ompl");
-  planning_interface::MotionPlanRequest req;
-  req.group_name = params.planning_group;
-  req.path_constraints = params.constraints;
-  moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), req.start_state);
-  req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(
+  ::planning_interface::MotionPlanRequest req;
+  req.data.group_name = params.planning_group;
+  req.data.path_constraints = params.constraints;
+  moveit::core::robotStateToRobotStateMsg(scene->getCurrentState(), req.data.start_state);
+  req.data.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(
       scene->getCurrentState(), scene->getRobotModel()->getJointModelGroup(params.planning_group)));
 
   ompl_interface::ModelBasedPlanningContextPtr context = ompl_interface.getPlanningContext(scene, req);

moveit_planners/ompl/ompl_interface/src/detail/planning_interface_objective.cpp

@@ -40,7 +40,7 @@
 namespace ompl_interface
 {
 PlanningInterfaceObjective::PlanningInterfaceObjective(const ompl::base::SpaceInformationPtr& si,
-                                                       const planning_interface::StateCostFn& state_cost_function)
+                                                       const ::planning_interface::StateCostFn& state_cost_function)
   : OptimizationObjective(si), state_cost_function_(state_cost_function)
 {
   description_ = "Planning Interface Objective";