Moveit octomap collision avoidance using point cloud data from Microsoft Kinect v2

edit

Hi again,

I've been trying to create a collision map of the surroundings (with rtabmap) for a Universal Robot ur5 using a Microsoft Kinect v2 mounted to an elbow close to the end effector of the robot. I am using ROS Indigo and Ubuntu 14.04.

The mapping works fine and now I want the robot to avoid the objects mapped in rtabmap.

I followed the 3D perception tutorial for moveit trying to update the internal moveit octomap directly using the point cloud output of my rtabmap with the help of the rgbd_kinect.yaml:

sensors:
  -sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /rtabmap/octomap_cloud
  max_range: 2.0
  point_subsample: 1
  padding_offset: 0.05
  padding_scale: 1.0
  filtered_cloud_topic: filtered_cloud

The point cloud topic is being published fine (I can visualize it in Rviz without any problems).

I've also updated the sensor_manager.launch as described in the tutorial:

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->  

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name ="octomap_frame" type ="string" value ="base_link"/>
  <param name ="octomap_resolution" type ="double" value ="0.01"/>
  <param name ="max_range" type ="double" value ="2"/>

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

</launch>

And also the linked file:

<launch>
    <rosparam command ="load" file ="/home/matthias/catkin_ws/src/rgbd_config/rgbd_kinect.yaml"/>
</launch>

However the robot will still crash into the identified objects as if he was not aware of them at all... Do I need to include some sort of check during the movement execution in my motion_publisher:

joints1["wrist_2_joint"] = pi/2; 
joints1["wrist_3_joint"] = 0; 
group.setJointValueTarget(joints1); 
my_plan;
sleep(5);
group.move();

Or in what way do I need to change something in order for the robot to be aware of the point cloud objects? I've really tried everything and don't really know what to change anymore... Any suggestions in any kind of direction are highly appreciated!

Many Thanks.