3D Sensor with Moveit2 Hybrid Planner

edit

Hi everyone, I'm really new to this forum, so I apologize if I'm not asking in the right place or in the correct format. I'm developing a ROS2 system able to control a robotic arm in online way avoiding what a 3D sensor publish.

My ROS2 distro is Humble, my Linux system is Ubuntu 22.04 configured with a real-time kernel. I correctly install ros2 and moveit2 dependencies in my workspace. I downloaded and be able to run the Hybrid Planner demo where a panda arm avoid in real time, thanks to the local planner, a collision object added when the first global trajectory is published.

The demo tutorial used is here: https://moveit.picknik.ai/humble/doc/...

The hybrid planner concept is explained here: https://moveit.picknik.ai/humble/doc/...

I build a sensor_3d_reader package that in this initial version publish on a /sensor_3d_topic a pointCloud2 showing a cylinder point cloud that every 1.5 second change his position in the world (to dynamically change the world around the robot). I understand that the consequent task is to use Octomap to correctly let the moveit planner add the collision object.

Now, i see from the concept hybrid planner that in the local planner there are 2 plugins used to add the trajectory and resolve local problem that are initialized to connect with 3d sensor:

"Both plugins receive a shared pointer to the ROS 2 node when they get initialized which can be used to create additional custom ROS 2 communication interfaces for example to subscribe to an additional sensor source."

But i can't find where to setup this 3d sensor.

I also see that there is a octomap server started while playing the demo, in fact from terminal the package moveit_ros_occupancy_map_monitor occupancy_map_monitor_middleware_handle.cpp show an error saying that there is no 3d sensor configured. I think there is a way to tell him which topic to listen, but I'm not sure that is here where i have to add the sensor topic.

So here is my question. How i can add to the word my pointcloud2 to let the hybrid planner see the collision object and make the panda arm to avoid it?

The demo launch use this command: ros2 launch moveit_hybrid_planning hybrid_planning_demo.launch.py

The demo adds 3 boxes as collision object. My plan is to replace them with a cylinder generated from the pointCloud that keep moving in front of the robot. The robot need to avoid it while moving in a online way.