Moveit collision avoidance using point cloud.

edit

Hi,

I have been stuck for weeks trying to achieve a collision avoidance for my robotic system. I got L515 intel realsense cameras giving me the pointcloud of the environment and I can visualize them realtime on rviz.

What I am trying to achieve is using those cloud points so the robot makes a real time collision avoidance. I saw this video https://www.youtube.com/watch?v=A_fnK... and I tried to use octomap for this porpoise. However I see that the octomap have a several delay compared to the realtime offered by the pointcloud.

https://answers.ros.org/question/4190...

I saw that post and I tried to change the parameters for the octomap that moveit runs. However I dont see any changes modifying hit/miss/mix/max parameters that are described in the previous post.

I also have some issues understanding how is actually octomap running? I have listed the servers with rosservers list and nothing related with octomap appears.

Here is my sensor manager

<launch>

  <param name="octomap_frame" type="string" value="odom_combined" />
  <param name="octomap_resolution" type="double" value="0.06" />
  <param name="octomap_latch" type="bool" value="false"/>

  <param name="octomap_sensor_model/min" value="0.99"/>
  <param name="octomap_sensor_model/max" value="0.01"/>

  <param name="octomap_sensor_model/hit" type="double" value="0.99"/>
  <param name="octomap_sensor_model/miss" type="double" value="0.01"/>

  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="edo" />
  <include file="$(find edo_moveit_gripper)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />

</launch>

and my PointCloudOctomapUpdater

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera_side/depth/color/points
    queue_size: 5
    max_range: 3.0
    point_subsample: 1
    shadow_threshold: 0.2
    padding_scale: 4.0
    padding_offset: 0.03
    max_update_rate: 5
    filtered_cloud_topic: filtered_cloud
    min_range: 0.3