Moveit ompl_planning.model_based_planning_context: Unable to construct goal representation

edit

Hi, i'm trying to use Moveit2 with ROS2 Foxy as a planner for an autonomous drone (quadcopter). In this project i'm trying to use moveit_cpp, for SLAM i'm using rtabmap multi-camera SLAM, i've managed to feed moveit_cpp planning scene monitor with the octomap generated by rtabmap by publishing a PlanningSceneWorld message to /planning_scene_world topic, i've verified this to be correct by visualizing the monitored planning scene in Rviz. I'm following this tutorial and adapting all the parameters to ROS2 format, and the run_moveit_cpp example files for creating the ROS node. The error message seems to be coming from when i called plan() from my PlanningComponent object. I've included the ROS node and launch file configurations below. I'm very new to Moveit2 and fairly new with ROS2, i'm sorry if i missed something, any help would be appreciated. Thank you!

My environment

• ROS Distro: Foxy
• OS Version: Ubuntu 20.04
• moveit 2 installed from source
• moveit 2 foxy version

Steps to reproduce

robot.urdf (the robot urdf is published by robot_state_publisher)

robot.srdf

moveit_cpp.yaml

moveit_controllers.yaml

moveit_ompl_planning.yaml

moveit_joint_limits.yaml

launch file:

<launch>
  <node pkg="flight_navigator" exec="planner_node" output="screen">
    <param name="robot_description_semantic" value="$(command 'xacro $(find-pkg-share aidrone_sim)/models/simple_quadcopter/iris_depth_cameras.srdf.xacro')"/>
    <param name="publish_robot_description_semantic" value="true"/>

    <param from="$(find-pkg-share flight_navigator)/config/moveit_cpp.yaml"/>
    <param from="$(find-pkg-share flight_navigator)/config/moveit_controllers.yaml"/>
    <param from="$(find-pkg-share flight_navigator)/config/moveit_ompl_planning.yaml"/>
    <param from="$(find-pkg-share flight_navigator)/config/moveit_joint_limits.yaml"/>
  </node>

  <node pkg="flight_navigator" exec="planning_scene_publisher" output="screen"/>
</launch>

planner.cpp:

#include <memory>
#include <thread>

#include <rclcpp/rclcpp.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("Planner");
static const std::string PLANNING_COMPONENT = "base";
static const std::string BASE_LINK = "base_link";

class Planner {
  public:
    Planner(const rclcpp::Node::SharedPtr& node) : node_(node) {}

    void run() {
      RCLCPP_INFO(LOGGER, "Initializing MoveItCpp");

      moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(node_);
      moveit_cpp_->getPlanningSceneMonitor()->startWorldGeometryMonitor("", "planning_scene_world", false);
      moveit_cpp_->getPlanningSceneMonitor()->startPublishingPlanningScene(moveit_cpp_->getPlanningSceneMonitor()->UPDATE_SCENE);

      RCLCPP_INFO(LOGGER, "Initializing planning components");
      robot_ = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_COMPONENT, moveit_cpp_);

      // A little delay before running the plan
      rclcpp::sleep_for(std::chrono::seconds(3));

      robot_->setStartStateToCurrentState();
      geometry_msgs::msg::PoseStamped target_pose;
      target_pose.header.frame_id = "map";
      target_pose.pose.position.z = 1.5;
      target_pose.pose.position.x = 1.0;
      robot_->setGoal(target_pose, BASE_LINK);

      // Error in this line
      auto plan_solution = robot_->plan();

      if (plan_solution) {
        RCLCPP_INFO(LOGGER, "Plan successfully created");
      }

    }
  private:
    rclcpp::Node::SharedPtr node_;
    moveit_cpp::MoveItCppPtr moveit_cpp_;
    moveit_cpp::PlanningComponentPtr robot_;
};

int main(int argc, char** argv) {
  RCLCPP_INFO(LOGGER, "Starting path planner");
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  rclcpp::Node::SharedPtr planner_node = rclcpp::Node::make_shared("planner_node", "", node_options);

  Planner planner(planner_node);
  std::thread run_planner([&planner]() {
    rclcpp::sleep_for(std::chrono::seconds(5));
    planner.run();
  });

  rclcpp::spin(planner_node);
  run_planner.join();

  rclcpp::shutdown();

  return 0;
}

Expected behaviour

moveit_cpp should be able to contruct the plan

Actual behaviour

moveit_cpp failed to construct goal representation

Backtrace or Console output

output