Add lifecycle node compatibility to camera_info_manager (#190)

* Add lifecycle node compatibility to camera_info_manager * Add additional lifecycle constructor * Fix CI
ros-perception · Sep 1, 2022 · 24e341a · 24e341a
1 parent 0b91d62
commit 24e341a
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 9 deletions.
diff --git a/camera_info_manager/CMakeLists.txt b/camera_info_manager/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(camera_calibration_parsers REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
@@ -31,6 +32,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 target_compile_definitions(${PROJECT_NAME} PRIVATE "CAMERA_INFO_MANAGER_BUILDING_DLL")
 target_link_libraries(${PROJECT_NAME} PUBLIC
   rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS})
 target_link_libraries(${PROJECT_NAME} PRIVATE
   ament_index_cpp::ament_index_cpp
@@ -47,7 +49,7 @@ install(
 
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
-ament_export_dependencies(ament_index_cpp camera_calibration_parsers rclcpp rcpputils sensor_msgs)
+ament_export_dependencies(ament_index_cpp camera_calibration_parsers rclcpp rclcpp_lifecycle rcpputils sensor_msgs)
 ament_export_targets(export_${PROJECT_NAME})
 
 if(BUILD_TESTING)

diff --git a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp
@@ -43,6 +43,7 @@
 #include <string>
 
 #include "rclcpp/node.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "sensor_msgs/msg/camera_info.hpp"
 #include "sensor_msgs/srv/set_camera_info.hpp"
 #include "camera_info_manager/visibility_control.h"
@@ -187,6 +188,20 @@ class CameraInfoManager
     const std::string & cname = "camera",
     const std::string & url = "");
 
+  CAMERA_INFO_MANAGER_PUBLIC
+  CameraInfoManager(
+    rclcpp_lifecycle::LifecycleNode * node,
+    const std::string & cname = "camera",
+    const std::string & url = "");
+
+  CAMERA_INFO_MANAGER_PUBLIC
+  CameraInfoManager(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logger_interface,
+    const std::string & cname = "camera", const std::string & url = "",
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
+
   CAMERA_INFO_MANAGER_PUBLIC
   CameraInfo getCameraInfo(void);
 

diff --git a/camera_info_manager/package.xml b/camera_info_manager/package.xml
@@ -27,6 +27,7 @@
   <depend>ament_index_cpp</depend>
   <depend>camera_calibration_parsers</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>sensor_msgs</depend>
 

diff --git a/camera_info_manager/src/camera_info_manager.cpp b/camera_info_manager/src/camera_info_manager.cpp
@@ -78,20 +78,37 @@ const std::string
  * @param url default Uniform Resource Locator for loading and saving data.
  */
 CameraInfoManager::CameraInfoManager(
-  rclcpp::Node * node,
-  const std::string & cname,
+  rclcpp::Node * node, const std::string & cname,
   const std::string & url)
-: logger_(node->get_logger()),
+: CameraInfoManager(node->get_node_base_interface(),
+    node->get_node_services_interface(), node->get_node_logging_interface(), cname, url)
+{
+}
+
+CameraInfoManager::CameraInfoManager(
+  rclcpp_lifecycle::LifecycleNode * node,
+  const std::string & cname, const std::string & url)
+: CameraInfoManager(node->get_node_base_interface(),
+    node->get_node_services_interface(), node->get_node_logging_interface(), cname, url)
+{
+}
+
+CameraInfoManager::CameraInfoManager(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logger_interface,
+  const std::string & cname, const std::string & url, rmw_qos_profile_t custom_qos)
+: logger_(node_logger_interface->get_logger()),
   camera_name_(cname),
   url_(url),
   loaded_cam_info_(false)
 {
+  using namespace std::placeholders;
+
   // register callback for camera calibration service request
-  info_service_ = node->create_service<SetCameraInfo>(
-    "set_camera_info",
-    std::bind(
-      &CameraInfoManager::setCameraInfoService, this, std::placeholders::_1,
-      std::placeholders::_2));
+  info_service_ = rclcpp::create_service<SetCameraInfo>(
+    node_base_interface, node_services_interface, "~/set_camera_info",
+    std::bind(&CameraInfoManager::setCameraInfoService, this, _1, _2), custom_qos, nullptr);
 }
 
 /** Get the current CameraInfo data.