0.2.0

coresense_instrumentation/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package coresense_instrumentation
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.0 (2023-11-30)
+------------------
+* Added new type: sensor_msgs/msg/Image with specialiced template
+
+0.1.0 (2023-11-29)
+------------------
+* Driver works correctly

coresense_instrumentation/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>coresense_instrumentation</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Coresense Instrumentation</description>
   <maintainer email="[email protected]">Juan Carlos Manzanares Serrano</maintainer>
   <license>Apache-2.0</license>

coresense_instrumentation_driver/CHANGELOG.rst

@@ -1,7 +1,11 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package coresense_instrumentation
+Changelog for package coresense_instrumentation_drived
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.2.0 (2023-11-30)
+------------------
+* Added new type: sensor_msgs/msg/Image with specialiced template
+
 0.1.0 (2023-11-29)
 ------------------
 * Driver works correctly

coresense_instrumentation_driver/CMakeLists.txt

@@ -12,26 +12,30 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(image_transport REQUIRED)
 
 set(dependencies
   rclcpp
   rclcpp_components
   rclcpp_lifecycle
   std_msgs
   sensor_msgs
+  image_transport
 )
 
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
-  src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp  #
+  src/coresense_instrumentation_driver/InstrumentationGeneric.cpp
+  src/coresense_instrumentation_driver/InstrumentationImage.cpp
 )
 
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})
 
 rclcpp_components_register_nodes(${PROJECT_NAME}
   "coresense_instrumentation_driver::InstrumentationLifecycleNode<std_msgs::msg::String>"
   "coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::LaserScan>"
+  "coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::Image>"
 )
 
 install(TARGETS

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp

@@ -19,10 +19,14 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "image_transport/image_transport.hpp"
 
 namespace coresense_instrumentation_driver
 {
 
+using std::placeholders::_1;
+
 template<typename TopicT>
 class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode
 {
@@ -50,6 +54,36 @@ class InstrumentationLifecycleNode : public rclcpp_lifecycle::LifecycleNode
   std::string topic_type_;
 };
 
+template<>
+class InstrumentationLifecycleNode<sensor_msgs::msg::Image>: public rclcpp_lifecycle::LifecycleNode
+{
+public:
+  InstrumentationLifecycleNode(
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  virtual ~InstrumentationLifecycleNode();
+
+  using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+  CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override;
+  CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override;
+
+  std::string get_topic();
+  std::string get_topic_type();
+
+private:
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
+
+  rclcpp::Node::SharedPtr node_;
+  image_transport::Publisher pub_;
+  image_transport::Subscriber sub_;
+  std::string topic_;
+  std::string topic_type_;
+};
+
 } // namespace coresense_instrumentation_driver
 
 #endif // INSTRUMENTATION_LIFECYCLE_NODE_HPP

coresense_instrumentation_driver/launch/coresense_instrumentation_driver.launch.py

@@ -19,9 +19,9 @@
 
 def generate_launch_description():
 
-    names = ['scan', 'chatter']
-    topics = ['/scan', '/chatter']
-    types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String']
+    names = ['scan', 'chatter', 'image']
+    topics = ['/scan', '/chatter', '/image']
+    types = ['sensor_msgs::msg::LaserScan', 'std_msgs::msg::String', 'sensor_msgs::msg::Image']
 
     composable_nodes = []
     for topic, topic_type, name in zip(topics, types, names):

coresense_instrumentation_driver/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>coresense_instrumentation_driver</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>Coresense Instrumentation Driver</description>
   <maintainer email="[email protected]">Juan Carlos Manzanares Serrano</maintainer>
   <license>Apache-2.0</license>
@@ -14,6 +14,7 @@
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationLifecycleNode.cpp → coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationGeneric.cpp

@@ -14,30 +14,31 @@
 
 #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp"
 
-template class coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::LaserScan>;
-template class coresense_instrumentation_driver::InstrumentationLifecycleNode<std_msgs::msg::String>;
 
 namespace coresense_instrumentation_driver
 {
 
+template class coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::LaserScan>;
+template class coresense_instrumentation_driver::InstrumentationLifecycleNode<std_msgs::msg::String>;
+
 template<typename TopicT>
 InstrumentationLifecycleNode<TopicT>::InstrumentationLifecycleNode(
   const rclcpp::NodeOptions & options)
-: rclcpp_lifecycle::LifecycleNode("InstrumentationLifecycleNode", "", options)
+: rclcpp_lifecycle::LifecycleNode("lifecycle_node", "", options)
 {
   declare_parameter("topic", std::string(""));
   declare_parameter("topic_type", std::string(""));
 
   get_parameter("topic", topic_);
   get_parameter("topic_type", topic_type_);
 
-  RCLCPP_DEBUG(get_logger(), "Creating InstrumentationLifecycleNode");
+  RCLCPP_INFO(get_logger(), "Creating InstrumentationGeneric");
 }
 
 template<typename TopicT>
 InstrumentationLifecycleNode<TopicT>::~InstrumentationLifecycleNode()
 {
-  RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationLifecycleNode");
+  RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationGeneric");
 }
 
 template<typename TopicT>

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationImage.cpp

@@ -0,0 +1,110 @@
+// Copyright 2023 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp"
+
+namespace coresense_instrumentation_driver
+{
+
+template class coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::Image>;
+
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::InstrumentationLifecycleNode(
+  const rclcpp::NodeOptions & options)
+: rclcpp_lifecycle::LifecycleNode("image_node", "", options)
+{
+  declare_parameter("topic", std::string(""));
+  declare_parameter("topic_type", std::string(""));
+
+  get_parameter("topic", topic_);
+  get_parameter("topic_type", topic_type_);
+
+  node_ = rclcpp::Node::make_shared("subnode");
+
+  RCLCPP_INFO(get_logger(), "Creating InstrumentationImage");
+}
+
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::~InstrumentationLifecycleNode()
+{
+  RCLCPP_DEBUG(get_logger(), "Destroying InstrumentationImage");
+}
+
+void InstrumentationLifecycleNode<sensor_msgs::msg::Image>::imageCallback(
+  const sensor_msgs::msg::Image::ConstSharedPtr & msg)
+{
+  if (pub_ && pub_.getNumSubscribers() > 0) {
+    pub_.publish(msg);
+  }
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_configure(const rclcpp_lifecycle::State &)
+{
+  auto subscription_options = rclcpp::SubscriptionOptions();
+
+  sub_ = image_transport::create_subscription(
+    node_.get(),
+    topic_,
+    std::bind(
+      &InstrumentationLifecycleNode<sensor_msgs::msg::Image>::imageCallback, this,
+      std::placeholders::_1),
+    "raw",
+    rmw_qos_profile_sensor_data,
+    subscription_options);
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_activate(const rclcpp_lifecycle::State &)
+{
+  image_transport::ImageTransport it(node_);
+  pub_ = it.advertise("/coresense" + topic_, 1);
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_deactivate(
+  const rclcpp_lifecycle::State &)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+InstrumentationLifecycleNode<sensor_msgs::msg::Image>::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+std::string InstrumentationLifecycleNode<sensor_msgs::msg::Image>::get_topic()
+{
+  return topic_;
+}
+
+std::string InstrumentationLifecycleNode<sensor_msgs::msg::Image>::get_topic_type()
+{
+  return topic_type_;
+}
+
+} // namespace coresense_instrumentation_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(
+  coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::Image>)

coresense_instrumentation_driver/src/coresense_instrumentation_driver_node.cpp

@@ -29,20 +29,23 @@ int main(int argc, char * argv[])
 
   auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
-  if (topic_type == "std_msgs::msg::String") {
+  if (topic_type == "sensor_msgs/msg/LaserScan") {
+    auto node =
+      std::make_shared<coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::LaserScan>>();
+    executor->add_node(node->get_node_base_interface());
+    executor->spin();
+  } else if (topic_type == "std_msgs/msg/String") {
     auto node =
       std::make_shared<coresense_instrumentation_driver::InstrumentationLifecycleNode<std_msgs::msg::String>>();
-
     executor->add_node(node->get_node_base_interface());
     executor->spin();
-  } else if (topic_type == "sensor_msgs::msg::LaserScan") {
+  } else if (topic_type == "sensor_msgs/msg/Image") {
     auto node =
-      std::make_shared<coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::LaserScan>>();
-
+      std::make_shared<coresense_instrumentation_driver::InstrumentationLifecycleNode<sensor_msgs::msg::Image>>();
     executor->add_node(node->get_node_base_interface());
     executor->spin();
   } else {
-    RCLCPP_INFO(rclcpp::get_logger("main"), "Usage: %s <topic> <topic_type>", argv[0]);
+    RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown topic type: %s", topic_type.c_str());
     return 1;
   }
 

coresense_instrumentation_driver/test/test_coresense_instrumentation_driver.cpp

@@ -2,6 +2,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <std_msgs/msg/string.hpp>
 #include "coresense_instrumentation_driver/InstrumentationLifecycleNode.hpp"
 
@@ -103,6 +104,48 @@ TEST_F(IntegrationTest, StringNodeLifecycle)
   executor.spin_once(std::chrono::seconds(1));
 }
 
+TEST_F(IntegrationTest, ImageNodeLifecycle)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+
+  auto node =
+    std::make_shared<coresense_instrumentation_driver::InstrumentationLifecycleNode<std_msgs::msg::String>>(
+    rclcpp::NodeOptions().append_parameter_override(
+      "topic",
+      "/test_topic").append_parameter_override(
+      "topic_type", "sensor_msgs::msg::Image"));
+
+  executor.add_node(node->get_node_base_interface());
+
+  auto result = node->on_configure(rclcpp_lifecycle::State());
+  ASSERT_EQ(
+    result,
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+
+  result = node->on_activate(rclcpp_lifecycle::State());
+  ASSERT_EQ(
+    result,
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+
+  node->on_activate(rclcpp_lifecycle::State());
+  result = node->on_deactivate(rclcpp_lifecycle::State());
+  ASSERT_EQ(
+    result,
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+
+  result = node->on_cleanup(rclcpp_lifecycle::State());
+  ASSERT_EQ(
+    result,
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+
+  result = node->on_shutdown(rclcpp_lifecycle::State());
+  ASSERT_EQ(
+    result,
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+
+  executor.spin_once(std::chrono::seconds(1));
+}
+
 TEST_F(IntegrationTest, GetTopic)
 {
   auto node =