Octomap only visible if outside collision environment

edit

ROS Kinetic (Ubuntu 16)

Hello,

I have a filtered point cloud from which all but collision geometry was removed. This filtered point cloud I am publishing under /OctoCloud (only once if that makes a difference). For tests I moved it to an test frame (octo_camera_pos).

I wanted to follow this tutorial to create an octomap for collision avoidance based on my filtered point cloud. http://docs.ros.org/en/kinetic/api/mo...

I can visualize the point cloud correctly by subscribing rviz to /OctoCloud, but an actual octomap does not show up in rviz unless it is outside of the collision environment.

This is my sensors_kinect_pointcloud.yaml:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /OctoCloud
    max_range: 500
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

This is my sensor_manager.launch.xml

<launch>

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

  <rosparam command="load" file="$(find grindcell_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
  <param name="octomap_frame" type="string" value="octo_camera_pos" />
  <param name="octomap_resolution" type="double" value="0.05" />
  <param name="max_range" type="double" value="50.0" />

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

</launch>

The relevant code from the workcell.xacro that creates the 3D cube collision enviroment:

<link
  name="boundary_environment_real">
  <visual>
    <origin
      xyz="0 0 0"
      rpy="1.5707963267949 0 0" />
    <geometry>
      <mesh
        filename="package://grindcell_support/meshes/boundary_environment_dan.STL" scale="0.001 0.001 0.001"/>
    </geometry>
    <material
      name="">
      <color
        rgba="0.79216 0.81961 0.93333 0.2" />
    </material>
  </visual>
  <collision>
    <origin
      xyz="0 0 0"
      rpy="1.5707963267949 0 0" />
    <geometry>
      <mesh
        filename="package://grindcell_support/meshes/boundary_environment_dan.STL" scale="0.001 0.001 0.001"/>
    </geometry>
  </collision>
</link>

This is how I change the frame from camera_depth_optical_frame to octo_camera_pos for the of the OctoCloud topic message:

  listener.waitForTransform("octo_camera_pos","camera_depth_optical_frame",  ros::Time(0), ros::Duration(3.0));
  listener.lookupTransform("octo_camera_pos","camera_depth_optical_frame",  ros::Time(0), transform);
  pcl_ros::transformPointCloud("octo_camera_pos", transform, leftCP, leftCP);
  leftCP.header.frame_id = "octo_camera_pos";
  outsideCloudpub.publish(leftCP);

When the point cloud within my collision environment (just a 3D cube so that the robot does not run into a wall, ceiling, or floor) the octomap does not show up. The visible coordinate system is the frame octo_camera_pos. Between the two pictures, nothing changed besides turning off the 3D cube collision environment.

I am suspecting now that it has something to do with occlusion? But since the frame octo_camera_pos and the point cloud are within the cube I don't know what occlusion could be.

Edit: Another picture with the collision environment turned on and the octo_camera_pos frame right on the border.

Edit:2 The problem seems to have been that occlusion by the robot's collision geometry. I have the original camera frame within a collision geometry to protect it ... (more)