Robotics StackExchange | Archived questions

Configuring Motion Planners for MoveIt in C++: Could not find planner,According to the loaded plugin descriptions the class with base class type planning_interface::PlannerManager does not exist. plugin name

Hello, I'm trying to configure a Motion Planner (OMPL) for MoveIt following the MOtion Planning API turorial ( http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/motion_planning_api/motion_planning_api_tutorial.html), but I'm always getting the errors:

[FATAL] [1666945744.248701445]: Could not find planner plugin name

[ERROR] [1666945744.250046759]: Exception while loading planner '': According to the loaded plugin descriptions the class with base class type planninginterface::PlannerManager does not exist. Declared types are chompinterface/CHOMPPlanner ompl_interface/OMPLPlanner

Available plugins: chompinterface/CHOMPPlanner omplinterface/OMPLPlanner

Here is my code for the configuration:

// Instantiate a RobotModelLoader object which will look up the robot description on the ROS parameter server and construct a RobotModel 
  const std::string PLANNING_GROUP = "mc004v_arm";
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

  /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
  robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
  const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

  // Using the`RobotModel`, we can construct a :planning_scene:`PlanningScene` that maintains the state of the world (including the robot).
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));


  // Configure a valid robot state
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

  // Construct a loader to load a planner, by name.
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  // We will get the name of planning plugin we want to load
  // from the ROS parameter server, and then load the planner
  // making sure to catch all exceptions.
  if (!node_handle.getParam("planning_plugin", planner_plugin_name))

    ROS_FATAL_STREAM("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)
  {

    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
      ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
  }



 catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (std::size_t i = 0; i < classes.size(); ++i)
      ss << classes[i] << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                                                         << "Available plugins: " << ss.str());
  }

And this is the launch file:

<launch>

  <node name="motion_planning_cpp" pkg="moveit_da3010" type="motion_planning_cpp" respawn="false" output="screen">
    <rosparam command="load" file="$(find moveit_da3010)/config/kinematics.yaml"/>
    <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
    <rosparam command="load" file="$(find moveit_da3010)/config/ompl_planning.yaml"/>
  </node>

</launch>

I have also set the database start to true.

Thank you for your help!

Asked by sissi12 on 2022-10-28 03:53:18 UTC

Comments

Answers