Add service to reinitialize global localization

[olmerg]

Closes #141.

Summary

• Implement a service to reinitialize_global_localization, which restart the particle through a uniform distribution.
• create a parameter always_reset_initial_pose. control if the position is reset when the node enter on_configure.

To complete this functionality a variable initial_pose_is_known_ in amcl_node control when should be published the pose and the particles of the filter.

Checklist

• Read the contributing guidelines.
• Configured pre-commit and ran colcon test locally.
• Signed all commits for DCO.
• Added tests (regression tests for bugs, coverage of new code for features).
• Updated documentation (as needed).
• Checked that CI is passing.

[nahueespinosa]

@olmerg First pass. It looks like you haven't signed your commits, and you haven't run the linter or the tests. Please do it.

[nahueespinosa]

Please, update the PR title following the contributing guidelines:

• Limit the subject line to 50 characters.
• Capitalize the subject line.
• Do not end the subject line with a period.
• Use the imperative mood in the subject line.

[olmerg]

I make a new commit with some change in amcl_node but beluga system test is not working.

 [ERROR] [1680529048.150431725] [rosbag2_storage]: No storage id specified, and no plugin found that could open URI
    
    closing.
    
    closing.
    unknown file: Failure
    C++ exception with description "No storage could be initialized. Abort" thrown in the test body.

[ivanpauno]

I make a new commit with some change in amcl_node but beluga system test is not working.

Check #146 (comment) and #154

[olmerg]

@hidmic @nahueespinosa @glpuga

I am going to describe my blocking test:

• I want to test save state when the service deactivate whis is implemented in AmclNode::on_cleanup
• so I run
  ros2 launch beluga_example example_launch.py
• in other terminal I make

ros2 lifecycle set /amcl deactivate
ros2 lifecycle set /amcl configure
ros2 lifecycle set /amcl activate

When I deactivate the particle filter stop to work but the new state is uncofigured and not inactive. The on_cleanup is never called (which I am looking to test)
Now, I try configure and activate sometimes the PF start again but always break after sometime. This same error occur if I try put autostart in false in common_nodes_launch.py and try to start the nodes by commands.

Another unsucessful test is after configure test cleanup state, but the action call (ros2 lifecycle) never finish.

after review of the log amcl finish with -6 error (line 2903)

[amcl_node-5] terminate called after throwing an instance of 'statemap::TransitionUndefinedException'
[amcl_node-5]   what():  no such transition in current state
[ERROR] [amcl_node-5]: process has died [pid 6376, exit code -6, cmd '/ws/install/beluga_amcl/lib/beluga_amcl/amcl_node --ros-args --log-level debug --ros-args -r __node:=amcl -p use_sim_time:=True --params-file /ws/install/beluga_example/share/beluga_example/config/params.yaml'].

I attach the log, after remove some noise like flatland or subscription (it is incredible the logs generated in debug mode)
log_beluga.log

[nahueespinosa]

@olmerg The error might be related to ros2/rclcpp#1880. I'd say we don't need to implement the feature to save the last pose since nav2_amcl appears to have the same issue.

Implementing the service to reinitialize global localization would be enough for this PR. Please remove other changes.
Also note that we have discussed that this approach is not the correct way to do it during our weekly sync and here #157 (comment).

[hidmic]

@olmerg how far are we here? Need help?

[olmerg]

the functionality is working and I remove the part to save the state. to test the functionality

ros2 launch beluga_example example_launch.py

after some seconds in a new terminal

ros2 service call /reinitialize_global_localization std_srvs/srv/Empty

[nahueespinosa]

@olmerg Could you rebase to current main and sign off your commits? GitHub seems to be confused about what belongs to your branch and what comes from main.

[hidmic]

Ping @olmerg

[nahueespinosa]

@olmerg Looking good! Please fix the whitespaces.

[nahueespinosa]

LGTM!