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

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](http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/perception_configuration.html) 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: And also the linked file: 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.