diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3453a665d..3cdcd8c93 100644 --- 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 index e6b4287a8..6066a14fc 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -17,6 +17,8 @@ #include #include #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 #endif +#include "rclcpp/rclcpp.hpp" #include #include #include @@ -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, 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 _monitor_options; rclcpp::Logger _logger; diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index d438ad937..7639b1f77 100644 --- 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 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 > _param_functions; std::map _param_names; diff --git a/realsense2_camera/include/image_publisher.h b/realsense2_camera/include/image_publisher.h index 3d7d004c7..6f7059d23 100644 --- a/realsense2_camera/include/image_publisher.h +++ b/realsense2_camera/include/image_publisher.h @@ -15,6 +15,8 @@ #pragma once #include +#include "rclcpp_lifecycle/lifecycle_node.hpp" + #include #include @@ -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 index 7fef1564c..9b30f3bc0 100644 --- 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 filter, rclcpp::Node& node, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled=false); + PointcloudFilter(std::shared_ptr filter, rclcpp_lifecycle::LifecycleNode& node, std::shared_ptr 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 index da807ac7a..45baf064f 100755 --- 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 #include @@ -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 index df1aa2620..de8353802 100644 --- a/realsense2_camera/include/ros_param_backend.h +++ b/realsense2_camera/include/ros_param_backend.h @@ -15,13 +15,15 @@ #pragma once #include +#include + 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 _ros_callback; }; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 54901a2b4..e4aba0ea6 100644 --- 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 index cb1ee3e72..cd3769efc 100755 --- 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, bool use_intra_process) : diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index ef7d73cd9..ecf0fa7c4 100644 --- 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 index 36525fa03..f408ac5ff 100644 --- 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 index 31fd80a8c..f2916407e 100644 --- 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 filter, rclcpp::Node& node, std::shared_ptr parameters, rclcpp::Logger logger, bool is_enabled): +PointcloudFilter::PointcloudFilter(std::shared_ptr filter, rclcpp_lifecycle::LifecycleNode& node, std::shared_ptr 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 index 791e3a952..643da9a2a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -31,7 +31,7 @@ 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(); @@ -39,7 +39,7 @@ RealSenseNodeFactory::RealSenseNodeFactory(const rclcpp::NodeOptions & node_opti 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();