How to change the default planner of OMPL pipeline

edit

Hi,

I'm trying to build my own path planner node according to the moveit2 tutorial motion_planning_api. The problem is, that I get really bad results. I found out that the tutorial uses the RTTConnect planner which seems according to my research pretty bad. Therefore I wanted to change the default planner in the ompl_planning.yaml to something else but it turns out that RRTConnect is used nevertheless. How is it possible to change the planner then? Everything I found so far didn't work for me... (group.setPlannerId("BKPIECE")) or can't I see the forest for the trees?

• ROS2: Foxy
• Moveit2: Foxy
• Ubuntu 20.04

Below you see the relevant code:

path_planner.cpp

#include <pluginlib/class_loader.hpp>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <iostream>
using namespace std;

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
using std::placeholders::_1;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("path_planner");

class PathPlanner : public rclcpp::Node
{ 
public: 
  PathPlanner() 
  : Node("PathPlanner")
  {
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    std::shared_ptr<rclcpp::Node> path_planner_node = rclcpp::Node::make_shared("path_planner_node", node_options);

    std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisherHelper = 
          path_planner_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1);
    display_publisher = display_publisherHelper;

    subscription_ = 
          this->create_subscription<std_msgs::msg::Float64MultiArray>("cartesian_goal", 10, std::bind(&PathPlanner::topic_callback, this, _1));

    robot_model_loader::RobotModelLoader robot_model_loader(path_planner_node, "robot_description");
    const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();

    planning_scene::PlanningScenePtr planning_sceneHelper(new planning_scene::PlanningScene(robot_model));
    planning_sceneObj = planning_sceneHelper;


    moveit::core::RobotStatePtr robot_stateHelper(new moveit::core::RobotState(robot_model));
    robot_state = robot_stateHelper;

    const std::string PLANNING_GROUP = "manipulator";
    const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

    planning_sceneObj->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "up");

    std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
    std::string planner_plugin_name;

    if (!path_planner_node->get_parameter("planning_plugin", planner_plugin_name))
      RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
    try
    {
      planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
          "moveit_core", "planning_interface::PlannerManager"));
    }
    catch (pluginlib::PluginlibException &ex)
    {
      RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
    }
    try
    {
      planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
      if (!planner_instance->initialize(robot_model, path_planner_node,
                                        path_planner_node->get_namespace()))
        RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
      RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance->getDescription().c_str());
    }
    catch (pluginlib::PluginlibException &ex)
    {
      const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
      std::stringstream ss;
      for (const auto &cls : classes)
        ss << cls << " ";
      RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
                   ex.what(), ss.str().c_str());
    }
  }
  private:
    void topic_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg) const
    {
      planning_interface::MotionPlanRequest req;
      planning_interface::MotionPlanResponse res;
      geometry_msgs::msg::PoseStamped pose;
      pose.header.frame_id = "base_link";
      pose.pose.position.x = msg->data[0];
      pose.pose.position.y = msg->data[1];
      pose.pose.position.z = msg->data[2];
      pose.pose.orientation.w = msg->data[3];

      // planner_instance.solve_type = "Distance"

      std::vector<double> tolerance_pose(3, 0.001);
      std::vector<double> tolerance_angle(3, 0.001);

      // moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
      moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose);


      const std::string PLANNING_GROUP = "manipulator";
      const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

      req.group_name = PLANNING_GROUP;

      // Adding Workspace Parameters to exclude regions from path planning -- Alternatively configure ...