Composition

CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(ament_cmake REQUIRED)
 find_package(Boost REQUIRED COMPONENTS chrono)
 find_package(diagnostic_updater REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -29,58 +30,56 @@ find_package(sick_safetyscanners2_interfaces REQUIRED)
 set(dependencies
         diagnostic_updater
         rclcpp
+        rclcpp_components
         rclcpp_lifecycle
         lifecycle_msgs
         sensor_msgs
         sick_safetyscanners_base
         sick_safetyscanners2_interfaces
 )
 
-add_executable(sick_safetyscanners2_node
-        src/sick_safetyscanners2_node.cpp
+add_library(sick_safetyscanners2 SHARED
         src/SickSafetyscanners.cpp
         src/SickSafetyscannersRos2.cpp
         src/utils/MessageCreator.cpp)
-
-target_link_libraries(sick_safetyscanners2_node
+target_link_libraries(sick_safetyscanners2
         sick_safetyscanners_base::sick_safetyscanners_base
         ${Boost_LIBRARIES})
-
-ament_target_dependencies(sick_safetyscanners2_node ${dependencies})
-
-target_include_directories(sick_safetyscanners2_node PUBLIC
+ament_target_dependencies(sick_safetyscanners2 ${dependencies})
+target_include_directories(sick_safetyscanners2 PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
 
-add_executable(sick_safetyscanners2_lifecycle_node
-        src/sick_safetyscanners2_lifecycle_node.cpp
-        src/SickSafetyscanners.cpp
+add_executable(sick_safetyscanners2_node
+        src/sick_safetyscanners2_node.cpp)
+target_link_libraries(sick_safetyscanners2_node
+        sick_safetyscanners2)
+
+add_library(sick_safetyscanners2_lifecycle SHARED
         src/SickSafetyscannersLifeCycle.cpp
+        src/SickSafetyscanners.cpp
         src/utils/MessageCreator.cpp)
-
-target_link_libraries(sick_safetyscanners2_lifecycle_node
+target_link_libraries(sick_safetyscanners2_lifecycle
         sick_safetyscanners_base::sick_safetyscanners_base
         ${Boost_LIBRARIES})
-
-ament_target_dependencies(sick_safetyscanners2_lifecycle_node ${dependencies})
-
-target_include_directories(sick_safetyscanners2_lifecycle_node PUBLIC
+ament_target_dependencies(sick_safetyscanners2_lifecycle ${dependencies})
+target_include_directories(sick_safetyscanners2_lifecycle PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
 
+add_executable(sick_safetyscanners2_lifecycle_node
+        src/sick_safetyscanners2_lifecycle_node.cpp)
+target_link_libraries(sick_safetyscanners2_lifecycle_node
+        sick_safetyscanners2_lifecycle)
+
+rclcpp_components_register_nodes(sick_safetyscanners2 "sick::SickSafetyscannersRos2")
+rclcpp_components_register_nodes(sick_safetyscanners2_lifecycle "sick::SickSafetyscannersLifeCycle")
+
 install(DIRECTORY
         launch description rviz
         DESTINATION share/${PROJECT_NAME}/
 )
 
-install(TARGETS sick_safetyscanners2_node
-        EXPORT export_${PROJECT_NAME}
-        DESTINATION lib/${PROJECT_NAME})
-
-install(TARGETS sick_safetyscanners2_lifecycle_node
-        EXPORT export_${PROJECT_NAME}
-        DESTINATION lib/${PROJECT_NAME})
-
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     # the following line skips the linter which checks for copyrights
@@ -92,8 +91,33 @@ if (BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 endif ()
 
-#TODO lookup what this does
-# For message creation,if used in this message
-ament_export_dependencies(${dependencies})
+install(
+  TARGETS 
+  sick_safetyscanners2
+  sick_safetyscanners2_lifecycle
+  EXPORT  sick_safetyscanners2_target   # target for ament_export_targets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  INCLUDES DESTINATION include  # for ament_export_include_directories
+)
+
+# Install executable
+install(
+  TARGETS 
+  sick_safetyscanners2_node
+  sick_safetyscanners2_lifecycle_node
+  RUNTIME DESTINATION lib/sick_safetyscanners2  # ros2 run <pkg> <exe>
+)
+
+# install header files
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) # this path is automatically added to the include path of the library
+ament_export_targets(sick_safetyscanners2_target) # target name
+ament_export_libraries(sick_safetyscanners2 sick_safetyscanners2_lifecycle)
+ament_export_dependencies(${dependencies})  # transitive dependencies for other packages
 
 ament_package()

include/sick_safetyscanners2/SickSafetyscannersLifeCycle.hpp

@@ -73,6 +73,12 @@ class SickSafetyscannersLifeCycle : public rclcpp_lifecycle::LifecycleNode,
   explicit SickSafetyscannersLifeCycle(const std::string &node_name,
                                        bool intra_process_comms = false);
 
+  /*!
+   * \brief Constructor of the ROS2 Node handling the Communication of the Sick
+   * Safetyscanner
+   */
+  explicit SickSafetyscannersLifeCycle(const rclcpp::NodeOptions& options);
+
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State &);
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

include/sick_safetyscanners2/SickSafetyscannersRos2.h

@@ -61,7 +61,7 @@ class SickSafetyscannersRos2 : public rclcpp::Node, public SickSafetyscanners {
    * \brief Constructor of the ROS2 Node handling the Communication of the Sick
    * Safetyscanner
    */
-  SickSafetyscannersRos2();
+  explicit SickSafetyscannersRos2(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
 
 private:
   // Publishers

include/sick_safetyscanners2/utils/MessageCreator.h

@@ -76,7 +76,7 @@ class MessageCreator {
    *
    * \returns The constructed LaserScan Message
    */
-  sensor_msgs::msg::LaserScan
+  std::unique_ptr<sensor_msgs::msg::LaserScan>
   createLaserScanMsg(const sick::datastructure::Data &data, rclcpp::Time now);
 
   /*!
@@ -86,7 +86,7 @@ class MessageCreator {
    *
    * \returns The constructed OutputPaths Message
    */
-  sick_safetyscanners2_interfaces::msg::OutputPaths
+  std::unique_ptr<sick_safetyscanners2_interfaces::msg::OutputPaths>
   createOutputPathsMsg(const sick::datastructure::Data &data);
 
   /*!
@@ -98,7 +98,7 @@ class MessageCreator {
    *
    * \returns The constructed extended LaserScan Message
    */
-  sick_safetyscanners2_interfaces::msg::ExtendedLaserScan
+  std::unique_ptr<sick_safetyscanners2_interfaces::msg::ExtendedLaserScan>
   createExtendedLaserScanMsg(const sick::datastructure::Data &data,
                              rclcpp::Time now);
 
@@ -109,7 +109,7 @@ class MessageCreator {
    *
    * \returns The raw values of the sensor in a ROS2 Message.
    */
-  sick_safetyscanners2_interfaces::msg::RawMicroScanData
+  std::unique_ptr<sick_safetyscanners2_interfaces::msg::RawMicroScanData>
   createRawDataMsg(const sick::datastructure::Data &data);
 
 private:

package.xml

@@ -17,6 +17,7 @@
   <depend>boost</depend>
   <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
   <depend>sick_safetyscanners_base</depend>

src/SickSafetyscannersLifeCycle.cpp

@@ -42,7 +42,15 @@ SickSafetyscannersLifeCycle::SickSafetyscannersLifeCycle(
           node_name,
           rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) {
   RCLCPP_INFO(this->get_logger(), "Initializing SickSafetyscannersLifeCycle ");
+  // read parameters!
+  initializeParameters(*this);
+  loadParameters(*this);
+}
 
+SickSafetyscannersLifeCycle::SickSafetyscannersLifeCycle(const rclcpp::NodeOptions& options)
+  : rclcpp_lifecycle::LifecycleNode("SickSafetyscannersLifecycle", options)
+{
+  RCLCPP_INFO(this->get_logger(), "Initializing SickSafetyscannersLifeCycle ");
   // read parameters!
   initializeParameters(*this);
   loadParameters(*this);
@@ -53,16 +61,18 @@ SickSafetyscannersLifeCycle::on_configure(const rclcpp_lifecycle::State &) {
   RCLCPP_INFO(this->get_logger(), "on_configure()...");
 
   // init publishers and services
+  // set QOS profile for publishers explicitly
+  auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data), rmw_qos_profile_sensor_data).keep_last(1);
   m_laser_scan_publisher =
-      this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 1);
+      this->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos);
   m_extended_laser_scan_publisher = this->create_publisher<
       sick_safetyscanners2_interfaces::msg::ExtendedLaserScan>("extended_scan",
-                                                               1);
+                                                               qos);
   m_output_paths_publisher =
       this->create_publisher<sick_safetyscanners2_interfaces::msg::OutputPaths>(
-          "output_paths", 1);
+          "output_paths", qos);
   m_raw_data_publisher = this->create_publisher<
-      sick_safetyscanners2_interfaces::msg::RawMicroScanData>("raw_data", 1);
+      sick_safetyscanners2_interfaces::msg::RawMicroScanData>("raw_data", qos);
 
   m_field_data_service =
       this->create_service<sick_safetyscanners2_interfaces::srv::FieldData>(
@@ -157,19 +167,29 @@ void SickSafetyscannersLifeCycle::receiveUDPPaket(
 
   if (!data.getMeasurementDataPtr()->isEmpty() &&
       !data.getDerivedValuesPtr()->isEmpty()) {
-    auto scan = m_config.m_msg_creator->createLaserScanMsg(data, this->now());
-    m_diagnosed_laser_scan_publisher->publish(scan);
-
-    sick_safetyscanners2_interfaces::msg::ExtendedLaserScan extended_scan =
-        m_config.m_msg_creator->createExtendedLaserScanMsg(data, this->now());
-
-    m_extended_laser_scan_publisher->publish(extended_scan);
-
-    auto output_paths = m_config.m_msg_creator->createOutputPathsMsg(data);
-    m_output_paths_publisher->publish(output_paths);
+    if(m_diagnosed_laser_scan_publisher->getPublisher()->get_subscription_count() > 0){
+      auto scan = m_config.m_msg_creator->createLaserScanMsg(data, this->now());
+      m_diagnosed_laser_scan_publisher->publish(std::move(scan));
+    }
+
+    if(m_extended_laser_scan_publisher->get_subscription_count() > 0){
+      auto extended_scan =
+          m_config.m_msg_creator->createExtendedLaserScanMsg(data, this->now());
+      m_extended_laser_scan_publisher->publish(std::move(extended_scan));
+    }
+    if(m_output_paths_publisher->get_subscription_count() > 0){
+      auto output_paths = m_config.m_msg_creator->createOutputPathsMsg(data);
+      m_output_paths_publisher->publish(std::move(output_paths));
+    }
   }
 
-  m_last_raw_msg = m_config.m_msg_creator->createRawDataMsg(data);
-  m_raw_data_publisher->publish(m_last_raw_msg);
+  auto raw_msg = m_config.m_msg_creator->createRawDataMsg(data);
+  m_last_raw_msg = *raw_msg;
+  if(m_raw_data_publisher->get_subscription_count() > 0) {
+    m_raw_data_publisher->publish(std::move(raw_msg));
+  }
 }
 } // namespace sick
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(sick::SickSafetyscannersLifeCycle)

src/SickSafetyscannersRos2.cpp

@@ -36,25 +36,27 @@
 
 namespace sick {
 
-SickSafetyscannersRos2::SickSafetyscannersRos2()
-    : Node("SickSafetyscannersRos2") {
+SickSafetyscannersRos2::SickSafetyscannersRos2(const rclcpp::NodeOptions& options):
+    Node("SickSafetyscannersRos2", options) {
   RCLCPP_INFO(this->get_logger(), "Initializing SickSafetyscannersRos2 Node");
 
   // read parameters!
   initializeParameters(*this);
   loadParameters(*this);
 
   // init publishers and services
+  // set QOS profile for publishers explicitly
+  auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data), rmw_qos_profile_sensor_data).keep_last(1);
   m_laser_scan_publisher =
-      this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 1);
+      this->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos);
   m_extended_laser_scan_publisher = this->create_publisher<
       sick_safetyscanners2_interfaces::msg::ExtendedLaserScan>("extended_scan",
-                                                               1);
+                                                               qos);
   m_output_paths_publisher =
       this->create_publisher<sick_safetyscanners2_interfaces::msg::OutputPaths>(
-          "output_paths", 1);
+          "output_paths", qos);
   m_raw_data_publisher = this->create_publisher<
-      sick_safetyscanners2_interfaces::msg::RawMicroScanData>("raw_data", 1);
+      sick_safetyscanners2_interfaces::msg::RawMicroScanData>("raw_data", qos);
 
   m_field_data_service =
       this->create_service<sick_safetyscanners2_interfaces::srv::FieldData>(
@@ -81,6 +83,7 @@ SickSafetyscannersRos2::SickSafetyscannersRos2()
   RCLCPP_INFO(this->get_logger(), "Node Configured and running");
 }
 
+
 void SickSafetyscannersRos2::receiveUDPPaket(
     const sick::datastructure::Data &data) {
   if (!m_config.m_msg_creator) {
@@ -91,19 +94,29 @@ void SickSafetyscannersRos2::receiveUDPPaket(
 
   if (!data.getMeasurementDataPtr()->isEmpty() &&
       !data.getDerivedValuesPtr()->isEmpty()) {
-    auto scan = m_config.m_msg_creator->createLaserScanMsg(data, this->now());
-    m_diagnosed_laser_scan_publisher->publish(scan);
-
-    sick_safetyscanners2_interfaces::msg::ExtendedLaserScan extended_scan =
-        m_config.m_msg_creator->createExtendedLaserScanMsg(data, this->now());
-
-    m_extended_laser_scan_publisher->publish(extended_scan);
-
-    auto output_paths = m_config.m_msg_creator->createOutputPathsMsg(data);
-    m_output_paths_publisher->publish(output_paths);
+    if(m_diagnosed_laser_scan_publisher->getPublisher()->get_subscription_count() > 0){
+      auto scan = m_config.m_msg_creator->createLaserScanMsg(data, this->now());
+      m_diagnosed_laser_scan_publisher->publish(std::move(scan));
+    }
+
+    if(m_extended_laser_scan_publisher->get_subscription_count() > 0){
+      auto extended_scan =
+          m_config.m_msg_creator->createExtendedLaserScanMsg(data, this->now());
+      m_extended_laser_scan_publisher->publish(std::move(extended_scan));
+    }
+    if(m_output_paths_publisher->get_subscription_count() > 0){
+      auto output_paths = m_config.m_msg_creator->createOutputPathsMsg(data);
+      m_output_paths_publisher->publish(std::move(output_paths));
+    }
   }
 
-  m_last_raw_msg = m_config.m_msg_creator->createRawDataMsg(data);
-  m_raw_data_publisher->publish(m_last_raw_msg);
+  auto raw_msg = m_config.m_msg_creator->createRawDataMsg(data);
+  m_last_raw_msg = *raw_msg;
+  if(m_raw_data_publisher->get_subscription_count() > 0) {
+    m_raw_data_publisher->publish(std::move(raw_msg));
+  }
 }
 } // namespace sick
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(sick::SickSafetyscannersRos2)