The Voxblox Node¶
Table of Contents¶
Published and Subscribed Topics¶
Note: the voxblox_node has been replaced with tsdf_server (if you want a TSDF) or esdf_server (if you want both a TSDF and an ESDF). The tsdf_server and esdf_server publish and subscribe to the following topics:
Published Topics¶
- mesh
voxblox_msgs::MeshBlock
- A visualization topic showing the mesh produced from the tsdf in a form that can be seen in RViz. Set
update_mesh_every_n_sec
to control its update rate. - surface_pointcloud
pcl::PointCloud<pcl::PointXYZRGB>
- A colored pointcloud of the voxels that are close to a surface.
- tsdf_pointcloud
pcl::PointCloud<pcl::PointXYZI>
- A pointcloud showing all allocated voxels.
- mesh_pointcloud
pcl::PointCloud<pcl::PointXYZRGB>
- Only appears if
output_mesh_as_pointcloud
is true, outputs a pointcloud containing the verticies of the generated mesh. - mesh_pcl
pcl_msgs::PolygonMesh
- Only appears if
output_mesh_as_pcl_mesh
is true, outputs any mesh generated by the generate_mesh service. - tsdf_slice
pcl::PointCloud<pcl::PointXYZI>
- Outputs a 2D horizontal slice of the TSDF colored by the stored distance value.
- esdf_pointcloud
pcl::PointCloud<pcl::PointXYZI>
- A pointcloud showing the values of all allocated ESDF voxels. Only appears if using
esdf_server
. - esdf_slice
pcl::PointCloud<pcl::PointXYZI>
- Outputs a 2D horizontal slice of the ESDF colored by the stored distance value. Only appears if using
esdf_server
. - occupied_nodes
visualization_msgs::MarkerArray
- Visualizes the location of the allocated voxels in the TSDF.
- tsdf_map_out
voxblox_msgs::Layer
- Publishes the entire TSDF layer to update other nodes (that listen on tsdf_layer_in). Only published if
publish_tsdf_map
is set to true. - esdf_map_out
voxblox_msgs::Layer
- Publishes the entire ESDF layer to update other nodes (that listen on esdf_layer_in). Only published if
publish_esdf_map
is set to true. - traversable
pcl::PointCloud<pcl::PointXYZI>
- (ESDF server only) Outputs all the points within the map that are considered traversable, controlled by the
publish_traversable
andtraversability_radius
parameters.
Subscribed Topics¶
- transform
geometry_msgs::TransformStamped
- Only appears if
use_tf_transforms
is false. The transformation from the world frame to the current sensor frame. - pointcloud
sensor_msgs::PointCloud2
- The input pointcloud to be integrated.
- freespace_pointcloud
sensor_msgs::PointCLoud2
- Only appears if
use_freespace_pointcloud
is true. Unlike thepointcloud
topic where the given points lie on surfaces, the points in thefreespace_pointcloud
are taken to be floating in empty space. These points can assist in generating more complete freespace information in a map. - tsdf_map_in
voxblox_msgs::Layer
- Replaces the current TSDF layer with that from this topic. Voxel size and voxels per side should match.
- esdf_map_in
voxblox_msgs::Layer
- Replaces the current ESDF layer with that from this topic. Voxel size and voxels per side should match.
- icp_transform
geometry_msgs::TransformStamped
- If ICP is enabled, this is the current corrected transform between the world frame and the ICP frame.
Services¶
The tsdf_server and esdf_server have the following services:
- generate_mesh
- This service has an empty request and response. Calling this service will generate a new mesh. The mesh will be saved as a ply file unless
mesh_filename
is set to “”. The mesh will also be output on themesh_pointcloud
topic ifoutput_mesh_as_pointcloud
is true and on themesh_pcl
topic ifoutput_mesh_as_pcl_mesh
is true. - generate_esdf
- This service has an empty request and response. It can be used to trigger an esdf map update.
- save_map
- This service has a
voxblox_msgs::FilePath::Request
andvoxblox_msgs::FilePath::Response
. The service call saves the tsdf layer to a .vxblx file. - load_map
- This service has a
voxblox_msgs::FilePath::Request
andvoxblox_msgs::FilePath::Response
. The service call loads the tsdf layer from a .vxblx file. - publish_map
- This service has an empty request and response. Publishes any TSDF and ESDF layers on the
tsdf_map_out
andesdf_map_out
topics. - publish_pointclouds
- This service has an empty request and response. Publishes TSDF and ESDF pointclouds and slices.
Parameters¶
A summary of the user setable tsdf_server and esdf_server parameters. All parameters are listed as:
Parameter
Default- Description.
General Parameters¶
min_time_between_msgs_sec
0.0- Minimum time to wait after integrating a message before accepting a new one.
pointcloud_queue_size
1- The size of the queue used to subscribe to pointclouds.
verbose
true- Prints additional debug and timing information.
max_block_distance_from_body
3.40282e+38- Blocks that are more than this distance from the latest robot pose are deleted, saving memory.
TSDF Integrator Parameters¶
method
“merged”
- “simple”
- The most straightfoward integrator. Every point in the pointcloud has a ray cast from the origin through it. Every voxel each ray passes through is updated individually. A very slow and exact approach.
- “merged”
- Rays that start and finish in the same voxel are bundled into a single ray. The properties of the points are merged and their weights added so no information is lost. The approximation means some voxels will recive updates that were otherwise meant for neighboring voxels. This approach works well with large voxels (10 cm or greater) and can give an order of magnitude speed up over the simple integrator.
- “fast”
- Rays that attempt to update voxels already updated by other rays from the same pointcloud are terminated early and discarded. An approximate method that has been designed to give the fastest possible results at the expense of discarding large quantities of information. The trade off between speed and information loss can be tuned via the
start_voxel_subsampling_factor
andmax_consecutive_ray_collisions
parameters. This method is currently the only viable integrator for real-time applications with voxels smaller than 5 cm.
tsdf_voxel_size
0.2- The size of the tsdf voxels
tsdf_voxels_per_side
16- TSDF voxels per side of an allocated block. Must be a power of 2
voxel_carving_enabled
true- If true, the entire length of a ray is integrated, if false only the region inside the trunaction distance is used.
truncation_distance
2*```tsdf_voxel_size`- The truncation distance for the TSDF
max_ray_length_m
5.0- The maximum range out to which a ray will be cast
min_ray_length_m
0.1- The point at which the ray casting will start
max_weight
10000.0- The upper limit for the weight assigned to a voxel
use_const_weight
false- If true all points along a ray have equal weighting
allow_clear
true- If true points beyond the
max_ray_length_m
will be integrated up to this distance use_freespace_pointcloud
false- If true a second subscription topic
freespace_pointcloud
appears. Clearing rays are cast from beyond this topic’s points’ truncation distance to assist in clearing freespace voxels
Fast TSDF Integrator Specific Parameters¶
These parameters are only used if the integrator method
is set to “fast”.
start_voxel_subsampling_factor
2- Before integration points are inserted into a sub-voxel, only one point is allowed per sub-voxel. This can be thought of as subsampling the pointcloud. The edge length of the sub-voxel is the voxel edge length divided by
start_voxel_subsampling_factor
. max_consecutive_ray_collisions
2- When a ray is cast by this integrator it detects if any other ray has already passed through the current voxel this scan. If it passes through more than
max_consecutive_ray_collisions
voxels other rays have seen in a row, it is taken to be adding no new information and the casting stops. max_integration_time_s
3.40282e+38- The time budget for frame integration, if this time is exceeded ray casting is stopped early. Used to guarantee real time performance.
clear_checks_every_n_frames
1- Governs how often the sets that indicate if a sub-voxel is full or a voxel has had a ray passed through it are cleared.
ESDF Integrator Parameters¶
esdf_max_distance_m
2.0- The maximum distance that the esdf will be calculated out to.
esdf_default_distance_m
2.0- Default distance set for unknown values and values >``esdf_max_distance_m``.
clear_sphere_for_planning
false- Enables setting unknown space to free near the current pose of the sensor, and unknown space to occupied further away from the sensor. Controlled by the two parameters below.
clear_sphere_radius
1.5- Radius of the inner sphere where unknown is set to free, in meters.
occupied_sphere_radius
5.0- Radius of the outer sphere where unknown is set to occupied, in meters.
ICP Refinement Parameters¶
ICP based refinement can be applied to the poses of the input pointclouds before merging.
enable_icp
false- Whether to use ICP to align all incoming pointclouds to the existing structure.
icp_refine_roll_pitch
true- True to apply 6-dof pose correction, false for 4-dof (x, y, z, yaw) correction.
accumulate_icp_corrections
true- Whether to accumulate transform corrections from ICP over all pointclouds. Reset at each new pointcloud if false.
icp_corrected_frame
icp_corrected- TF frame to output the ICP corrections to.
pose_corrected_frame
pose_corrected- TF frame used to output the ICP corrected poses relative to the
icp_corrected_frame
. icp_mini_batch_size
20- Number of points used in each batch of point matching corrections.
icp_subsample_keep_ratio
0.5- Random subsampling will be used to reduce the number of points used for matching.
icp_min_match_ratio
0.8- For a mini batch refinement to be accepted, at least this ratio of points in the pointcloud must fall within the truncation distance of the existing TSDF layer.
icp_inital_translation_weighting
100.0- A rough measure of the confidence the system has in the provided inital pose. Each point used in ICP contributes 1 point of weighting information to the translation.
icp_inital_rotation_weighting
100.0- A rough measure of the confidence the system has in the provided inital pose. Each point used in ICP contributes 2 points of weighting information to the rotation.
Input Transform Parameters¶
use_tf_transforms
true- If true the ros tf tree will be used to get the pose of the sensor relative to the world (
sensor_frame
andworld_frame
will be used). If false the pose must be given via thetransform
topic. world_frame
“world”- The base frame used when looking up tf transforms. This is also the frame that most outputs are given in.
sensor_frame
“”- The sensor frame used when looking up tf transforms. If set to “” the frame of the input pointcloud message will be used.
T_B_D
- A static transformation from the base to the dynamic system that will be applied.
invert_T_B_D
false- If the given
T_B_D
should be inverted before it is used. T_B_C
- A static transformation from the base to the sensor that will be applied.
invert_T_B_C
false- If the given
T_B_C
should be inverted before it is used.
Output Parameters¶
output_mesh_as_pointcloud
false- If true the verticies of the generated mesh will be ouput as a pointcloud on the topic
mesh_pointcloud
whenever the generate_mesh service is called. output_mesh_as_pcl_mesh
false- If true the generated mesh will be ouput as a
pcl::PolygonMesh
on the topicmesh_pcl
whenever the generate_mesh service is called. slice_level
0.5- The height at which generated tsdf and esdf slices will be made.
color_ptcloud_by_weight
false- If the pointcloud should be colored by the voxel weighting.
mesh_filename
“”- Filename output mesh will be saved to, leave blank if no file should be generated.
color_mode
“color”- The method that will be used for coloring the mesh. Options are “color”, “height”, “normals”, “lambert” and “gray”.
mesh_min_weight
1e-4- The minimum weighting needed for a point to be included in the mesh.
update_mesh_every_n_sec
0.0- Rate at which the mesh topic will be published to, a value of 0 disables. Note, this will not trigger any other mesh operations, such as generating a ply file.
publish_tsdf_map
false- Whether to publish the complete TSDF map periodically over ROS topics.
publish_esdf_map
false- Whether to publish the complete ESDF map periodically over ROS topics.
publish_tsdf_info
false- Enables publishing of
tsdf_pointcloud
,surface_pointcloud
andoccupied_nodes
.
publish_pointclouds
| If true the tsdf and esdf (if generated) is published as a pointcloud when the mesh is updated.
intensity_colormap
“rainbow”
If the incoming pointcloud is an intensity (not RGB) pointcloud, such as from laser, this sets how the intensities will be mapped to a color. Valid options arerainbow
,inverse_rainbow
,grayscale
,inverse_grayscale
,ironbow
(thermal).
intensity_max_value
100.0- Maximum value to use for the intensity mapping. Minimum value is always 0.
publish_traversable
false- Whether to display a traversability pointcloud from an ESDF server.
traversability_radius
1.0- The minimum radius at which a point is considered traversable.