How can Dynablox be used for motion planning?

[9woods123]

Hello, great job! As we know, Voxblox is used not only for reconstructing 3D meshes but also for generating ESDF maps for robot motion planning. My question is: How can Dynablox be used for motion planning?

The planning example for Voxblox mentioned here https://voxblox.readthedocs.io/en/latest/pages/Using-Voxblox-for-Planning.html states that you should use esdf_server node and subscribe to the incremental mapping data in the planner node. However, in Dynablox, it uses tsdf_server instead. I am wondering if there is an easy method to generate an ESDF map while also considering dynamic obstacle detection using Dynablox?

I sincerely seek your guidance and hope to receive your kind advice.

[Schmluk]

Hi @9woods123

Thank you for your interest! Planning on dynablox should be very easy! You can either:

• directly use the TSDF layer to perform occupancy checks,
• Add an ESDF integrator and compute the esdf from the tsdf and plan on this (this is exactly the pipeline voxblox uses, c.f. here)

In addition, having the dynamic objects output by dynablox should boost your planner in dynamic scenes. Hope this helps!

[9woods123]

Thank you for your response！Your suggestion helped me better understand the ESDF server framework.

I am considering running the ESDF as a separate process for map building ( as the planning example for Voxblox mentioned here https://voxblox.readthedocs.io/en/latest/pages/Using-Voxblox-for-Planning.html), and at the same time, ensuring that the point clouds from dynamic obstacles are not used to build the voxel map. I have made some small modifications, which I would like to share with you.

  esdf_server_=std::make_shared<voxblox::EsdfServer>(nh_voxblox, nh_voxblox);
  esdf_server_->setTraversabilityRadius(3);
  esdf_server_->publishTraversable();
  
  // tsdf_server_ = esdf_server_.

  tsdf_layer_.reset(esdf_server_->getTsdfMapPtr()->getTsdfLayerPtr());

and

  sensor_msgs::PointCloud2::Ptr  static_object_pointcloud_msg=
  getStaticObjectPointcloud(msg,T_M_S,cloud,cloud_info);
  // The point cloud was obtained, and the points corresponding to dynamic obstacles were filtered out.
  // Only the  points corresponding to static obstacles are input into processPointCloudMessageAndInsert() 
  // for  building esdf_map.
  esdf_server_->processPointCloudMessageAndInsert(static_object_pointcloud_msg, T_G_C, false);
  esdf_server_->publishPointclouds();

I hope this helps with future work on online motion planning in dynamic environments. If needed, I would be happy to upload a PR and contribute to this great work.

[Schmluk]

Hi @9woods123

It should actually be much easier than this. The code you posted performs TSDF integration and motion detection in dynablox, and then takes the resulting static points to do TSDF and ESF integration again in voxblox. You should be able to directly take the TSDF from dynablox (which already integrates and updates dynamic points accordingly), and then perform ESDF integration only on that layer. Hope that helps!

If you want to follow the voxblox multi-node architecture you should also be able to publish the TSDF from dynablox, and subscribe to it and do esdf integration in a separate node.

Hope this helps!

[9woods123]

Thank you for your response! I have adjusted my approach based on your suggestions. However, I have encountered something that confuses me. When using the "fast" or "merged" TSDF integrator in Voxblox, I observed that dynamic objects are not effectively filtered out, as shown in the screenshot below:

The situation looks like

It appears that dynamic objects become integrated into the mesh structure. Conversely, the "projective" integrator seems to handle dynamic object filtering more effectively. I have reviewed the implementation of the "projective" integrator in voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h, but I couldn't ascertain how it achieves this dynamic object filtering.

Could you kindly direct me to any relevant papers, documentation, or specific sections of the Voxblox source code that explain how dynamic object filtering is achieved in the "projective" integrator? Your guidance would be greatly appreciated.

[Schmluk]

Hi @9woods123,

We've only updated the projective integrator to build the free-space map needed for motion detection. In general, we strongly recommend using only the projective integrator in general, as it is a strictly preferable option over the ray-casting-based ones (fast and merged).
I think the only difference required to run dynablox is that some small field (last observed or so) is set, so by adding this to the other integrators they could also work. The rest of the work is done by the tracking integrator in dynablox.

Hope this helps!