STVL layer stops working in Rviz2 after initialization (ROS humble and Ubuntu 22.04)
anayghatpande opened this issue · comments
I have successfully setup the configuration and fixed all dependency installations.
I have followed all the process written in the GitHub page of STVL.
- I am using Realsense camera d435 in Gazebo along with gazebo plugin to get the simulated point cloud data.
- I tried all possible avaible solution within the repository.
- Double chekced with the TF tree, but topics and nodes are connected properly.
- Debugging main launch file along with spatio-temporal scripts is done. (It does not call the callback of point cloud after initialization)
but Still after initialization there will be voxel layer formation in Rviz2 but after navigation (nav2Goal) next voxel layer it can not form.
The configuration of the STVL is as follows mentioned on ROS2 documentation:
stvl_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
enabled: true
voxel_decay: 15.
decay_model: 0
voxel_size: 0.05
track_unknown_space: true
unknown_threshold: 15
mark_threshold: 0
update_footprint_enabled: true
combination_method: 1
origin_z: 0.0
publish_voxel_map: true
transform_tolerance: 0.1 #0.2
mapping_mode: false
map_save_duration: 60.0
observation_sources: pointcloud
pointcloud:
data_type: PointCloud2
topic: /camera_realsense/depth/points
marking: true
clearing: true
obstacle_range: 3.0
min_obstacle_height: 0.0
max_obstacle_height: 2.0
expected_update_rate: 0.0
observation_persistence: 0.0
inf_is_valid: false
voxel_filter: true
filter: "voxel"
clear_after_reading: true
max_z: 7.0
min_z: 0.1
vertical_fov_angle: 0.8745
horizontal_fov_angle: 1.048
decay_acceleration: 15.0
model_type: 0This is the launch file ros log for reference :
[gzserver-1] [INFO] [1674424328.415918603] [GazeboRealsenseNode]: Realsense Gazebo ROS plugin loading.
[gzserver-1]
[gzserver-1] RealSensePlugin: The realsense_camera plugin is attach to robot
[gzserver-1] [INFO] [1674424328.428908882] [GazeboRealsenseNode]: Loaded Realsense Gazebo ROS plugin.
[INFO] [spawn_entity.py-3]: process has finished cleanly [pid 1499851]
[controller_server-7] [INFO] [1674424328.662561241] [local_costmap.local_costmap]: start
[controller_server-7] [INFO] [1674424328.663355880] [local_costmap.local_costmap]: stvl_layer was activated.
[controller_server-7] [INFO] [1674424328.664297691] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-14] [INFO] [1674424328.766997422] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-14] [INFO] [1674424328.767040309] [lifecycle_manager_navigation]: Activating smoother_server
[smoother_server-8] [INFO] [1674424328.767281121] [smoother_server]: Activating
[smoother_server-8] [INFO] [1674424328.767317158] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
[lifecycle_manager-14] [INFO] [1674424328.869621673] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
[lifecycle_manager-14] [INFO] [1674424328.869672977] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-9] [INFO] [1674424328.869969560] [planner_server]: Activating
[planner_server-9] [INFO] [1674424328.870011180] [global_costmap.global_costmap]: Activating
[planner_server-9] [INFO] [1674424328.870025731] [global_costmap.global_costmap]: Checking transform
[planner_server-9] [INFO] [1674424328.870051501] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[planner_server-9] [INFO] [1674424329.370107136] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[planner_server-9] [INFO] [1674424329.637595765] [global_costmap.global_costmap]: Message Filter dropping message: frame 'camera_realsense_depth_optical_frame' at time 1361.060 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[planner_server-9] [INFO] [1674424329.870103794] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[laserscan_multi_merger-6] [INFO] [1674424329.974398840] [laserscan_multi_merger]: Subscribing to 2 topics.
[laserscan_multi_merger-6] [INFO] [1674424329.974876234] [laserscan_multi_merger]: Subscribed to /scan_back.
[laserscan_multi_merger-6] [INFO] [1674424329.975144966] [laserscan_multi_merger]: Subscribed to /scan_front.
[async_slam_toolbox_node-15] Registering sensor: [Custom Described Lidar]
[controller_server-7] [INFO] [1674424330.110978817] [local_costmap.local_costmap]: StaticLayer: Resizing static layer to 307 X 294 at 0.020000 m/pix
[planner_server-9] [INFO] [1674424330.111331769] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 307 X 294 at 0.020000 m/pix
[planner_server-9] [INFO] [1674424330.370206924] [global_costmap.global_costmap]: start
[planner_server-9] [INFO] [1674424330.371912989] [global_costmap.global_costmap]: stvl_layer was activated.
[controller_server-7] [INFO] [1674424325.172403329] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[controller_server-7] [INFO] [1674424325.174642399] [local_costmap.local_costmap]: Subscribed to Topics: scan_front scan_back
[controller_server-7] [INFO] [1674424325.187856584] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[controller_server-7] [INFO] [1674424325.189043188] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-7] [INFO] [1674424325.198567715] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-7] [INFO] [1674424325.198631151] [local_costmap.local_costmap]: Using plugin "stvl_layer"
[controller_server-7] [INFO] [1674424325.252194439] [local_costmap.local_costmap]: stvl_layer being initialized as SpatioTemporalVoxelLayer!
[controller_server-7] [INFO] [1674424325.252263428] [local_costmap.local_costmap]: stvl_layer's global frame is odom.
[controller_server-7] [INFO] [1674424325.254637741] [local_costmap.local_costmap]: stvl_layer loaded parameters from parameter server.
[controller_server-7] [INFO] [1674424325.259647989] [local_costmap.local_costmap]: stvl_layer created underlying voxel grid.
[controller_server-7] [INFO] [1674424325.262917096] [local_costmap.local_costmap]: Voxel filter activated.
[controller_server-7] [INFO] [1674424325.268782297] [local_costmap.local_costmap]: stvl_layer initialization complete!Please can somone suggest me in which direction I should debug this problem? or any possible suggestions where may be I am missing something in the configuration?
Thanks in advance.
Try cyclone dds
I am already using it. but I checked with environmental variables are set for cyclone dds properly and those are also fine. so no problem with that. :(
Just a doubt I am using the ubuntu 22.04 in docker container that should not be a problem right?
Doe the normal voxel layer work for you if used instead in your costmap configuration?
Now I tried the above given configuration on turtlebot3 navigation, but it seems there are no problems in setting the stvl layer configurations there... but I am still struggling on making it work on my custom robot as I have segregated the file from all parameters of nav2_params.yaml for different use cases. eg. amcl.yaml ,slam.yaml, etc. So Currently debugging the issue which parameter is affecting the actual working of stvl layer. If you suggest which crucial parameters may affect this (in general dependent)? I will update as soon as I can make it work.
I tried the normal voxel layer as well but it does not work as well in my case.
The only obvious things are the topic names that are aligned to the actual topics + that your custom robots TF tree has the necessary transformations available for STVL to transform the data in a global frame for buffering into the sparse voxel grid
Just for the update:
The issue was from gazebo plugin for Camera D435 for not able to publish voxel grids.
I am working on fixing the plugin.
Right now with real camera the STVL works but it reduces overall updating the point cloud publishing from the camera.
Because otherwise camera works fine without enabling STVL.
Is there any setting that can decrease the publishing rate of point clouds from STVL? Because without stvl layer it publishes above 10Hz and after initialization stvl it changes to 0.5Hz Due to which I am facing issue while updating local and global cost maps as, I am updating them at 10Hz frequency but the voxels are not formed on the same rate due to suppression of data rate from camera point cloud.
If there are externally adjustable parameters from ros2 params would be beneficial otherwise I will debug the scripts for this. Thanks in advance! @SteveMacenski