Convert nodes to lifecycle nodes

CMakeLists.txt

@@ -25,6 +25,8 @@ find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(rviz_common REQUIRED)
 find_package(rviz_default_plugins REQUIRED)
 find_package(rviz_ogre_vendor REQUIRED)
@@ -52,6 +54,8 @@ set(dependencies
   std_msgs
   std_srvs
   builtin_interfaces
+  rclcpp_lifecycle
+  lifecycle_msgs
   rviz_common
   rviz_default_plugins
   rviz_ogre_vendor
@@ -123,7 +127,7 @@ rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typ
 target_link_libraries(SlamToolboxPlugin "${cpp_typesupport_target}")
 target_compile_definitions(SlamToolboxPlugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 target_compile_definitions(SlamToolboxPlugin PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
-pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) 
+pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml)
 
 #### Ceres Plugin
 add_library(ceres_solver_plugin solvers/ceres_solver.cpp)
@@ -190,6 +194,7 @@ install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node
                 localization_slam_toolbox_node
                 map_and_localization_slam_toolbox_node
+                # lifelong_slam_toolbox_node
                 merge_maps_kinematic
                 ${libraries}
   ARCHIVE DESTINATION lib

include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp

@@ -30,6 +30,8 @@ class LifelongSlamToolbox : public SlamToolbox
 public:
   explicit LifelongSlamToolbox(rclcpp::NodeOptions options);
   ~LifelongSlamToolbox() {}
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
 
   // computation metrics
   double computeObjectiveScore(

include/slam_toolbox/experimental/slam_toolbox_map_and_localization.hpp

@@ -31,6 +31,8 @@ class MapAndLocalizationSlamToolbox : public LocalizationSlamToolbox
   virtual ~MapAndLocalizationSlamToolbox() {}
   void loadPoseGraphByParams() override;
   void configure() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
 
 protected:
   void laserCallback(

include/slam_toolbox/laser_utils.hpp

@@ -25,6 +25,7 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "slam_toolbox/toolbox_types.hpp"
 #include "tf2/utils.h"
 
@@ -75,8 +76,9 @@ class LaserMetadata
 class LaserAssistant
 {
 public:
+  template<class NodeT>
   LaserAssistant(
-    rclcpp::Node::SharedPtr node, tf2_ros::Buffer * tf,
+    std::shared_ptr<NodeT> node, tf2_ros::Buffer * tf,
     const std::string & base_frame);
   ~LaserAssistant();
   LaserMetadata toLaserMetadata(sensor_msgs::msg::LaserScan scan);
@@ -85,7 +87,8 @@ class LaserAssistant
   karto::LaserRangeFinder * makeLaser(const double & mountingYaw);
   bool isInverted(double & mountingYaw);
 
-  rclcpp::Node::SharedPtr node_;
+  rclcpp::Logger logger_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_;
   tf2_ros::Buffer * tf_;
   sensor_msgs::msg::LaserScan scan_;
   std::string frame_, base_frame_;

include/slam_toolbox/loop_closure_assistant.hpp

@@ -29,6 +29,7 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/utils.h"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "interactive_markers/interactive_marker_server.hpp"
 #include "interactive_markers/menu_handler.hpp"
 
@@ -44,8 +45,9 @@ using namespace ::toolbox_types;  // NOLINT
 class LoopClosureAssistant
 {
 public:
+  template<class NodeT>
   LoopClosureAssistant(
-    rclcpp::Node::SharedPtr node, karto::Mapper * mapper,
+    std::shared_ptr<NodeT> node, karto::Mapper * mapper,
     laser_utils::ScanHolder * scan_holder, PausedState & state,
     ProcessType & processor_type);
 
@@ -54,15 +56,18 @@ class LoopClosureAssistant
     const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback);
   void publishGraph();
   void setMapper(karto::Mapper * mapper);
+  void on_activate();
+  void on_deactivate();
+  bool is_activated();
 
 private:
   bool manualLoopClosureCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<slam_toolbox::srv::LoopClosure::Request> req, 
+    const std::shared_ptr<slam_toolbox::srv::LoopClosure::Request> req,
     std::shared_ptr<slam_toolbox::srv::LoopClosure::Response> resp);
   bool clearChangesCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<slam_toolbox::srv::Clear::Request> req, 
+    const std::shared_ptr<slam_toolbox::srv::Clear::Request> req,
     std::shared_ptr<slam_toolbox::srv::Clear::Response> resp);
   bool interactiveModeCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
@@ -86,10 +91,14 @@ class LoopClosureAssistant
   std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;
   boost::mutex interactive_mutex_;
   bool interactive_mode_, enable_interactive_mode_;
-  rclcpp::Node::SharedPtr node_;
   std::string map_frame_;
   PausedState & state_;
   ProcessType & processor_type_;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_;
+  bool is_activated_;
 };
 
 }   // namespace loop_closure_assistant

include/slam_toolbox/map_saver.hpp

@@ -23,6 +23,7 @@
 #include <memory>
 #include <chrono>
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "slam_toolbox/toolbox_msgs.hpp"
 
 namespace map_saver
@@ -32,7 +33,8 @@ namespace map_saver
 class MapSaver
 {
 public:
-  MapSaver(rclcpp::Node::SharedPtr node, const std::string & service_name);
+  template<class NodeT>
+  MapSaver(std::shared_ptr<NodeT> node, const std::string & map_name);
 
 protected:
   bool saveMapCallback(
@@ -41,7 +43,7 @@ class MapSaver
     std::shared_ptr<slam_toolbox::srv::SaveMap::Response> response);
 
 private:
-  rclcpp::Node::SharedPtr node_;
+  rclcpp::Logger logger_;
   rclcpp::Service<slam_toolbox::srv::SaveMap>::SharedPtr server_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_;
   std::string service_name_, map_name_;

include/slam_toolbox/serialization.hpp

@@ -22,7 +22,9 @@
 #include <sys/stat.h>
 #include <vector>
 #include <string>
+#include <memory>
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "karto_sdk/Mapper.h"
 
 namespace serialization
@@ -34,11 +36,12 @@ inline bool fileExists(const std::string & name)
   return stat(name.c_str(), &buffer) == 0;
 }
 
+template<class NodeT>
 inline bool write(
   const std::string & filename,
   karto::Mapper & mapper,
   karto::Dataset & dataset,
-  rclcpp::Node::SharedPtr node)
+  std::shared_ptr<NodeT> node)
 {
   try {
     mapper.SaveToFile(filename + std::string(".posegraph"));
@@ -51,11 +54,12 @@ inline bool write(
   }
 }
 
+template<class NodeT>
 inline bool read(
   const std::string & filename,
   karto::Mapper & mapper,
   karto::Dataset & dataset,
-  rclcpp::Node::SharedPtr node)
+  std::shared_ptr<NodeT> node)
 {
   if (!fileExists(filename + std::string(".posegraph"))) {
     RCLCPP_ERROR(node->get_logger(),

include/slam_toolbox/slam_mapper.hpp

@@ -22,6 +22,7 @@
 #include <memory>
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/utils.h"
 #include "slam_toolbox/toolbox_types.hpp"
@@ -46,7 +47,9 @@ class SMapper
   // convert TF pose to karto pose
   karto::Pose2 toKartoPose(const tf2::Transform & pose) const;
 
-  void configure(const rclcpp::Node::SharedPtr & node);
+  template<class NodeT>
+  void configure(const std::shared_ptr<NodeT> & node);
+
   void Reset();
 
   // // processors

include/slam_toolbox/slam_toolbox_async.hpp

@@ -31,6 +31,7 @@ class AsynchronousSlamToolbox : public SlamToolbox
 public:
   explicit AsynchronousSlamToolbox(rclcpp::NodeOptions options);
   ~AsynchronousSlamToolbox() {}
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
 
 protected:
   void laserCallback(

include/slam_toolbox/slam_toolbox_common.hpp

@@ -29,7 +29,10 @@
 #include <memory>
 #include <fstream>
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "message_filters/subscriber.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
@@ -56,7 +59,7 @@ namespace slam_toolbox
 using namespace ::toolbox_types;  // NOLINT
 using namespace ::karto;  // NOLINT
 
-class SlamToolbox : public rclcpp::Node
+class SlamToolbox : public rclcpp_lifecycle::LifecycleNode
 {
 public:
   explicit SlamToolbox(rclcpp::NodeOptions);
@@ -65,6 +68,12 @@ class SlamToolbox : public rclcpp::Node
   virtual void configure();
   virtual void loadPoseGraphByParams();
 
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
 protected:
   // threads
   void publishVisualizations();
@@ -130,11 +139,13 @@ class SlamToolbox : public rclcpp::Node
   std::unique_ptr<tf2_ros::Buffer> tf_;
   std::unique_ptr<tf2_ros::TransformListener> tfL_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
-  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> scan_filter_sub_;
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> scan_filter_sub_;
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> scan_filter_;
-  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
-  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
-  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>> sst_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::MapMetaData>> sstm_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<
+      geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
   std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::SerializePoseGraph>> ssSerialize_;
@@ -172,6 +183,7 @@ class SlamToolbox : public rclcpp::Node
   ProcessType processor_type_;
   std::unique_ptr<karto::Pose2> process_near_pose_;
   tf2::Transform reprocessing_transform_;
+  bool do_threads_clean_up_;
 
   // pluginlib
   pluginlib::ClassLoader<karto::ScanSolver> solver_loader_;

include/slam_toolbox/slam_toolbox_localization.hpp

@@ -32,6 +32,8 @@ class LocalizationSlamToolbox : public SlamToolbox
   explicit LocalizationSlamToolbox(rclcpp::NodeOptions options);
   virtual ~LocalizationSlamToolbox() {}
   virtual void loadPoseGraphByParams();
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
 
 protected:
   virtual void laserCallback(

include/slam_toolbox/slam_toolbox_sync.hpp

@@ -33,6 +33,8 @@ class SynchronousSlamToolbox : public SlamToolbox
   explicit SynchronousSlamToolbox(rclcpp::NodeOptions options);
   ~SynchronousSlamToolbox() {}
   void run();
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
 
 protected:
   void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override;

include/slam_toolbox/toolbox_types.hpp

@@ -23,6 +23,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "tf2_ros/buffer.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/transform_datatypes.h"
@@ -124,6 +125,8 @@ typedef slam_toolbox::srv::DeserializePoseGraph::Request procType;
 typedef std::unordered_map<int, Eigen::Vector3d>::iterator GraphIterator;
 typedef std::unordered_map<int, Eigen::Vector3d>::const_iterator ConstGraphIterator;
 
+typedef rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CallbackReturn;
+
 }   // namespace toolbox_types
 
 #endif  // SLAM_TOOLBOX__TOOLBOX_TYPES_HPP_

include/slam_toolbox/visualization_utils.hpp

@@ -20,24 +20,28 @@
 #define SLAM_TOOLBOX__VISUALIZATION_UTILS_HPP_
 
 #include <string>
+#include <memory>
 
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 #include "visualization_msgs/msg/interactive_marker.hpp"
 #include "visualization_msgs/msg/interactive_marker_feedback.hpp"
 
 namespace vis_utils
 {
 
+template<class ClockHandler>
 inline visualization_msgs::msg::Marker toMarker(
   const std::string & frame,
   const std::string & ns,
   const double & scale,
-  rclcpp::Node::SharedPtr node)
+  std::shared_ptr<ClockHandler> handler)
 {
   visualization_msgs::msg::Marker marker;
 
   marker.header.frame_id = frame;
-  marker.header.stamp = node->now();
+  marker.header.stamp = handler->now();
   marker.ns = ns;
   marker.type = visualization_msgs::msg::Marker::SPHERE;
   marker.pose.position.z = 0.0;
@@ -55,15 +59,16 @@ inline visualization_msgs::msg::Marker toMarker(
   return marker;
 }
 
+template<class ClockHandler>
 inline visualization_msgs::msg::InteractiveMarker toInteractiveMarker(
   visualization_msgs::msg::Marker & marker,
   const double & scale,
-  rclcpp::Node::SharedPtr node)
+  std::shared_ptr<ClockHandler> handler)
 {
   // marker basics
   visualization_msgs::msg::InteractiveMarker int_marker;
   int_marker.header.frame_id = marker.header.frame_id;
-  int_marker.header.stamp = node->now();
+  int_marker.header.stamp = handler->now();
   int_marker.name = std::to_string(marker.id);
   int_marker.pose.orientation.w = 1.;
   int_marker.pose.position.x = marker.pose.position.x;