Controller server error with cycloneDDS with nav2 stack
charlielito opened this issue · comments
Hi!
I am trying to run the nav2 stack with the STVL in ros2 foxy. I was able to run everything smoothly with the default DDS.
Nonetheless, when using cycloneDDS, an error arises in the controller when initializing the local costmap plugins.
To reproduce it I just run:
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch nav2_bringup navigation_launch.py params_file:=nav2_params.yaml
The error looks like the following:
[controller_server-1] [INFO][1616544103.536367858][local_costmap.local_costmap]:
[controller_server-1] local_costmap lifecycle node launched.
[controller_server-1] Waiting on external lifecycle transitions to activate
[controller_server-1] See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-1] [INFO][1616544103.537550284][local_costmap.local_costmap]: Creating Costmap
[controller_server-1] [INFO][1616544103.545565721][controller_server]: Configuring controller interface
[controller_server-1] [INFO][1616544103.545893914][controller_server]: Controller frequency set to 20.0000Hz
[controller_server-1] [INFO][1616544103.545917231][local_costmap.local_costmap]: Configuring
[controller_server-1] [INFO][1616544103.548490687][local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-1] [INFO][1616544103.552205475][local_costmap.local_costmap]: Subscribed to Topics: scan semantic_scan
[controller_server-1] [INFO][1616544103.556169898][local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-1] [INFO][1616544103.556196861][local_costmap.local_costmap]: Using plugin "stvl_layer"
[controller_server-1] [INFO][1616544103.574677532][local_costmap.local_costmap]: stvl_layer being initialized as SpatioTemporalVoxelLayer!
[controller_server-1] [INFO][1616544103.574729328][local_costmap.local_costmap]: stvl_layer's global frame is odom.
[controller_server-1] [INFO][1616544103.575700693][local_costmap.local_costmap]: stvl_layer loaded parameters from parameter server.
[controller_server-1] [ERROR][1616544103.576399186][]: Caught exception in callback for transition 10
[controller_server-1] [ERROR][1616544103.576410856][]: Original error: could not create publisher: failed to create topic, at /tmp/binarydeb/ros-foxy-rmw-cyclonedds-cpp-0.7.6/src/rmw_node.cpp:1873, at /tmp/binarydeb/ros-foxy-rcl-1.1.10/src/rcl/publisher.c:180
[controller_server-1] [WARN][1616544103.576437685][]: Error occurred while doing error handling.
[controller_server-1] [FATAL][1616544103.576444115][controller_server]: Lifecycle node controller_server does not have error state implemented
[lifecycle_manager-6] [ERROR][1616544103.576903755][lifecycle_manager_navigation]: Failed to change state for node: controller_server
[lifecycle_manager-6] [ERROR][1616544103.576933129][lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.
The nav2_params.yaml local costmap:
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: chassis
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.5
plugins: ["voxel_layer", "stvl_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.4
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 4.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 4.0
clearing: True
marking: True
data_type: "LaserScan"
stvl_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" # For Galactic and later
enabled: true
voxel_decay: 15.
decay_model: 0
voxel_size: 0.05
track_unknown_space: true
max_obstacle_height: 2.0
unknown_threshold: 15
mark_threshold: 0
update_footprint_enabled: true
combination_method: 1
origin_z: 0.0
publish_voxel_map: true
transform_tolerance: 0.2
mapping_mode: false
map_save_duration: 60.0
observation_sources: pointcloud
pointcloud:
data_type: PointCloud2
topic: /camera/depth/color/points
marking: true
clearing: true
obstacle_range: 6.0
min_obstacle_height: 0.2
max_obstacle_height: 3.0
expected_update_rate: 0.0
observation_persistence: 0.0
inf_is_valid: false
voxel_filter: false
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 runs without errors if we omit the stvl_layer from plugins. This also runs well with the default DDS.
Is there any known issue with cycloneDDS?
Not sure, you'd have to debug more to see where the issue is coming from specifically, but I don't think there should be any issue with Cyclone vs FastDDS w.r.t. something this high level. Nothing immediately jumps out at me as being incorrect.
On my perception this error is caused by the implementation of the LifeCycle Nodes on the stack.
Caught exception in callback for transition 10
Apparently CycloneDDS is unable to handle this states and crash on try. This is just a guess, logs are not clear at all.
@SteveMacenski
That would not be a good assumption, you should dig into it more. There’s nothing cyclone related with lifecycle nodes, that’s 3 level of abstraction above it.
Did you look at a backtrace or resolve this issue?
Did you look at a backtrace or resolve this issue?
No, there is no backtrace I can look at (at least I am aware of) and the logs don't help. Note that this crashes only when using stvl on the local costmap. It does not crash when specified in the global costmap
Please obtain a backtrace.
Closing - apparently an issue with Cyclone but not w.r.t. STVL