search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.58k stars 1.3k forks source link

CostmapFilterInfoServer transition error for Re-Activation #3291

Closed MartiBolet closed 1 year ago

MartiBolet commented 1 year ago

When the CostmapFilerInfoServer is deactivated and then reactivated via lifecycle transitions makes node go to error state. I know where the error is so I will open a PR adressing this.

Bug report

Required Info:

Steps to reproduce issue

In 1 terminal execute CostmapFilterInfoServer:

ros2 run nav2_map_server costmap_filter_info_server

In other terminal change node state to Configure -> Active -> Inactive -> Active:

ros2 service call /costmap_filter_info_server/change_state lifecycle_msgs/srv/ChangeState "transition: {id: 1}" # Config
ros2 service call /costmap_filter_info_server/change_state lifecycle_msgs/srv/ChangeState "transition: {id: 3}" # Activate
ros2 service call /costmap_filter_info_server/change_state lifecycle_msgs/srv/ChangeState "transition: {id: 4}" # Deactivate
ros2 service call /costmap_filter_info_server/change_state lifecycle_msgs/srv/ChangeState "transition: {id: 3}" # Activate

Expected behavior

Have the /costmap_filter_info_server in active state

Actual behavior

Following error message:

:~$ ros2 run nav2_map_server costmap_filter_info_server 
[INFO] [1668772096.580942161] [costmap_filter_info_server]: This is costmap filter info publisher
[INFO] [1668772096.590525225] [costmap_filter_info_server]: 
    costmap_filter_info_server lifecycle node launched. 
    Waiting on external lifecycle transitions to activate
    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1668772098.766254821] [costmap_filter_info_server]: Configuring
[INFO] [1668772102.599998211] [costmap_filter_info_server]: Activating
[INFO] [1668772102.600045231] [costmap_filter_info_server]: Creating bond (costmap_filter_info_server) to lifecycle manager.
[INFO] [1668772135.501066577] [costmap_filter_info_server]: Deactivating
[INFO] [1668772135.501103582] [costmap_filter_info_server]: Destroying bond (costmap_filter_info_server) to lifecycle manager.
[INFO] [1668772137.492469271] [costmap_filter_info_server]: Activating

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'timer is canceled, at ./src/rcl/timer.c:249'

with this new error message:

  'ros_message argument is null, at ./src/rcl/publisher.c:247'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
[ERROR] [1668772137.493031231] []: Caught exception in callback for transition 13
[ERROR] [1668772137.493044688] []: Original error: failed to publish message: ros_message argument is null, at ./src/rcl/publisher.c:247
[WARN] [1668772137.493067716] []: Error occurred while doing error handling.
[FATAL] [1668772137.493076499] [costmap_filter_info_server]: Lifecycle node costmap_filter_info_server does not have error state implemented

Additional information

Feature request

Feature description

Implementation considerations

AlexeyMerzlyakov commented 1 year ago

@MartiBolet, thank you for localizing an issue and came with a working solution. I've modified the testcase slightly to cover the case that re-publishing goes well, and then PR was merged.