TM5-900 "No kinematics plugins defined." and "Failed to call service get_planning_scene"

Introduction

Hello, I'm trying to make the TM ROS2 Driver (tmr_ros2) work with the main brach MoveIt2, because I intend to use moveit_py later. I made some alterations to the source material, but I'm not familiar enough with ROS2 to now what I'm doing exactly.

Enviroment

Ubuntu 22.04 on WSL
ROS2 Humble - Binary
MoveIt2 - Source from main branch
TM ROS2 Driver - Source with moveit packages from Foxy branch

This is how I got here:

I'm using Ubuntu 22.04 and installed ROS2 Humble. Then I followed the Moveit2 guide to bulid it from the main branch. Aside from a couple of type casting warnings, everything went smoothly. Since the TM ROS2 Driver's Humble branch doesn't contain the Moveit2 packages I had to copy it from the Foxy branch (a month ago I used the driver just like this without any issue but I could only use Cartesian Planning in RViz). Building the driver with the source built MoveIt2 :

--- stderr: tm_moveit_cpp_demo                               
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:134:11: error: ‘planning_interface’ in namespace ‘moveit’ does not name a type; did you mean ‘planning_pipeline_interfaces’?
  134 |   moveit::planning_interface::MoveItCppPtr moveit_cpp_;
      |           ^~~~~~~~~~~~~~~~~~
      |           planning_pipeline_interfaces
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp: In member function ‘void MoveItCppDemo::run()’:
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:67:5: error: ‘moveit_cpp_’ was not declared in this scope; did you mean ‘moveit_cpp’?
   67 |     moveit_cpp_ = std::make_shared<moveit::planning_interface::MoveItCpp>(node_);
      |     ^~~~~~~~~~~
      |     moveit_cpp
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:67:44: error: ‘planning_interface’ is not a member of ‘moveit’; did you mean ‘planning_pipeline_interfaces’?
   67 |     moveit_cpp_ = std::make_shared<moveit::planning_interface::MoveItCpp>(node_);
      |                                            ^~~~~~~~~~~~~~~~~~
      |                                            planning_pipeline_interfaces
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:67:24: error: parse error in template argument list
   67 |     moveit_cpp_ = std::make_shared<moveit::planning_interface::MoveItCpp>(node_);
      |                        ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:67:74: error: no matching function for call to ‘make_shared<<expression error> >(rclcpp::Node::SharedPtr&)’
   67 |     moveit_cpp_ = std::make_shared<moveit::planning_interface::MoveItCpp>(node_);
      |                   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
In file included from /usr/include/c++/11/memory:77,
                 from /opt/ros/humble/include/rclcpp/rclcpp/rclcpp.hpp:153,
                 from /home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:46:
/usr/include/c++/11/bits/shared_ptr.h:875:5: note: candidate: ‘template<class _Tp, class ... _Args> std::shared_ptr<_Tp> std::make_shared(_Args&& ...)’
  875 |     make_shared(_Args&&... __args)
      |     ^~~~~~~~~~~
/usr/include/c++/11/bits/shared_ptr.h:875:5: note:   template argument deduction/substitution failed:
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:67:74: error: template argument 1 is invalid
   67 |     moveit_cpp_ = std::make_shared<moveit::planning_interface::MoveItCpp>(node_);
      |                   ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:72:13: error: ‘moveit::planning_interface’ has not been declared
   72 |     moveit::planning_interface::PlanningComponent arm("tmr_arm", moveit_cpp_);
      |             ^~~~~~~~~~~~~~~~~~
/home/andrasmakany/tmdriver_ws/src/tm_moveit_cpp_demo/src/run_moveit_cpp.cpp:103:5: error: ‘arm’ was not declared in this scope
  103 |     arm.setGoal("ready1");
      |     ^~~
gmake[2]: *** [CMakeFiles/run_moveit_cpp.dir/build.make:76: CMakeFiles/run_moveit_cpp.dir/src/run_moveit_cpp.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:137: CMakeFiles/run_moveit_cpp.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< tm_moveit_cpp_demo [9.20s, exited with code 2]

Summary: 17 packages finished [9.43s]
  1 package failed: tm_moveit_cpp_demo
  1 package had stderr output: tm_moveit_cpp_demo

As best as I could I corrected these until the package built successfully. The end result was:

#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>

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

class MoveItCppDemo
{
public:
  MoveItCppDemo(const rclcpp::Node::SharedPtr& node)
    : node_(node)
    , robot_state_publisher_(node_->create_publisher<moveit_msgs::msg::DisplayRobotState>("display_robot_state", 1))
  {
  }

  void run()
  {
    RCLCPP_INFO(LOGGER, "Initialize MoveItCpp");
    moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(node_);
    moveit_cpp_->getPlanningSceneMonitorNonConst()->providePlanningSceneService();  // let RViz display query PlanningScene
    RCLCPP_INFO(LOGGER, "Planning Scene Service Provided");
    moveit_cpp_->getPlanningSceneMonitorNonConst()->setPlanningScenePublishingFrequency(100);

    RCLCPP_INFO(LOGGER, "Initialize PlanningComponent");
    moveit_cpp::PlanningComponent arm("tmr_arm", moveit_cpp_);

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

    // Create collision object, planning shouldn't be too easy
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = "base";
    collision_object.id = "box";

    shape_msgs::msg::SolidPrimitive box;
    box.type = box.BOX;
    box.dimensions = { 0.1, 0.2, 0.1 };

    geometry_msgs::msg::Pose box_pose;
    box_pose.position.x = 0.7;
    box_pose.position.y = 0.0;
    box_pose.position.z = 0.9;

    collision_object.primitives.push_back(box);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    // Add object to planning scene
    {  // Lock PlanningScene
      planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst());
      scene->processCollisionObjectMsg(collision_object);
    }  // Unlock PlanningScene

    // Set joint state goal
    RCLCPP_INFO(LOGGER, "Set goal");
    arm.setGoal("ready1");

    // Run actual plan
    RCLCPP_INFO(LOGGER, "Plan to goal");
    auto plan_solution = arm.plan();
    if (plan_solution)
    {
      RCLCPP_INFO(LOGGER, "arm.execute()");
      moveit_cpp_->execute(plan_solution.trajectory);
    }
  }

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Publisher<moveit_msgs::msg::DisplayRobotState>::SharedPtr robot_state_publisher_;
  std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_;
};

int main(int argc, char** argv)
{
  RCLCPP_INFO(LOGGER, "Initialize node");
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  // This enables loading undeclared parameters
  // best practice would be to declare parameters in the corresponding classes
  // and provide descriptions about expected use
  node_options.automatically_declare_parameters_from_overrides(true);
  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options);

  MoveItCppDemo demo(node);
  std::thread run_demo([&demo]() {
    rclcpp::sleep_for(std::chrono::seconds(5));
    demo.run();
  });

  rclcpp::spin(node);
  run_demo.join();

  return 0;
}

Before launch I imported my older configs, namely: In tm_moveit_config_tm5-900/config/ompl_planning.yaml:

tmr_arm:
  default_planner_config: STRIDE
...
enforce_constrained_state_space: True
projection_evaluator: joints(joint_1, joint_6)
longest_valid_segment_fraction: 0.05

In tm_moveit_config_tm5-900/config/ros_controllers.yaml:

controller_list:
  - name: tmr_arm_controller
    allowed_execution_duration_scaling: 1.2
    allowed_goal_duration_margin: 0.5

In tm_moveit_cpp_demo/config/controllers.yaml:

tmr_arm_controller:
  allowed_execution_duration_scaling: 1.2
  allowed_goal_duration_margin: 0.5

The error

With all of these I launched the demo with ros2 launch tm_moveit_cpp_demo tm5-900_run_moveit_cpp.launch.py. The following happened:

[INFO] [tm_driver-1]: process started with pid [202190]
[INFO] [static_transform_publisher-2]: process started with pid [202192]
[INFO] [robot_state_publisher-3]: process started with pid [202194]
[INFO] [rviz2-4]: process started with pid [202196]
[INFO] [run_moveit_cpp-5]: process started with pid [202198]
[tm_driver-1] ip is not found, use fake robot
[tm_driver-1] only ip or robot_ip support, but your type is --ros-args
[tm_driver-1] [INFO] [1713920509.426921788] [rclcpp]: Ethernet slave communication: TmSvrCommunication
[tm_driver-1] [INFO] [1713920509.426963014] [rclcpp]: Listen node communication: TmSctCommunication
[static_transform_publisher-2] [WARN] [1713920509.427747715] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-2] [INFO] [1713920509.435362839] [static_transform_publisher]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'world' to 'base'
[tm_driver-1] [INFO] [1713920509.436646830] [rclcpp]: TM_ROS: fake publisher thread begin
[robot_state_publisher-3] [INFO] [1713920509.446188329] [robot_state_publisher]: got segment base
[robot_state_publisher-3] [INFO] [1713920509.446378359] [robot_state_publisher]: got segment flange
[robot_state_publisher-3] [INFO] [1713920509.446388575] [robot_state_publisher]: got segment link_0
[robot_state_publisher-3] [INFO] [1713920509.446393934] [robot_state_publisher]: got segment link_1
[robot_state_publisher-3] [INFO] [1713920509.446399517] [robot_state_publisher]: got segment link_2
[robot_state_publisher-3] [INFO] [1713920509.446404584] [robot_state_publisher]: got segment link_3
[robot_state_publisher-3] [INFO] [1713920509.446411224] [robot_state_publisher]: got segment link_4
[robot_state_publisher-3] [INFO] [1713920509.446415737] [robot_state_publisher]: got segment link_5
[robot_state_publisher-3] [INFO] [1713920509.446421733] [robot_state_publisher]: got segment link_6
[run_moveit_cpp-5] [INFO] [1713920509.452520005] [moveit_cpp_demo]: Initialize node
[rviz2-4] [INFO] [1713920510.101016752] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [INFO] [1713920510.101152575] [rviz2]: OpenGl version: 4.1 (GLSL 4.1)
[rviz2-4] [INFO] [1713920510.126937511] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [WARN] [1713920510.188831885] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-4] [INFO] [1713920510.198669994] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00198231 seconds
[rviz2-4] [INFO] [1713920510.198751108] [rviz2.moveit.core.robot_model]: Loading robot model 'tm5-900'...
[run_moveit_cpp-5] [INFO] [1713920514.465098314] [moveit_cpp_demo]: Initialize MoveItCpp
[run_moveit_cpp-5] [INFO] [1713920514.471878021] [moveit_3730791688.moveit.ros.rdf_loader]: Loaded robot model in 0.00169684 seconds
[run_moveit_cpp-5] [INFO] [1713920514.471929470] [moveit_3730791688.moveit.core.robot_model]: Loading robot model 'tm5-900'...
[run_moveit_cpp-5] [WARN] [1713920514.503526798] [moveit_3730791688.moveit.ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[run_moveit_cpp-5] [INFO] [1713920514.574576439] [moveit_3730791688.moveit.ros.moveit_cpp]: Listening to '/joint_states' for joint states
[run_moveit_cpp-5] [INFO] [1713920514.575210998] [moveit_3730791688.moveit.ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[run_moveit_cpp-5] [INFO] [1713920514.576477109] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Listening to '/moveit_cpp/planning_scene_monitor' for attached collision objects
[run_moveit_cpp-5] [INFO] [1713920514.577554452] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on '/moveit_cpp/monitored_planning_scene'
[run_moveit_cpp-5] [INFO] [1713920514.577919077] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[run_moveit_cpp-5] [INFO] [1713920514.579473328] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Listening to '/moveit_cpp/publish_planning_scene'
[run_moveit_cpp-5] [INFO] [1713920514.579546847] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[run_moveit_cpp-5] [INFO] [1713920514.580603657] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Listening to 'collision_object'
[run_moveit_cpp-5] [INFO] [1713920514.581353487] [moveit_3730791688.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[run_moveit_cpp-5] [WARN] [1713920514.582111793] [moveit_3730791688.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[run_moveit_cpp-5] [ERROR] [1713920514.582171489] [moveit_3730791688.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[rviz2-4] [INFO] [1713920514.589339549] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00194532 seconds
[rviz2-4] [INFO] [1713920514.589422218] [rviz2.moveit.core.robot_model]: Loading robot model 'tm5-900'...
[rviz2-4] [WARN] [1713920514.622158312] [rviz2.moveit.ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[run_moveit_cpp-5] [INFO] [1713920514.650695221] [moveit_3730791688.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL'
[run_moveit_cpp-5] [INFO] [1713920514.653099982] [run_moveit_cpp]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[run_moveit_cpp-5] [INFO] [1713920514.655213001] [run_moveit_cpp]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[run_moveit_cpp-5] [INFO] [1713920514.655310477] [run_moveit_cpp]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[run_moveit_cpp-5] [INFO] [1713920514.656588259] [run_moveit_cpp]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[run_moveit_cpp-5] [INFO] [1713920514.656668648] [run_moveit_cpp]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[run_moveit_cpp-5] [INFO] [1713920514.656936921] [run_moveit_cpp]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[run_moveit_cpp-5] [INFO] [1713920514.656970915] [run_moveit_cpp]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[run_moveit_cpp-5] [INFO] [1713920514.657003259] [run_moveit_cpp]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[run_moveit_cpp-5] [INFO] [1713920514.657017623] [run_moveit_cpp]: Try loading adapter 'default_planning_request_adapters/AddTimeOptimalParameterization'
[run_moveit_cpp-5] [FATAL] [1713920514.657405497] [run_moveit_cpp]: Exception while loading planning adapter plugin 'default_planning_request_adapters/AddTimeOptimalParameterization': According to the loaded plugin descriptions the class default_planning_request_adapters/AddTimeOptimalParameterization with base class type planning_interface::PlanningRequestAdapter does not exist. Declared types are  default_planning_request_adapters/CheckForStackedConstraints default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds planning_pipeline_test/AlwaysSuccessRequestAdapter
[run_moveit_cpp-5] terminate called after throwing an instance of 'pluginlib::LibraryLoadException'
[run_moveit_cpp-5]   what():  According to the loaded plugin descriptions the class default_planning_request_adapters/AddTimeOptimalParameterization with base class type planning_interface::PlanningRequestAdapter does not exist. Declared types are  default_planning_request_adapters/CheckForStackedConstraints default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/ResolveConstraintFrames default_planning_request_adapters/ValidateWorkspaceBounds planning_pipeline_test/AlwaysSuccessRequestAdapter
[ERROR] [run_moveit_cpp-5]: process has died [pid 202198, exit code -6, cmd '/home/andrasmakany/tmdriver_ws/install/tm_moveit_cpp_demo/lib/tm_moveit_cpp_demo/run_moveit_cpp --ros-args --params-file /home/andrasmakany/tmdriver_ws/install/tm_moveit_cpp_demo/share/tm_moveit_cpp_demo/config/moveit_cpp.yaml --params-file /tmp/launch_params_m6n1ld7w --params-file /tmp/launch_params_bb_ck12g --params-file /tmp/launch_params_1lhfgqdh --params-file /tmp/launch_params_o25tn7te --params-file /tmp/launch_params_s23l7lkq'].
[rviz2-4] [INFO] [1713920514.694690163] [rviz2.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[rviz2-4] [INFO] [1713920514.696627029] [rviz2.moveit.ros.planning_scene_monitor]: Listening to '/moveit_cpp/publish_planning_scene'
[rviz2-4] [INFO] [1713920519.748710400] [rviz2.moveit.ros.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?

Generally this is what happens every time, but the "run_moveit_cpp" node dies rarely before the last row's fail. I've checked a few issues regarding the "No kinematics plugins defined.", but with no improvement. What could be a solution?

Thanks in advance!

moveit / moveit2