Octomap only visible if outside collision environment

asked 2021-02-12 15:28:07 -0600

dschimpf gravatar image

updated 2021-02-23 11:26:47 -0600

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.

image description image description

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. image description

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)

edit retag flag offensive close merge delete

Comments

1

Just an observation (and up to you in the end):

frame_id: "OctoCameraPos"

Frame names are expected to follow ROS naming guidelines, meaning: no whitespace, no special characters and no uppercase.

Same for names of topics, services, actions and parameters.

It's (almost certainly) not going to fix what you describe though.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-20 01:16:14 -0600 )edit

Thanks for the suggestion! I made the change, but as you said it did not have an impact on it showing the octomap when it is in the positive z-range of the base link... Is my only other option to add the points as individual collision objects? Do you know if that has an impact on performance (many individual collision objects vs one octomap)? Thanks!

dschimpf gravatar image dschimpf  ( 2021-02-22 15:33:51 -0600 )edit

We still see point_cloud_topic: /OctoCloud, but perhaps you've forgotten to update that.

re: your problem: I would personally try to figure out why your pointcloud has points in the negative Z volume. As REP 103: Standard Units of Measure and Coordinate Conventions states, camera frames should place data along the Z+ axis.

"The rest of ROS" will assume components adhere to that, hence why you observe what you observe (likely, this is just an hypothesis).

I'm also not sure why you transform the pointcloud yourself. That should not be necessary, but I don't know exactly which component you're using here.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-23 03:08:08 -0600 )edit

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.

is it an actual cube, in which you've placed your robot, or a bunch of "flat boxes"?

If the former, (robot) self-filtering may be filtering out all the points which lie "inside the cube", as it considers your box part of the robot.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-23 03:08:41 -0600 )edit

It is a thin-walled cube, so the inside is hollow. Each side could be described as a flat box.

The points are in the positive Z direction of the camera, but by moving the octo_camera_pos frame without transforming the point cloud I can move it in the negative range of the base_link. Also, my most recent findings seem to indicate that it is not related to the positive Z direction of the base-link, but if the points are within the thin-walled cube that represents my collision environment.

dschimpf gravatar image dschimpf  ( 2021-02-23 09:43:36 -0600 )edit