search

SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.68k stars 525 forks source link

Convert nodes to lifecycle nodes #561

Closed kei1107 closed 10 months ago

kei1107 commented 1 year ago

Basic Info

Info Please fill out this column
Ticket(s) this addresses #244
Primary OS tested on Ubuntu22 (docker)
Robotic platform tested on Turtlebot3 gazebo simulation

https://user-images.githubusercontent.com/32297845/207318790-aa181d4d-66dd-4256-8e44-acfbc30c0798.mp4

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

kei1107 commented 1 year ago

Hi, a decent first stab.

Lots of thematic cleanup needed before I can more deeply dive into the Common changes made. In general:

  • Make sure to lint
  • Remove all duplicated functions who's role is only to pass particula things of a node to another version of the function, just use the NodeT
  • Use shared pointer of the NodeT, not passing by double references
  • Get parameters using the NodeT, not the interfaces

That should clean up about ~60% of the changes here and make this PR much more easily reviewable

[0b8ad824d5e544425bc08bfc629a213b032ca9d0]

I'm very sory to bother you. Please check when you have time. :pray:

(*)details - This also occurs in other files. ```shell $ ament_uncrustify src/laser_utils.cpp Code style divergence in file 'src/laser_utils.cpp': --- src/laser_utils.cpp +++ src/laser_utils.cpp.uncrustify @@ -101,2 +101,4 @@ - laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, - laser_pose_.transform.translation.y, mountingYaw)); + laser->SetOffsetPose( + karto::Pose2( + laser_pose_.transform.translation.x, + laser_pose_.transform.translation.y, mountingYaw)); @@ -111 +113,3 @@ - if (std::fabs(angular_range - 2.0 * M_PI) < (scan_.angle_increment - (std::numeric_limits::epsilon() * 2.0f*M_PI))) { + if (std::fabs(angular_range - 2.0 * M_PI) < + (scan_.angle_increment - (std::numeric_limits::epsilon() * 2.0f * M_PI))) + { @@ -117 +121,3 @@ - if (angular_range > 6.10865 /*350 deg*/ && std::round(angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) { + if (angular_range > 6.10865 /*350 deg*/ && + std::round(angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) + { 1 files with code style divergence ```
kei1107 commented 1 year ago

Please check when you have time.

Edit : fixed variable name ( [7b213e81ab1a36c1cc05dfc731ec563463be6f60] )

SteveMacenski commented 1 year ago

I'll look in more detail next week, but I did another quick pass over it and nothing else obvious come to me. So I'll drill into the specifics and make sure its all A-OK but seems plausible now!

kei1107 commented 1 year ago

https://github.com/SteveMacenski/slam_toolbox/pull/561#discussion_r1091041729 - https://github.com/SteveMacenski/slam_toolbox/pull/561#discussion_r1092514569

rscova commented 1 year ago

I have tested it with ROS2 Humble for several weeks and works fine. I know other people who have been using it in its projects for some months without any problem. It's fully compatible with nav2 as the standard slam_toolbox. I haven't found any bug or reason to not accept the pull request.

SteveMacenski commented 1 year ago

As noted, needs a second detailed technical review to make sure there aren't any subtle issues. At this point, also needs to resolve a merge conflict.

I'll post on Nav2 Slack to see if someone can provide a review

AntoBrandi commented 1 year ago

I've been using this one along with ROS 2 Components and it is working perfectly.

Just some considerations:

AntoBrandi commented 1 year ago

Another consideration:

When using the lifecycle implementation and starting rviz with the SlamToolbox plugin, it crashes. Specifically here: https://github.com/SteveMacenski/slam_toolbox/blob/03d1b49655875f861a9c914930ea23f5213c1864/rviz_plugin/slam_toolbox_rviz_plugin.cpp#L557-L561

As the nodes will start in the Unconfigured state the parameters are not yet declared and so the get_parameters fails and Rviz crashes

We might change to something like this:

while (rclcpp::ok()) {
    if(parameters_client->has_parameter("paused_new_measurements") && 
       parameters_client->has_parameter("interactive_mode"))
    {
      auto parameters = parameters_client->get_parameters(
        {"paused_new_measurements", "interactive_mode"});
      paused_measure = parameters[0].as_bool();
      interactive = parameters[1].as_bool();

      bool oldState = _check1->blockSignals(true);
      _check1->setChecked(interactive);
      _check1->blockSignals(oldState);

      oldState = _check2->blockSignals(true);
      _check2->setChecked(!paused_measure);
      _check2->blockSignals(oldState);
    }

    r.sleep();
  }
SteveMacenski commented 12 months ago

Those are all good points.

On the bond bits: The lifecycle manager does not require bond connections, there's a parameter for it. With that said, if you want to use the same manager instance as the rest of Nav2, then it does. Otherwise, you can have a second instance of it for just localization nodes which don't use it and that works fine. But having that options seems great.

On stack memory changes - good catch. I suppose we can move that into the node itself, I didn't test that but I don't see why it wouldn't work.

On rviz: Makes sense. I'd add a log there so people knew what was going on during that period, but a good solution

kei1107 commented 12 months ago

@AntoBrandi I had completely missed the bug about the rviz_plugin. Thanks for the info!

This commit appears to fix the Rviz crash in my humble envirionment. I checked SyncParametersClient::get_parameters and was able to set a timeout, so my code specifies a timeout of 1s. That way, when slam_toolbox is not activated, an empty vector is returned after 1s. ( See video get_parameters_with_timeout.mp4 )

https://github.com/SteveMacenski/slam_toolbox/assets/32297845/8ef12d10-75cb-4d37-a78d-061528dbdb18

I also checked the method you suggested (has_parameter). Looking at the implementation of SyncParametersClient::has_parameter, SyncParametersClient::list_parameter is executed without timeout. This caused the has_parameter to wait forever without completing. (See video has_parameter_without_timeout )

This test diff ```diff diff --git a/rviz_plugin/slam_toolbox_rviz_plugin.cpp b/rviz_plugin/slam_toolbox_rviz_plugin.cpp index d04d0b3..ee7bc54 100644 --- a/rviz_plugin/slam_toolbox_rviz_plugin.cpp +++ b/rviz_plugin/slam_toolbox_rviz_plugin.cpp @@ -555,17 +555,14 @@ void SlamToolboxPlugin::updateCheckStateIfExternalChange() std::make_shared(node, "slam_toolbox"); while (rclcpp::ok()) { - auto parameters = parameters_client->get_parameters( - {"paused_new_measurements", "interactive_mode"}, std::chrono::seconds(1)); - if (parameters.empty()) { - RCLCPP_INFO_ONCE( - ros_node_->get_logger(), - "Waiting for the slam_toolbox node configuration."); - } else { + if(parameters_client->has_parameter("paused_new_measurements") && + parameters_client->has_parameter("interactive_mode")) + { RCLCPP_INFO_ONCE( ros_node_->get_logger(), "Start the slam_toolbox node state check."); - + auto parameters = parameters_client->get_parameters( + {"paused_new_measurements", "interactive_mode"}); paused_measure = parameters[0].as_bool(); interactive = parameters[1].as_bool(); @@ -576,7 +573,12 @@ void SlamToolboxPlugin::updateCheckStateIfExternalChange() oldState = _check2->blockSignals(true); _check2->setChecked(!paused_measure); _check2->blockSignals(oldState); + } else { + RCLCPP_INFO_ONCE( + ros_node_->get_logger(), + "Waiting for the slam_toolbox node configuration."); } + r.sleep(); } } ```

https://github.com/SteveMacenski/slam_toolbox/assets/32297845/7f70518f-05cd-434f-b304-f21f08f58ecd

kei1107 commented 12 months ago

On stack memory changes... Even in the latest original ros2 branch, component nodes are not started with slam_toolbox_XXX_node.cpp, so not only stack memory changes but also configure and loadPoseGraphByParams are not executed. Therefore, slam_toolbox will not work. This PR can handle stack memory change, but it might be better to fix it in another PR.

$ git log -n1
commit 03d1b49655875f861a9c914930ea23f5213c1864 (HEAD, upstream/ros2)
Author: Antonio Brandi <34162460+AntoBrandi@users.noreply.github.com>
Date:   Tue Nov 21 17:16:44 2023 +0100

    added support for ROS 2 Components (#652)
$ git remote show -n upstream | grep URL
  Fetch URL: https://github.com/SteveMacenski/slam_toolbox
  Push  URL: https://github.com/SteveMacenski/slam_toolbox
$ ros2 component standalone slam_toolbox slam_toolbox::SynchronousSlamToolbox 
[INFO] [1701489462.818720130] [standalone_container_ba793c8c6c7a]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/libsync_slam_toolbox.so
[INFO] [1701489462.831510908] [standalone_container_ba793c8c6c7a]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
[INFO] [1701489462.831530769] [standalone_container_ba793c8c6c7a]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
$ ros2 node info /slam_toolbox 
/slam_toolbox
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
  Service Servers:
    /slam_toolbox/clear_queue: slam_toolbox/srv/ClearQueue
    /slam_toolbox/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /slam_toolbox/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /slam_toolbox/get_parameters: rcl_interfaces/srv/GetParameters
    /slam_toolbox/list_parameters: rcl_interfaces/srv/ListParameters
    /slam_toolbox/set_parameters: rcl_interfaces/srv/SetParameters
    /slam_toolbox/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

In lifecycle nodes, the stack memory changes will work if not via components.
( Node using stack size... log is output )

$ ros2 launch slam_toolbox online_sync_launch.py 
...
[sync_slam_toolbox_node-1] [INFO] [1701493561.608445099] [slam_toolbox]: Node using stack size 40000000
[sync_slam_toolbox_node-1] [INFO] [1701493561.839430087] [slam_toolbox]: Configuring
[sync_slam_toolbox_node-1] [INFO] [1701493561.853614399] [slam_toolbox]: Using solver plugin solver_plugins::CeresSolver
[sync_slam_toolbox_node-1] [INFO] [1701493561.853864764] [slam_toolbox]: CeresSolver: Using SCHUR_JACOBI preconditioner.
[INFO] [launch.user]: [LifecycleLaunch] Slamtoolbox node is activating.
[sync_slam_toolbox_node-1] [INFO] [1701493561.862764516] [slam_toolbox]: Activating
...
SteveMacenski commented 11 months ago

It looks like you may have just done that anyway?

Its been a hot minute since I looked at this in detail and given several of you have commented that you've been using this, I'm more open this week / next to look at this is critical detail to get this merged in before the end of the year. A couple of pre-questions:

@kei1107 are you done with changes and/or is this ready for final review? Did you handle all of the component issues here as well fully (and tested working)? How did you test the stack expansion?

Sorry for the delay here, sometimes certain things slip through the cracks and this certainly was one of those things until now

kei1107 commented 11 months ago

@SteveMacenski

The stack memory change was easy to handle, so it was handled at the same time the conflicts were fixed. 👍

commit: [27ace6d368b76be056af7b25f102649016d0664a]

$ ros2 component standalone slam_toolbox slam_toolbox::SynchronousSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701826099.450789437] [standalone_container_d4684c27f8dc]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/libsync_slam_toolbox.so
[INFO] [1701826099.460284390] [standalone_container_d4684c27f8dc]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
[INFO] [1701826099.460304346] [standalone_container_d4684c27f8dc]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
[INFO] [1701826099.464084151] [slam_toolbox]: Node using stack size 12345678
^C[INFO] [1701826102.484434728] [rclcpp]: signal_handler(signum=2)
$ ros2 component standalone slam_toolbox slam_toolbox::AsynchronousSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701826108.762787636] [standalone_container_15cede2b20f1]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/libasync_slam_toolbox.so
[INFO] [1701826108.771860425] [standalone_container_15cede2b20f1]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::AsynchronousSlamToolbox>
[INFO] [1701826108.771882183] [standalone_container_15cede2b20f1]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::AsynchronousSlamToolbox>
[INFO] [1701826108.776211866] [slam_toolbox]: Node using stack size 12345678
^C[INFO] [1701826109.720144695] [rclcpp]: signal_handler(signum=2)
$ ros2 component standalone slam_toolbox slam_toolbox::LocalizationSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701826126.962188975] [standalone_container_239ae5d6a0fb]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/liblocalization_slam_toolbox.so
[INFO] [1701826126.972350827] [standalone_container_239ae5d6a0fb]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::LocalizationSlamToolbox>
[INFO] [1701826126.972386504] [standalone_container_239ae5d6a0fb]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::LocalizationSlamToolbox>
[INFO] [1701826126.976728370] [slam_toolbox]: Node using stack size 12345678
^C[INFO] [1701826128.333306099] [rclcpp]: signal_handler(signum=2)
$ ros2 component standalone slam_toolbox slam_toolbox::LifelongSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701826141.685357673] [standalone_container_1bacce7fafa6]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/liblifelong_slam_toolbox.so
[INFO] [1701826141.694593315] [standalone_container_1bacce7fafa6]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::LifelongSlamToolbox>
[INFO] [1701826141.694612232] [standalone_container_1bacce7fafa6]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::LifelongSlamToolbox>
[INFO] [1701826141.699040444] [slam_toolbox]: Node using stack size 12345678
[WARN] [1701826141.699062907] [slam_toolbox]: Lifelong mapping mode in SLAM Toolbox is considered experimental and should be understood before proceeding. Please visit: https://github.com/SteveMacenski/slam_toolbox/wiki/Experimental-Lifelong-Mapping-Node for more information.
^C[INFO] [1701826143.986663969] [rclcpp]: signal_handler(signum=2)
$ ros2 component standalone slam_toolbox slam_toolbox::MapAndLocalizationSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701826161.831423628] [standalone_container_283a50e8a3cd]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/libmap_and_localization_slam_toolbox.so
[INFO] [1701826161.846489319] [standalone_container_283a50e8a3cd]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::LocalizationSlamToolbox>
[INFO] [1701826161.846515435] [standalone_container_283a50e8a3cd]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::MapAndLocalizationSlamToolbox>
[INFO] [1701826161.846520753] [standalone_container_283a50e8a3cd]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::MapAndLocalizationSlamToolbox>
[INFO] [1701826161.850950065] [slam_toolbox]: Node using stack size 12345678
^C[INFO] [1701826164.780755563] [rclcpp]: signal_handler(signum=2)

Edit: Additional Test:

#terminal1
$ ros2 component standalone slam_toolbox slam_toolbox::SynchronousSlamToolbox --parameter stack_size_to_use:=12345678
[INFO] [1701861313.941864284] [standalone_container_0d29b2901c04]: Load Library: /home/ubuntu/dev_ws/install/slam_toolbox/lib/libsync_slam_toolbox.so
[INFO] [1701861313.951494930] [standalone_container_0d29b2901c04]: Found class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
[INFO] [1701861313.951518738] [standalone_container_0d29b2901c04]: Instantiate class: rclcpp_components::NodeFactoryTemplate<slam_toolbox::SynchronousSlamToolbox>
[INFO] [1701861313.954739030] [slam_toolbox]: Node using stack size 12345678

#terminal2 
$ ps -ef | grep component_container
ubuntu       969     968  0 11:15 pts/0    00:00:00 /opt/ros/humble/lib/rclcpp_components/component_container --ros-args -r __node:=standalone_container_0d29b2901c04
ubuntu      1001     726  0 11:15 pts/2    00:00:00 grep --color=auto component_container
$ cat /proc/969/limits 
Limit                     Soft Limit           Hard Limit           Units     
Max cpu time              unlimited            unlimited            seconds   
Max file size             unlimited            unlimited            bytes     
Max data size             unlimited            unlimited            bytes     
Max stack size            12345678             unlimited            bytes     
Max core file size        unlimited            unlimited            bytes     
Max resident set          unlimited            unlimited            bytes     
Max processes             unlimited            unlimited            processes 
Max open files            1048576              1048576              files     
Max locked memory         8388608              8388608              bytes     
Max address space         unlimited            unlimited            bytes     
Max file locks            unlimited            unlimited            locks     
Max pending signals       62766                62766                signals   
Max msgqueue size         819200               819200               bytes     
Max nice priority         0                    0                    
Max realtime priority     0                    0                    
Max realtime timeout      unlimited            unlimited            us

There is no other work remaining.

AntoBrandi commented 11 months ago

After testing it I can confirm that the latest changes did the trick! Good job 👍

kei1107 commented 11 months ago

@SteveMacenski

All changes have been completed.

Diffs : [319607f4d63dd91ab5c63c06749df9a5909484de...21e9e6b823d4a73471a6fc6d5f9b2a773b06ed2a]

Bond bits

Added use_lifecycle_manager node parameter and launch parameter.
When the launch parameter of use_lifecycle_manager is true, the event handlers for lifecycle transition in launch are disabled. Also, the parameter is passed in slam_toolbox to create the bond connection on activation.

$ ros2 launch slam_toolbox online_sync_launch.py -s
Arguments (pass arguments as '<name>:=<value>'):

    'autostart':
        Automatically startup the slamtoolbox. Ignored when use_lifecycle_manager is true.
        (default: 'true')

    'use_lifecycle_manager':
        Enable bond connection duaring node activation
        (default: 'false')

    'use_sim_time':
        Use simulation/Gazebo clock
        (default: 'true')

    'slam_params_file':
        Full path to the ROS2 parameters file to use for the slam_toolbox node
        (default: '/home/ubuntu/dev_ws/install/slam_toolbox/share/slam_toolbox/config/mapper_params_online_sync.yaml')

Test

#Enable bond connection, Disable the event handlers of lifecycle transition.
$ ros2 launch slam_toolbox online_sync_launch.py use_lifecycle_manager:=true
#Start lifecycle manager
$ ros2 run nav2_lifecycle_manager lifecycle_manager --ros-args -p node_names:=["slam_toolbox"] -p autostart:=true

#Startup
$ ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "command: 0"
#Reset
$ ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "command: 3"

https://github.com/SteveMacenski/slam_toolbox/assets/32297845/6de71e1e-a07f-42eb-83d9-930d9841d3b5

SteveMacenski commented 11 months ago

OK great. I will try very hard to get this a review next week, but it might happen over the holidays. I'm going to commit to getting this in before EOY

kei1107 commented 10 months ago

@SteveMacenski Thanks for your review! 😄

Has this been tested with Nav2's default bringup with slam:=True to trigger this launch file (and work fine)?

I just tested it and confirmed it works fine. 👍 ( I had to pass map parameter )

$ ros2 launch nav2_bringup bringup_launch.py slam:=True use_sim_time:=True map:=dummy.yaml
log : no map parameter ```console $ ros2 launch nav2_bringup bringup_launch.py slam:=True use_sim_time:=True [INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2024-01-04-02-57-55-395185-c6ea82625ecd-10363 [INFO] [launch]: Default logging verbosity is set to INFO [ERROR] [launch]: Caught exception in launch (see debug for traceback): Included launch description missing required argument 'map' (description: 'Full path to map yaml file to load'), given: [slam, use_sim_time] ```

※ 2x speed

https://github.com/SteveMacenski/slam_toolbox/assets/32297845/12f70f70-70a7-445a-ad6c-247ef526f710

kei1107 commented 10 months ago

Thanks. As far as I know, everything else is fine.