Convert nodes to lifecycle nodes #561
Conversation
…ith LifecycleNode
…ransform in loop_closure_assistant
There was a problem hiding this comment.
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
[0b8ad82]
I'm very sory to bother you. Please check when you have time. 🙏 (*)details
$ 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<float>::epsilon() * 2.0f*M_PI))) {
+ if (std::fabs(angular_range - 2.0 * M_PI) <
+ (scan_.angle_increment - (std::numeric_limits<float>::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 |
|
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. |
|
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 |
|
I've been using this one along with ROS 2 Components and it is working perfectly. Just some considerations:
|
|
Another consideration: When using the lifecycle implementation and starting rviz with the SlamToolbox plugin, it crashes. Specifically here: slam_toolbox/rviz_plugin/slam_toolbox_rviz_plugin.cpp Lines 557 to 561 in 03d1b49 As the nodes will start in the Unconfigured state the parameters are not yet declared and so the We might change to something like this: |
|
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 |
- Lifecycle support for 'use_map_server' parameter - updated to get namespace at initialization in map_server
There was a problem hiding this comment.
I like the idea of logging the status of the panel, anyway this commit won't fix the RViz crash. It is due to the get_parameters function being called with unexisting parameters
First it should be checked whether the parameters exists, and then call the get_parameters function.
|
@AntoBrandi This commit appears to fix the Rviz crash in my humble envirionment. get_parameters_with_timeout.mp4I also checked the method you suggested (has_parameter). This test diffdiff --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<rclcpp::SyncParametersClient>(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();
}
}has_parameters_without_timeout.mp4 |
|
On stack memory changes... $ git log -n1
commit 03d1b49655875f861a9c914930ea23f5213c1864 (HEAD, upstream/ros2)
Author: Antonio Brandi <[email protected]>
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. $ 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
... |
|
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 |
|
The stack memory change was easy to handle, so it was handled at the same time the conflicts were fixed. 👍 commit: [27ace6d]
$ 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. |
|
After testing it I can confirm that the latest changes did the trick! Good job 👍 |
|
All changes have been completed. Diffs : [319607f...21e9e6b]
Bond bits
Added $ 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"use_lifecycle_manager.mp4 |
|
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 |
There was a problem hiding this comment.
Overall, there are some utils in Nav2 that could have made this more ergonomic, but I don't see an issue as written. Has this been tested with Nav2's default bringup with slam:=True to trigger this launch file (and work fine)?
If so, I'd be ready to merge shortly
|
@SteveMacenski
I just tested it and confirmed it works fine. 👍 ( I had to pass $ ros2 launch nav2_bringup bringup_launch.py slam:=True use_sim_time:=True map:=dummy.yamllog : no map parameter$ 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 slam_toolbox_via_nav2_bringup.mp4 |
|
Thanks. As far as I know, everything else is fine. |
Basic Info
async_toolbox_lifecycle_x2.mp4
Description of contribution in a few bullet points
Updated some classes to initialize withnode_interfaceinstead of usingnode ptr. Now, those classes can be generated byrclcpp_lifecycle::LifecyclNodein the same way asrclcpp::Node.node templateinstead of usingnode ptr. ( Convert nodes to lifecycle nodes #561 (comment) )Description of documentation updates required from your changes
Future work that may be required in bullet points
I think this initialization of tf2_ros::TransformBroadcaster needs to be updated.Constructor of tf2_ros::TransformBroadcaster using node_interface is not yet implemented in humble. However, it is implemented in rolling. I think it needs to be updated when this implementation is reflected in humble (or other LTS?).