lifecylce test/demo v1

remibettan · Jan 9, 2025 · 29a711b · 29a711b
1 parent 11bea2e
commit 29a711b
Show file tree

Hide file tree

Showing 13 changed files with 43 additions and 23 deletions.
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
@@ -118,6 +118,8 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 
 find_package(realsense2 2.56.0)
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)
@@ -253,6 +255,9 @@ set(dependencies
   tf2
   tf2_ros
   diagnostic_updater
+  #Eigen3
+  rclcpp_lifecycle
+  lifecycle_msgs
 )
 
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -17,6 +17,8 @@
 #include <librealsense2/rs.hpp>
 #include <librealsense2/rsutil.h>
 #include "constants.h"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
 
 // cv_bridge.h last supported version is humble
 #if defined(CV_BRDIGE_HAS_HPP)
@@ -25,6 +27,7 @@
 #include <cv_bridge/cv_bridge.h>
 #endif
 
+#include "rclcpp/rclcpp.hpp"
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
 #include <std_srvs/srv/empty.hpp>
@@ -115,7 +118,7 @@ namespace realsense2_camera
     class BaseRealSenseNode
     {
     public:
-        BaseRealSenseNode(rclcpp::Node& node,
+        BaseRealSenseNode(rclcpp_lifecycle::LifecycleNode& node,
                           rs2::device dev,
                           std::shared_ptr<Parameters> parameters,
                           bool use_intra_process = false);
@@ -152,7 +155,7 @@ namespace realsense2_camera
 
         std::string _base_frame_id;
         bool _is_running;
-        rclcpp::Node& _node;
+        rclcpp_lifecycle::LifecycleNode& _node;
         std::string _camera_name;
         std::vector<rs2_option> _monitor_options;
         rclcpp::Logger _logger;

diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h
@@ -23,7 +23,7 @@ namespace realsense2_camera
     class Parameters
     {
         public:
-            Parameters(rclcpp::Node& node);
+            Parameters(rclcpp_lifecycle::LifecycleNode& node);
             ~Parameters();
             template <class T>
             T setParam(std::string param_name, const T& initial_value, 
@@ -54,7 +54,7 @@ namespace realsense2_camera
             void monitor_update_functions();
 
         private:
-            rclcpp::Node& _node;
+            rclcpp_lifecycle::LifecycleNode& _node;
             rclcpp::Logger _logger;
             std::map<std::string, std::function<void(const rclcpp::Parameter&)> > _param_functions;
             std::map<void*, std::string> _param_names;

diff --git a/realsense2_camera/include/image_publisher.h b/realsense2_camera/include/image_publisher.h
@@ -15,6 +15,8 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
 #include <sensor_msgs/msg/image.hpp>
 
 #include <image_transport/image_transport.hpp>
@@ -32,7 +34,7 @@ class image_publisher
 class image_rcl_publisher : public image_publisher
 {
 public:
-    image_rcl_publisher( rclcpp::Node & node,
+    image_rcl_publisher( rclcpp_lifecycle::LifecycleNode & node,
                          const std::string & topic_name,
                          const rmw_qos_profile_t & qos );
     void publish( sensor_msgs::msg::Image::UniquePtr image_ptr ) override;
@@ -46,7 +48,7 @@ class image_rcl_publisher : public image_publisher
 class image_transport_publisher : public image_publisher
 {
 public:
-    image_transport_publisher( rclcpp::Node & node,
+    image_transport_publisher( rclcpp_lifecycle::LifecycleNode & node,
                                const std::string & topic_name,
                                const rmw_qos_profile_t & qos );
     void publish( sensor_msgs::msg::Image::UniquePtr image_ptr ) override;

diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h
@@ -51,7 +51,7 @@ namespace realsense2_camera
     class PointcloudFilter : public NamedFilter
     {
         public:
-            PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp::Node& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);
+            PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp_lifecycle::LifecycleNode& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);
 
             void setPublisher();
             void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id);
@@ -61,7 +61,7 @@ namespace realsense2_camera
 
         private:
             bool _is_enabled_pc;
-            rclcpp::Node& _node;
+            rclcpp_lifecycle::LifecycleNode& _node;
             bool _allow_no_texture_points;
             bool _ordered_pc;
             std::mutex _mutex_publisher;

diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h
@@ -16,6 +16,7 @@
 
 // cpplint: c system headers
 #include "constants.h"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "base_realsense_node.h"
 #include <builtin_interfaces/msg/time.hpp>
 #include <console_bridge/console.h>
@@ -34,7 +35,7 @@
 
 namespace realsense2_camera
 {
-    class RealSenseNodeFactory : public rclcpp::Node
+    class RealSenseNodeFactory : public rclcpp_lifecycle::LifecycleNode //public rclcpp::Node
     {
     public:
         explicit RealSenseNodeFactory(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
@@ -43,6 +44,13 @@ namespace realsense2_camera
             const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
         virtual ~RealSenseNodeFactory();
 
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &)
+  {
+    ROS_INFO_STREAM("Samer......");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+  }
+
     private:
         void init();
         void closeDevice();

diff --git a/realsense2_camera/include/ros_param_backend.h b/realsense2_camera/include/ros_param_backend.h
@@ -15,13 +15,15 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
 
 namespace realsense2_camera
 {
     class ParametersBackend
     {
         public:
-            ParametersBackend(rclcpp::Node& node) : 
+            ParametersBackend(rclcpp_lifecycle::LifecycleNode& node) : 
                 _node(node),
                 _logger(node.get_logger())
                 {}
@@ -38,7 +40,7 @@ namespace realsense2_camera
 
 
         private:
-            rclcpp::Node& _node;
+            rclcpp_lifecycle::LifecycleNode& _node;
             rclcpp::Logger _logger;
             std::shared_ptr<void> _ros_callback;
     };

diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py
@@ -19,7 +19,7 @@
 import launch_ros.actions
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
-
+from launch_ros.actions import LifecycleNode
 
 configurable_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
                            {'name': 'camera_namespace',             'default': 'camera', 'description': 'namespace for camera'},
@@ -114,7 +114,7 @@ def launch_setup(context, params, param_name_suffix=''):
         _output = context.perform_substitution(_output)
 
     return [
-        launch_ros.actions.Node(
+        launch_ros.actions.LifecycleNode(
             package='realsense2_camera',
             namespace=LaunchConfiguration('camera_namespace' + param_name_suffix),
             name=LaunchConfiguration('camera_name' + param_name_suffix),

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -88,7 +88,7 @@ size_t SyncedImuPublisher::getNumSubscribers()
     return _publisher->get_subscription_count();
 }
 
-BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
+BaseRealSenseNode::BaseRealSenseNode(rclcpp_lifecycle::LifecycleNode& node,
                                      rs2::device dev,
                                      std::shared_ptr<Parameters> parameters,
                                      bool use_intra_process) :

diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp
@@ -16,7 +16,7 @@
 
 namespace realsense2_camera
 {
-    Parameters::Parameters(rclcpp::Node& node) :
+    Parameters::Parameters(rclcpp_lifecycle::LifecycleNode& node) :
     _node(node),
     _logger(node.get_logger()),
     _params_backend(node),

diff --git a/realsense2_camera/src/image_publisher.cpp b/realsense2_camera/src/image_publisher.cpp
@@ -17,7 +17,7 @@
 using namespace realsense2_camera;
 
 // --- image_rcl_publisher implementation ---
-image_rcl_publisher::image_rcl_publisher( rclcpp::Node & node,
+image_rcl_publisher::image_rcl_publisher( rclcpp_lifecycle::LifecycleNode & node,
                                           const std::string & topic_name,
                                           const rmw_qos_profile_t & qos )
 {
@@ -37,16 +37,16 @@ size_t image_rcl_publisher::get_subscription_count() const
 }
 
 // --- image_transport_publisher implementation ---
-image_transport_publisher::image_transport_publisher( rclcpp::Node & node,
+image_transport_publisher::image_transport_publisher( rclcpp_lifecycle::LifecycleNode & node,
                                                       const std::string & topic_name,
                                                       const rmw_qos_profile_t & qos )
 {
-    image_publisher_impl = std::make_shared< image_transport::Publisher >(
-        image_transport::create_publisher( &node, topic_name, qos ) );
+    image_publisher_impl = std::make_shared< image_transport::Publisher >();
+    //image_transport::create_publisher( n, topic_name, qos ) );
 }
 void image_transport_publisher::publish( sensor_msgs::msg::Image::UniquePtr image_ptr )
 {
-    image_publisher_impl->publish( *image_ptr );
+    //image_publisher_impl->publish( *image_ptr );
 }
 
 size_t image_transport_publisher::get_subscription_count() const

diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp
@@ -73,7 +73,7 @@ rs2::frame NamedFilter::Process(rs2::frame frame)
 }
 
 
-PointcloudFilter::PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp::Node& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
+PointcloudFilter::PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp_lifecycle::LifecycleNode& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
     NamedFilter(filter, parameters, logger, is_enabled, false),
     _node(node),
     _allow_no_texture_points(ALLOW_NO_TEXTURE_POINTS),

diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp
@@ -31,15 +31,15 @@ using namespace realsense2_camera;
 constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR;
 
 RealSenseNodeFactory::RealSenseNodeFactory(const rclcpp::NodeOptions & node_options) :
-    Node("camera", "/camera", node_options),
+    rclcpp_lifecycle::LifecycleNode("camera", "/", node_options),
     _logger(this->get_logger())
 {
   init();
 }
 
 RealSenseNodeFactory::RealSenseNodeFactory(const std::string & node_name, const std::string & ns,
                                            const rclcpp::NodeOptions & node_options) :
-    Node(node_name, ns, node_options),
+    rclcpp_lifecycle::LifecycleNode(node_name, ns, node_options),
     _logger(this->get_logger())
 {
   init();