Tests and initial complete version

Signed-off-by: Francisco Martín Rico <[email protected]>
IntelligentRoboticsLabs · Mar 7, 2024 · 7e26905 · 7e26905
1 parent beb0e32
commit 7e26905
Show file tree

Hide file tree

Showing 12 changed files with 385 additions and 27 deletions.
diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml
@@ -0,0 +1,32 @@
+name: main
+
+on:
+  pull_request:
+    branches:
+      - main
+  push:
+    branches:
+      - main
+
+jobs:
+  build-and-test:
+    runs-on: ${{ matrix.os }}
+    container:
+      image: osrf/ros:rolling-desktop
+    strategy:
+      matrix:
+        os: [ubuntu-22.04]
+      fail-fast: false
+    steps:
+      - name: build and test
+        uses: ros-tooling/[email protected]
+        with:
+          package-name: quantif_ros2 quantif_ros2_interfaces
+          target-ros2-distro: rolling
+          vcs-repo-file-url: 
+          colcon-defaults: |
+            {
+              "test": {
+                "parallel-workers" : 1
+              }
+            }
diff --git a/quantif_ros2/CMakeLists.txt b/quantif_ros2/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(quantif_ros2_interfaces REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -24,6 +25,7 @@ set(dependencies
   cv_bridge
   OpenCV
   quantif_ros2_interfaces
+  ament_index_cpp
 )
 
 include_directories(include)
@@ -45,7 +47,7 @@ install(TARGETS
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config tests/configs DESTINATION share/${PROJECT_NAME})
 
 
 
@@ -54,6 +56,9 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(tests)
 endif()
 
 ament_package()
diff --git a/quantif_ros2/include/quantif_ros2/QR2Node.hpp b/quantif_ros2/include/quantif_ros2/QR2Node.hpp
@@ -30,7 +30,7 @@ class QR2Node : public rclcpp::Node
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(QR2Node)
-  
+
   QR2Node(
     const std::string & node_name,
     const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

diff --git a/quantif_ros2/include/quantif_ros2/QR2Publisher.hpp b/quantif_ros2/include/quantif_ros2/QR2Publisher.hpp
@@ -36,6 +36,7 @@ class QR2PublisherBase
   virtual ~QR2PublisherBase() {}
 
   virtual void produce_and_publish() = 0;
+  virtual std::string get_type() = 0;
 
 protected:
   void fill(quantif_ros2_interfaces::msg::Vector3 & msg)
@@ -53,7 +54,7 @@ class QR2PublisherBase
     msg.seq = counter_++;
     msg.data.header.frame_id = "laser";
     msg.data.header.stamp = node_->now();
-    msg.data.angle_increment = 0.01749303564429283; 
+    msg.data.angle_increment = 0.01749303564429283;
     msg.data.angle_min = -M_PI;
     msg.data.angle_max = M_PI;
     int size = (msg.data.angle_max - msg.data.angle_min) / msg.data.angle_increment;
@@ -79,9 +80,13 @@ class QR2Publisher : public QR2PublisherBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(QR2Publisher)
-
-  QR2Publisher(rclcpp::Node::SharedPtr node, const std::string & topic, rclcpp::QoS qos)
+
+  QR2Publisher(
+    rclcpp::Node::SharedPtr node, const std::string & topic,
+    rclcpp::QoS qos, const std::string & type)
   {
+    type_ = type;
+
     node_ = node;
     publisher_ = node->create_publisher<T>(topic, qos);
   }
@@ -93,13 +98,20 @@ class QR2Publisher : public QR2PublisherBase
     publisher_->publish(msg);
   }
 
-  void publish(typename T::SharedPtr msg) {
-    publisher_->publish(msg);
+  void publish(typename T::UniquePtr msg)
+  {
+    publisher_->publish(std::move(msg));
+  }
+
+  std::string get_type()
+  {
+    return type_;
   }
 
 protected:
   typename rclcpp::Publisher<T>::SharedPtr publisher_;
   long counter_ {0};
+  std::string type_;
 };
 
 class QR2PublisherFactory
@@ -142,19 +154,19 @@ class QR2PublisherFactory
     std::shared_ptr<QR2PublisherBase> ret;
     if (type == "Vector3") {
       ret = std::make_shared<QR2Publisher<quantif_ros2_interfaces::msg::Vector3>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "Twist") {
       ret = std::make_shared<QR2Publisher<quantif_ros2_interfaces::msg::Twist>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "LaserScan") {
       ret = std::make_shared<QR2Publisher<quantif_ros2_interfaces::msg::LaserScan>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "Image") {
       ret = std::make_shared<QR2Publisher<quantif_ros2_interfaces::msg::Image>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "PointCloud2") {
       ret = std::make_shared<QR2Publisher<quantif_ros2_interfaces::msg::PointCloud2>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else {
       RCLCPP_ERROR(
         node_->get_logger(), "Unsupported type (%s) for topic %s", type.c_str(), id.c_str());

diff --git a/quantif_ros2/include/quantif_ros2/QR2Subscriber.hpp b/quantif_ros2/include/quantif_ros2/QR2Subscriber.hpp
@@ -35,35 +35,47 @@ class QR2SubscriberBase
   virtual ~QR2SubscriberBase() {}
 
   virtual void clear_msgs() = 0;
+  virtual std::string get_type() = 0;
 };
 
 template<class T>
 class QR2Subscriber : public QR2SubscriberBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(QR2Subscriber)
-  
+
   QR2Subscriber(
-    rclcpp::Node::SharedPtr node, const std::string & topic, rclcpp::QoS qos)
+    rclcpp::Node::SharedPtr node, const std::string & topic,
+    rclcpp::QoS qos, const std::string & type)
   {
-    subscriber_ = node->create_subscription<T>(topic, qos,
-      [&] (typename T::UniquePtr msg) {
+    type_ = type;
+
+    subscriber_ = node->create_subscription<T>(
+      topic, qos,
+      [&](typename T::UniquePtr msg) {
         last_msg_ = std::move(msg);
       });
   }
 
-  typename T::UniquePtr get_last_msg()
+  void get_last_msg(typename T::UniquePtr msg)
   {
-    return std::move(last_msg_);
+    msg = std::move(last_msg_);
   }
 
-  void clear_msgs() {
+  void clear_msgs()
+  {
     last_msg_ = nullptr;
   }
 
+  std::string get_type()
+  {
+    return type_;
+  }
+
 protected:
   typename rclcpp::Subscription<T>::SharedPtr subscriber_;
   typename T::UniquePtr last_msg_;
+  std::string type_;
 };
 
 class QR2SubscriberFactory
@@ -106,19 +118,19 @@ class QR2SubscriberFactory
     std::shared_ptr<QR2SubscriberBase> ret;
     if (type == "Vector3") {
       ret = std::make_shared<QR2Subscriber<quantif_ros2_interfaces::msg::Vector3>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "Twist") {
       ret = std::make_shared<QR2Subscriber<quantif_ros2_interfaces::msg::Twist>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "LaserScan") {
       ret = std::make_shared<QR2Subscriber<quantif_ros2_interfaces::msg::LaserScan>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "Image") {
       ret = std::make_shared<QR2Subscriber<quantif_ros2_interfaces::msg::Image>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else if (type == "PointCloud2") {
       ret = std::make_shared<QR2Subscriber<quantif_ros2_interfaces::msg::PointCloud2>>(
-        node_, topic, qos);
+        node_, topic, qos, type);
     } else {
       RCLCPP_ERROR(
         node_->get_logger(), "Unsupported type (%s) for topic %s", type.c_str(), id.c_str());

diff --git a/quantif_ros2/package.xml b/quantif_ros2/package.xml
@@ -14,9 +14,11 @@
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
   <depend>quantif_ros2_interfaces</depend>
+  <depend>ament_index_cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/quantif_ros2/src/quantif_ros2/QR2Node.cpp b/quantif_ros2/src/quantif_ros2/QR2Node.cpp
@@ -82,6 +82,26 @@ QR2Node::init()
 }
 
 
+template<class T>
+void get_and_publish(
+  std::shared_ptr<QR2SubscriberBase> sub, const std::string & type,
+  std::vector<std::shared_ptr<QR2PublisherBase>> & publishers)
+{
+  typename T::UniquePtr msg;
+  auto typed_sub = std::dynamic_pointer_cast<QR2Subscriber<T>>(sub);
+  typed_sub->get_last_msg(std::move(msg));
+
+  if (msg != nullptr) {
+    for (auto & publisher : publishers) {
+      if (publisher->get_type() == type) {
+        auto typed_pub = std::dynamic_pointer_cast<QR2Publisher<T>>(publisher);
+        typed_pub->publish(std::move(msg));
+        break;
+      }
+    }
+  }
+}
+
 void
 QR2Node::control_cycle()
 {
@@ -90,12 +110,54 @@ QR2Node::control_cycle()
     for (auto & publisher : publishers_) {
       publisher->produce_and_publish();
     }
+  }
+
+  if (type_ == "Processor") {
+    for (auto & publisher : publishers_) {
+      publisher->produce_and_publish();
+    }
     for (auto & subscriber : subscribers_) {
-      // subscriber->produce_and_publish();
+      subscriber->clear_msgs();
     }
   }
 
-  while ((now() - start).seconds() < processing_time_) {};
+  if (type_ == "Filter") {
+    for (auto & subscriber : subscribers_) {
+      auto type_sub = subscriber->get_type();
+
+      if (type_sub == "Vector3") {
+        get_and_publish<quantif_ros2_interfaces::msg::Vector3>(
+          subscriber, type_sub, publishers_);
+      }
+      if (type_sub == "Twist") {
+        get_and_publish<quantif_ros2_interfaces::msg::Twist>(
+          subscriber, type_sub, publishers_);
+      }
+      if (type_sub == "LaserScan") {
+        get_and_publish<quantif_ros2_interfaces::msg::LaserScan>(
+          subscriber, type_sub, publishers_);
+      }
+      if (type_sub == "Image") {
+        get_and_publish<quantif_ros2_interfaces::msg::Image>(
+          subscriber, type_sub, publishers_);
+      }
+      if (type_sub == "PointCloud2") {
+        get_and_publish<quantif_ros2_interfaces::msg::PointCloud2>(
+          subscriber, type_sub, publishers_);
+      }
+
+      subscriber->clear_msgs();
+    }
+  }
+
+  if (type_ == "Consumer") {
+    for (auto & subscriber : subscribers_) {
+      subscriber->clear_msgs();
+    }
+  }
+
+
+  while ((now() - start).seconds() < processing_time_) {}
 }
 
 }  // namespace quantif_ros2
diff --git a/quantif_ros2/src/small_arq_main.cpp b/quantif_ros2/src/small_arq_main.cpp
@@ -31,7 +31,7 @@ int main(int argc, char * argv[])
     "obstacle_detector",
     "controller",
     "mobile_base",
-   };
+  };
 
   for (const auto & id : node_ids) {
     auto node = quantif_ros2::QR2Node::make_shared(id);

diff --git a/quantif_ros2/tests/CMakeLists.txt b/quantif_ros2/tests/CMakeLists.txt
@@ -0,0 +1,4 @@
+
+ament_add_gtest(qr2node_tests_1 qr2node_tests_1.cpp)
+ament_target_dependencies(qr2node_tests_1 ${dependencies})
+target_link_libraries(qr2node_tests_1 ${PROJECT_NAME})