How to use MoveIt2 without ros2_control
Hi all! At our company we are trying to use MoveIt2 (MoveGroupInteface) to plan trajectories for our robots. As of now, we are not using ros2_control to communicate with the real hardware, and hence we don't use joint_state_broadcaster to publish joint states. Instead, we are publishing them from our own ros2 node. This seems to break fcl collision check and we often get messages like the following:
Even though the srdf disables collisions between these links. This seems to happen only when a planning scene is published. Trying with several options, again, it seems that if the planning scene is published using a publisher of PlanningScene message to the /planning_scene topic, this error happens. Instead, if PlanningSceneInterface is used with the method addCollisionObjects, the planning is successful.
In general, the documentation or the tutorials explain only the case in which ros2_control is used to publish joint states. We seem to have understood that this problem has something to do with getting properly the state of the robot. However, it is not entirely clear and we believe it would be interesting to understand how to keep MoveIt2 planning independent from ros2_control while publishing joint states on our own. What are the specifications that such a joint state publisher should meet in order to work with MoveIt2?
Our environment
- ROS Distro: Humble
- OS Version: Ubuntu 22.04
- Docker Image: ykrobotics/humble-dev
Steps to reproduce
To reproduce the issue, we have prepared a working demo.
- Clone the repo and open in container.
- In a terminal, run
ros2 launch moveit_debug demo.launch.py - Then, in a separate terminal
ros2 launch moveit_debug collision.launch.py
Lines 100 or 101 of src/moveit_collision.cpp can be commented to try publishing planning scene through a classic publisher or PlanningSceneInterface.
Expected behaviour
The planning produced valid trajectories and are "executed" in RViz.
Actual behaviour
When publishing using a publisher to the /planning_scene topic, unexpected collisions are found. This is not the case when using planning_scene_interface.addCollisionObjects(collision_objects);.
We believe this is quite a common and important issue, which is not receiving the deserved interest from the community.
