Merge branch 'main' into pr-cost_function_interface

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -0,0 +1,18 @@
+planning_plugin: stomp_moveit/StompPlanner
+request_adapters: >-
+  default_planner_request_adapters/AddTimeOptimalParameterization
+  default_planner_request_adapters/ResolveConstraintFrames
+  default_planner_request_adapters/FixWorkspaceBounds
+  default_planner_request_adapters/FixStartStateBounds
+  default_planner_request_adapters/FixStartStateCollision
+  default_planner_request_adapters/FixStartStatePathConstraints
+
+stomp_moveit:
+  num_timesteps: 60
+  num_iterations: 40
+  num_iterations_after_valid: 0
+  num_rollouts: 30
+  max_rollouts: 30
+  exponentiated_cost_sensitivity: 0.8
+  control_cost_weight: 0.1
+  delta_t: 0.1

moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp

@@ -53,6 +53,7 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
                          planning_interface::MotionPlanDetailedResponse& res) const
 {
   auto start_time = std::chrono::system_clock::now();
+  res.planner_id = std::string("chomp");
   if (!planning_scene)
   {
     RCLCPP_ERROR(LOGGER, "No planning scene initialized.");

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

@@ -75,7 +75,7 @@ struct ModelBasedPlanningContextSpecification
    * When the parameter "use_ompl_constrained_planning" is set to true in ompl_planning.yaml,
    * the path constraints are handled by this state space.
    *
-   * **Important**: because code often depents on the attribute `state_space_` to copy states from MoveIt to OMPL, we
+   * **Important**: because code often depends on the attribute `state_space_` to copy states from MoveIt to OMPL, we
    * must set `state_space_` to have type `ompl_interface::ConstrainedPlanningStateSpace`. The actual planning does
    * not happen with this `state_space_`, but it is used to create the `constrained_state_space_` of type
    * `ompl::base::ConstrainedStateSpace`. The latter is the one passed to OMPL simple setup (after creating a

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp

@@ -307,6 +307,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
   RCLCPP_INFO_STREAM(LOGGER, "Generating " << req.planner_id << " trajectory...");
   rclcpp::Time planning_begin = clock_->now();
 
+  res.planner_id = req.planner_id;
   try
   {
     validateRequest(req);

moveit_planners/stomp/src/stomp_moveit_planning_context.cpp

@@ -214,6 +214,7 @@ bool StompPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   // Start time
   auto time_start = std::chrono::steady_clock::now();
 
+  res.planner_id = std::string("stomp");
   // Default to happy path
   res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
 

moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp

@@ -58,40 +58,44 @@ void init_moveit_py(py::module& m)
                        const py::object& config_dict, bool provide_planning_service) {
              static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer");
 
-             if (!rclcpp::ok())
-             {
-               RCLCPP_INFO(LOGGER, "Initialize rclcpp");
-               rclcpp::init(0, nullptr);
-             }
-
-             RCLCPP_INFO(LOGGER, "Initialize node parameters");
-             rclcpp::NodeOptions node_options;
-
              // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance
              // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters
              // and finally no supplied parameters.
+             std::vector<std::string> launch_arguments;
              if (!config_dict.is(py::none()))
              {
                auto utils = py::module::import("moveit.utils");
                // TODO (peterdavidfagan): replace python method with C++ method
                std::string params_filepath =
-                   utils.attr("create_params_file_from_dict")(config_dict, "moveit_py").cast<std::string>();
-               RCLCPP_INFO(LOGGER, "params_filepath: %s", params_filepath.c_str());
-               node_options.allow_undeclared_parameters(true)
-                   .automatically_declare_parameters_from_overrides(true)
-                   .arguments({ "--ros-args", "--params-file", params_filepath });
+                   utils.attr("create_params_file_from_dict")(config_dict, node_name).cast<std::string>();
+               launch_arguments = { "--ros-args", "--params-file", params_filepath };
              }
              else if (!launch_params_filepath.empty())
              {
-               node_options.allow_undeclared_parameters(true)
-                   .automatically_declare_parameters_from_overrides(true)
-                   .arguments({ "--ros-args", "--params-file", launch_params_filepath });
+               launch_arguments = { "--ros-args", "--params-file", launch_params_filepath };
              }
-             else
+
+             // Initialize ROS, pass launch arguments with rclcpp::init()
+             if (!rclcpp::ok())
              {
-               // TODO (peterdavidfagan): consider failing initialization if no params file is provided
-               node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
+               RCLCPP_INFO(LOGGER, "Initialize rclcpp");
+               std::vector<const char*> chars;
+               chars.reserve(launch_arguments.size());
+               for (const auto& arg : launch_arguments)
+               {
+                 chars.push_back(arg.c_str());
+               }
+
+               rclcpp::init(launch_arguments.size(), chars.data());
              }
+
+             // Build NodeOptions
+             RCLCPP_INFO(LOGGER, "Initialize node parameters");
+             rclcpp::NodeOptions node_options;
+             node_options.allow_undeclared_parameters(true)
+                 .automatically_declare_parameters_from_overrides(true)
+                 .arguments(launch_arguments);
+
              RCLCPP_INFO(LOGGER, "Initialize node and executor");
              rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options);
              std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor =

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

@@ -53,7 +53,13 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM
                                                       const std::string& parameter_namespace,
                                                       const std::string& planner_plugin_param_name,
                                                       const std::string& adapter_plugins_param_name)
-  : active_{ false }, node_(node), parameter_namespace_(parameter_namespace), robot_model_(model)
+  : active_{ false }
+  , node_(node)
+  , parameter_namespace_(parameter_namespace)
+  , display_computed_motion_plans_{ false }
+  , publish_received_requests_{ false }
+  , robot_model_(model)
+  , check_solution_paths_{ false }
 {
   std::string planner_plugin_fullname = parameter_namespace_ + "." + planner_plugin_param_name;
   if (parameter_namespace_.empty())
@@ -88,19 +94,18 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM
   : active_{ false }
   , node_(node)
   , parameter_namespace_(parameter_namespace)
+  , display_computed_motion_plans_{ false }
+  , publish_received_requests_{ false }
   , planner_plugin_name_(planner_plugin_name)
   , adapter_plugin_names_(adapter_plugin_names)
   , robot_model_(model)
+  , check_solution_paths_{ false }
 {
   configure();
 }
 
 void planning_pipeline::PlanningPipeline::configure()
 {
-  check_solution_paths_ = false;  // this is set to true below
-  publish_received_requests_ = false;
-  display_computed_motion_plans_ = false;  // this is set to true below
-
   // load the planning plugin
   try
   {
@@ -110,42 +115,37 @@ void planning_pipeline::PlanningPipeline::configure()
   catch (pluginlib::PluginlibException& ex)
   {
     RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
+    throw;
   }
 
   std::vector<std::string> classes;
   if (planner_plugin_loader_)
     classes = planner_plugin_loader_->getDeclaredClasses();
-  if (planner_plugin_name_.empty() && classes.size() == 1)
-  {
-    planner_plugin_name_ = classes[0];
-    RCLCPP_INFO(
-        LOGGER,
-        "No '~planning_plugin' parameter specified, but only '%s' planning plugin is available. Using that one.",
-        planner_plugin_name_.c_str());
-  }
-  if (planner_plugin_name_.empty() && classes.size() > 1)
+
+  if (planner_plugin_name_.empty())
   {
-    planner_plugin_name_ = classes[0];
-    RCLCPP_INFO(
-        LOGGER,
-        "Multiple planning plugins available. You should specify the '~planning_plugin' parameter. Using '%s' for "
-        "now.",
-        planner_plugin_name_.c_str());
+    std::string classes_str = boost::algorithm::join(classes, ", ");
+    throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " +
+                             classes_str);
   }
+
   try
   {
     planner_instance_ = planner_plugin_loader_->createUniqueInstance(planner_plugin_name_);
     if (!planner_instance_->initialize(robot_model_, node_, parameter_namespace_))
+    {
       throw std::runtime_error("Unable to initialize planning plugin");
+    }
     RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance_->getDescription().c_str());
   }
   catch (pluginlib::PluginlibException& ex)
   {
     std::string classes_str = boost::algorithm::join(classes, ", ");
-    RCLCPP_ERROR(LOGGER,
+    RCLCPP_FATAL(LOGGER,
                  "Exception while loading planner '%s': %s"
                  "Available plugins: %s",
                  planner_plugin_name_.c_str(), ex.what(), classes_str.c_str());
+    throw;
   }
 
   // load the planner request adapters
@@ -160,7 +160,8 @@ void planning_pipeline::PlanningPipeline::configure()
     }
     catch (pluginlib::PluginlibException& ex)
     {
-      RCLCPP_ERROR(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
+      RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
+      throw;
     }
 
     if (adapter_plugin_loader_)
@@ -174,8 +175,9 @@ void planning_pipeline::PlanningPipeline::configure()
         }
         catch (pluginlib::PluginlibException& ex)
         {
-          RCLCPP_ERROR(LOGGER, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(),
+          RCLCPP_FATAL(LOGGER, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(),
                        ex.what());
+          throw;
         }
         if (ad)
         {
@@ -194,6 +196,10 @@ void planning_pipeline::PlanningPipeline::configure()
       }
     }
   }
+  else
+  {
+    RCLCPP_WARN(LOGGER, "No planning request adapter names specified.");
+  }
   displayComputedMotionPlans(true);
   checkSolutionPaths(true);
 }
@@ -427,6 +433,9 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
   // Make sure that planner id is set
   if (res.planner_id.empty())
   {
+    RCLCPP_WARN(LOGGER, "The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting "
+                        "it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn "
+                        "you if it does not use the requested planner.");
     res.planner_id = req.planner_id;
   }
 

moveit_ros/planning_interface/move_group_interface/CMakeLists.txt

@@ -26,6 +26,3 @@ endif()
 
 install(DIRECTORY include/ DESTINATION include/moveit_ros_planning_interface)
 install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_move_group_interface_export.h DESTINATION include/moveit_ros_planning_interface)
-
-#add_executable(demo src/demo.cpp)
-#target_link_libraries(demo moveit_move_group_interface ${LIBS} ${Boost_LIBRARIES})

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -45,10 +45,6 @@
 #include <moveit_msgs/msg/planner_interface_description.hpp>
 #include <moveit_msgs/msg/constraints.hpp>
 #include <moveit_msgs/msg/grasp.hpp>
-// #include <moveit_msgs/msg/place_location.hpp>
-
-// #include <moveit_msgs/action/pickup.hpp>
-// #include <moveit_msgs/action/place.hpp>
 #include <moveit_msgs/action/move_group.hpp>
 #include <moveit_msgs/action/execute_trajectory.hpp>
 
@@ -784,99 +780,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       in \e request */
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);
 
-  /** \brief Build a PickupGoal for an object named \e object and store it in \e goal_out */
-
-  //  moveit_msgs::action::Pickup::Goal constructPickupGoal(const std::string& object,
-  //                                                      std::vector<moveit_msgs::msg::Grasp> grasps,
-  //                                                      bool plan_only) const;
-  //
-  //  /** \brief Build a PlaceGoal for an object named \e object and store it in \e goal_out */
-  //  moveit_msgs::action::Place::Goal constructPlaceGoal(const std::string& object,
-  //                                                    std::vector<moveit_msgs::msg::PlaceLocation> locations,
-  //                                                    bool plan_only) const;
-  //
-  //  /** \brief Convert a vector of PoseStamped to a vector of PlaceLocation */
-  //  std::vector<moveit_msgs::msg::PlaceLocation>
-  //  posesToPlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& poses) const;
-
   /**@}*/
 
   /**
    * \name High level actions that trigger a sequence of plans and actions.
    */
   /**@{*/
 
-  /** \brief Pick up an object
-
-      This applies a number of hard-coded default grasps */
-  //  moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false)
-  //  {
-  //    return pick(constructPickupGoal(object, std::vector<moveit_msgs::msg::Grasp>(), plan_only));
-  //  }
-  //
-  /** \brief Pick up an object given a grasp pose */
-  //  moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false)
-  //  {
-  //    return pick(constructPickupGoal(object, { grasp }, plan_only));
-  //  }
-  //
-  /** \brief Pick up an object given possible grasp poses */
-  //  moveit::core::MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::msg::Grasp> grasps, bool
-  //  plan_only = false)
-  //  {
-  //    return pick(constructPickupGoal(object, std::move(grasps), plan_only));
-  //  }
-
-  /** \brief Pick up an object given a PickupGoal
-
-      Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other
-      desired variable in the goal, and finally pass it on to this function */
-
-  // moveit::core::MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal);
-
-  /** \brief Pick up an object
-
-      calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */
-  // moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
-
-  /** \brief Pick up an object
-      calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */
-  // moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false);
-
-  /** \brief Place an object somewhere safe in the world (a safe location will be detected) */
-  //  moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false)
-  //  {
-  //    return place(constructPlaceGoal(object, std::vector<moveit_msgs::msg::PlaceLocation>(), plan_only));
-  //  }
-
-  /** \brief Place an object at one of the specified possible locations */
-  //  moveit::core::MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations,
-  //                        bool plan_only = false)
-  //  {
-  //    return place(constructPlaceGoal(object, std::move(locations), plan_only));
-  //  }
-
-  /** \brief Place an object at one of the specified possible locations */
-  //  moveit::core::MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::msg::PoseStamped>& poses,
-  //                        bool plan_only = false)
-  //  {
-  //    return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
-  //  }
-
-  /** \brief Place an object at one of the specified possible location */
-  //  moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool
-  //  plan_only = false)
-  //  {
-  //    return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
-  //  }
-
-  /** \brief Place an object given a PlaceGoal
-
-      Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other
-      desired variable in the goal, and finally pass it on to this function */
-
-  // moveit::core::MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal);
-
   /** \brief Given the name of an object in the planning scene, make
       the object attached to a link of the robot.  If no link name is
       specified, the end-effector is used. If there is no