[perception] octomap not properly cleared when collision model is added

[bit-pirate]

When adding collision models into the planning scene, the octomap voxels covered by the shape of the collision object don't get properly cleared (some remain).

This issues is described in more detailed (including pictures) on the mailing list: https://groups.google.com/forum/#!topic/moveit-users/T2feBu5uNos

I'm not sure, if this is a bug in the code, a performance issue or a wired synchronisation problem.

[bit-pirate]

After more testing (both SolidPrimitive shapes and with meshes from the household database) I get the feeling the problematic voxels are the ones inside (or very close to) the shapes. I added and removed a lot of collision objects (using this simple script [1]) and although this problem doesn't occur all the time it occurs often enough to be a show stopper.

I'll keep looking for more hints about the cause.

[1] requires the household object database to be installed and the object information server be running (roslaunch object_recognition_ros object_information_server.launch)

[mikeferguson]

@isucan I'm also seeing something like this, but I think the actual cause of the problem is a bit different than @bit-pirate conclusion: basically once you insert an object, it only clears out the associated voxels when a point hits that voxel. As a simple test, I pointed my robot at a table, insert a collision object that is 2x2x2m at pose 1,0,0, which takes up most of the table space, you can see as soon as it's inserted, most points disappear -- but some remain. Eventually some of them go away - but any points outside the field of view remain for sure (and a few inside).

I made a quick video of this: http://www.youtube.com/watch?v=Ig1uSNrlHbQ&feature=youtu.be

Is there some easy fix to the pointcloud updater to make this work? Or should we be looking at switching to the depthimage updater (although I presume it has the same issue).

[adolfo-rt]

When Octomap refreshes, it clears the cells intersecting a ray up to the point it hits an actual (current) obstacle. Octomap has no way of knowing if the points beyond the ray hit are occupied or not, hence does not clear them. This is expected Octomap behavior.

If resetting the map and taking a new pointcloud is not an option, it may be reasonable to propose a feature request for a sensor model in which obstacles beyond a ray hit (and for efficiency, within a workspace volume) are cleared from the map.

[mikeferguson]

@adolfo-rt Regardless of the ray tracing, when you put a collision object or attached object into the scene, it should clear out the octomap in that region

[adolfo-rt]

@adolfo-rt Regardless of the ray tracing, when you put a collision object or attached object into the scene, it should clear out the octomap in that region

Sounds reasonable.

[isucan]

What I believe is happening here is that when a new object is registered in the world, there is no update of the octomap step. That step would be something like that gets executed when the OccupancyMapUpdater::excludeShape() function gets called. In theory, the function would run a collision check between the new shape and the collision checker. That step is not included in any of the octomap updaters because points that are inside primitives are not going to be removed (as there is no collision reported by FCL). Furthermore, this could be an expensive operation if we were to use point containment tests to update the octomap. Perhaps we should do that anyway... not sure.

Instead, what is being done is I guess what you observed: points that you see again are removed if they overlap with the object. This is done with point containment test operations in the point cloud updater, so points inside boxes should be removed. The problem is you do not see all the exact same points over and over again and not all points will be removed.

When using the depth octomap updater, the number of points is much higher and chances of removing points that need to be removed are much higher. The problem there is that everything is done with a depth buffer. So you have to specify a maximum depth for the points you remove. In any case, I recommend trying to use this updater instead.

This does not fully solve the problem. I do not know of something terribly simple that we can do here to make things be nice & general & fast & work as expected. I plan to work on this when I get my hands on some hardware. In the meantime, can you try to use the other updater to see if things work better? (this one requires graphical acceleration and an X server running)

[bit-pirate]

Thanks for the feedback guys! I'm now trying to use the depth image updater.

Does anyone by chance have any hints on how to start the xserver using normal ssh and connecting to it ? My attempts using start kdm, startx have failed so far. Using ssh -Y leads to

freeglut ():  ERROR:  Internal error <FBConfig with necessary capabilities not found> in function fgOpenWindow
X Error of failed request:  BadWindow (invalid Window parameter)
  Major opcode of failed request:  4 (X_DestroyWindow)
  Resource id in failed request:  0x0
  Serial number of failed request:  37
  Current serial number in output stream:  40
move_group: /usr/include/boost/thread/pthread/mutex.hpp:47: boost::mutex::~mutex(): Assertion `!pthread_mutex_destroy(&m)' failed.

/edit: I got further using desktop sharing (not ideal though) and got stuck with this:

Program received signal SIGSEGV, Segmentation fault.
(gdb) bt
#0  0x0000000000000000 in ?? ()
#1  0x00007fffba8baaa9 in mesh_filter::GLRenderer::initFrameBuffers() () from /opt/korus_workspace/moveit_ws_hydro-devel/devel/lib/libmoveit_mesh_filter.so
#2  0x00007fffba8b2edc in mesh_filter::MeshFilterBase::initialize(std::string const&, std::string const&, std::string const&, std::string const&) ()
   from /opt/korus_workspace/moveit_ws_hydro-devel/devel/lib/libmoveit_mesh_filter.so
#3  0x00007fffba8b3280 in mesh_filter::MeshFilterBase::run(std::string const&, std::string const&, std::string const&, std::string const&) ()
   from /opt/korus_workspace/moveit_ws_hydro-devel/devel/lib/libmoveit_mesh_filter.so
#4  0x00007ffff44d8ce9 in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1
#5  0x00007ffff7580e9a in start_thread () from /lib/x86_64-linux-gnu/libpthread.so.0
#6  0x00007ffff588eccd in clone () from /lib/x86_64-linux-gnu/libc.so.6
#7  0x0000000000000000 in ?? ()

Any idea about the reason behind this failure? The depth image octomap updater runs without problems on my desktop machine - the difference being my desktop machine running a NVidia graphics card (GL_RENDERER = GeForce GTX 550 Ti/PCIe/SSE2, GL_VERSION = 4.2.0 NVIDIA 304.88, GL_VENDOR = NVIDIA Corporation ) and the robot only an integrated Intel GPU (GL_RENDERER = Mesa DRI Intel(R) Sandybridge Mobile, GL_VERSION = 3.0 Mesa 8.0.4, GL_VENDOR = Tungsten Graphics, Inc). Is the Intel GPU missing any important feature?

[bit-pirate]

OK, I found a fix for the segfault (see PR #329).

Unfortunately, it doesn't look like the depth image updater solves the issue discussed here. After adding/attaching objects my octomap still doesn't get cleared properly.

@isucan how do I set this depth buffer you are talking about? Here is my current config file (sensor.yaml):

octomap_frame: base_footprint
octomap_resolution: 0.02
max_range: 3.0
sensors: 
  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    sensor_type: asus_xtion_pro_live
    image_topic: /korus/sensor_3d/depth/image_raw
    filtered_cloud_topic: filtered_cloud
    queue_size: 1
    near_clipping_plane_distance_: 0.5
    far_clipping_plane_distance_: 3.0
    shadow_threshold: 0.04
    padding_scale: 1.0
    padding_offset: 0.03
    skip_vertical_pixels: 4
    skip_horizontal_pixels: 6
    self_mask:
      min_sensor_dist: .5
      self_see_default_padding: 0.03
      self_see_default_scale: 1.0
      self_see_links:
       - [...]

[isucan]

Please try the configs as described here: http://moveit.ros.org/wiki/3D_Sensors

[bit-pirate]

After many distractions I'm finally back with some feedback:

Although using the DepthImageOctomapUpdater did improve the situation for me, i.e. significantly lower computation load and better removal of contained Octomap voxels, it didn't resolve this issue completely. Still voxels remain inside and at the boarder of newly added and attached collision objects. I often get away with this, since I'm using collision-unaware operations (e.g. IK) in those situations. However, I'm also experiencing this clearing issue with points on and inside the robot links, e.g. when using a badly calibrated robot or badly aligned 3D sensor. Additionally, this happens sometimes when the robot is moving.

I gave up on tuning the padding_offset and padding_scale parameters for the DepthImageOctomapUpdater (see #342 for the reasons). The other parameters seem not to be related to this issue. Are the other parameters I could tune or other ways to solve/improve on this clearance issue?

[skohlbr]

I´m currently looking into how the whole adding objects business works and have a question (highly related to this issue): Our workflow works essentially like this: Our robot system walks into an unknown environment while creating a octomap in real-time. It walks up to a object of interest and the operator or automatic object detection estimate the pose of the object. At this point the object gets added to the planning scene. It appears that with the current state of affairs, the object will not be cleared out of the planning scene octomap, as the octomap updaters will only use the object for exclusion from the octomap from the moment on it has been added. What appears to be missing is a "clear object from Octomap" step that should be executed (once) as soon as the object is added to planning scene. Is this correct? Are there any good ideas (or even implementations) of how this steps can be added? The hacky intermediate solution would be to clear a box around the object in the octomap. Clearing the whole Octomap every time we detect an object really doesn´t sound very appealing :)

[romayiii]

Hi,

In our case we are having the contrary problem to bit-pirate.

The Octomap is been cleared too much around a collision object (image attached). I would expect that the cells inside and the ones really close to the object will get cleared, but in this case there are cell that are considerably away from the object that are being cleared.

I can imagine there is some kind of padding parameter for collision objects but I'm not able to find it.

Any suggestions? Thanks in advance

Using Trusty, Indigo and the latest deb packages.

[davetcoleman]

Yes, there is an adjustable padding as you can see here: https://github.com/ros-planning/moveit_pr2/blob/indigo-devel/pr2_moveit_config/config/sensors_kinect.yaml

dave 325.261.3283 c

On Wed, Oct 22, 2014 at 1:36 PM, Alberto Romay [email protected] wrote:

Hi,

In our case we are having the contrary problem to bit-pirate.

The Octomap is been cleared too much around a collision object (image attached). I would expect that the cells inside and the ones really close to the object will get cleared, but in this case there are cell that are considerably away from the object that are being cleared.

I can imagine there is some kind of padding parameter for collision objects but I'm not able to find it.

Any suggestions? Thanks in advance

Using Trusty, Indigo and the latest deb packages.

[image: screenshot from 2014-10-22 14 44 08] https://cloud.githubusercontent.com/assets/8075064/4742714/387c1610-5a21-11e4-8d56-1c06f9d5037d.png [image: screenshot from 2014-10-22 15 18 29] https://cloud.githubusercontent.com/assets/8075064/4742713/387b0ee6-5a21-11e4-8421-10edfb40c94c.png

— Reply to this email directly or view it on GitHub https://github.com/ros-planning/moveit_ros/issues/315#issuecomment-60142172 .

[corot]

I have a related problem, but not sure if it's the same reported by @bit-pirate. I posted it on MoveIt! users group

[dornhege]

From what I've seen the explanation by @isucan is the correct one. It also seems that this problem is more prevalent in simulation as on the real robot sensor noise helps to cover all voxels to be removed.

Given, how it is currently implemented I believe that the suggestion by @skohlbr is actually the best one. This could be done by a dummy octomap updater that doesn't listen to any sensors, but only handles excludeShape. This could remember the already excluded shapes and thus only perform this once.