test: add E2E test for 1pub-1sub with ros2sub (#335)

* test: add system test for 1to1 with ros2sub

Signed-off-by: atsushi421 <atsushi.yano.2@tier4.jp>

* rename

* fix

* fix: convert to composable nodes

Signed-off-by: atsushi421 <atsushi.yano.2@tier4.jp>

* fix

* fix

* fix

* fix

* fix: add flush

* chore: add test info

* refactor: delay

* fix

* fix

* fix

* fix and refactor script

* fix expected num

* fix: to assert the number of received messages

* refactor

* fix

* Update src/e2e_test/test/test_1to1_with_ros2sub.py

Co-authored-by: Koichi Imai <45482193+Koichi98@users.noreply.github.com>

* Update src/e2e_test/test/test_1to1_with_ros2sub.py

Co-authored-by: Koichi Imai <45482193+Koichi98@users.noreply.github.com>

* fix: to use rclcpp_components

* fix workflow

* fix: PR template

---------

Signed-off-by: atsushi421 <atsushi.yano.2@tier4.jp>
Co-authored-by: Koichi Imai <45482193+Koichi98@users.noreply.github.com>

.github/pull_request_template.md

@@ -6,5 +6,6 @@
 
 - [ ] sample application (required)
 - [ ] Autoware (required)
+- [ ] `bash scripts/e2e_test_1to1_with_ros2sub`
 
 ## Notes for reviewers

.github/workflows/build-test.yaml

@@ -25,7 +25,7 @@ jobs:
     - name: Check for .cpp or .hpp file changes
       id: check_changes_cpp
       run: |
-        if git diff --name-only ${{ github.event.pull_request.base.sha }} ${{ github.event.pull_request.head.sha }} | grep -E '\.cpp$|\.hpp$'; then
+        if git diff --name-only ${{ github.event.pull_request.base.sha }} ${{ github.event.pull_request.head.sha }} | grep -E 'agnocastlib/.*\.(cpp|hpp)$'; then
           echo ".cpp or .hpp files changed"
           echo "cpp_changed=true" >> $GITHUB_OUTPUT
         else

.gitignore

@@ -5,3 +5,4 @@ log
 coverage_report_agnocastlib
 .vscode
 agnocast_kmod_coverage_report
+*.pyc

scripts/e2e_test_1to1_with_ros2sub

@@ -0,0 +1,117 @@
+#!/bin/bash
+
+# Parsing arguments
+OPTIONS=$(getopt -o hsc --long help,single,continue -- "$@")
+if [ $? -ne 0 ]; then
+    echo "Invalid options provided"
+    exit 1
+fi
+eval set -- "$OPTIONS"
+
+usage() {
+    echo "Usage: $0 [options]"
+    echo "Options:"
+    echo "  -h, --help      Show this help message"
+    echo "  -s, --single    Run only one test case using current config file"
+    echo "  -c, --continue  Continue running tests even if one fails"
+    exit 0
+}
+
+RUN_SINGLE=false
+CONTINUE_ON_FAILURE=false
+while true; do
+    case "$1" in
+    -h | --help)
+        usage
+        ;;
+    -s | --single)
+        RUN_SINGLE=true
+        shift
+        ;;
+    -c | --continue)
+        CONTINUE_ON_FAILURE=true
+        shift
+        ;;
+    --)
+        shift
+        break
+        ;;
+    *)
+        break
+        ;;
+    esac
+done
+
+# Setup
+rm -rf build/e2e_test install/e2e_test
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install --packages-select e2e_test --cmake-args -DCMAKE_BUILD_TYPE=Release
+source install/setup.bash
+
+# Run test
+CONFIG_FILE="src/e2e_test/test/config_test_1to1_with_ros2sub.yaml"
+show_config() {
+    echo "----------------------------------------------"
+    cat $CONFIG_FILE
+    echo "----------------------------------------------"
+}
+
+FAILURE_COUNT=0
+if [ "$RUN_SINGLE" = true ]; then
+    show_config
+    launch_test src/e2e_test/test/test_1to1_with_ros2sub.py
+    if [ $? -ne 0 ]; then
+        echo "Test failed"
+        exit 1
+    fi
+else
+    LAUNCH_PUB_BEFORE_SUB=(true false)
+    PUB_QOS_DEPTH=(1 10)
+    PUB_TRANSIENT_LOCAL=(true false)
+    SUB_QOS_DEPTH=(1 10)
+    SUB_TRANSIENT_LOCAL=(true false)
+    USE_TAKE_SUB=(true false)
+
+    COUNT=0
+    for launch_pub_before_sub in ${LAUNCH_PUB_BEFORE_SUB[@]}; do
+        for pub_qos_depth in ${PUB_QOS_DEPTH[@]}; do
+            for pub_transient_local in ${PUB_TRANSIENT_LOCAL[@]}; do
+                for sub_qos_depth in ${SUB_QOS_DEPTH[@]}; do
+                    for sub_transient_local in ${SUB_TRANSIENT_LOCAL[@]}; do
+                        for use_take_sub in ${USE_TAKE_SUB[@]}; do
+                            COUNT=$((COUNT + 1))
+                            sed -i "s/launch_pub_before_sub:.*/launch_pub_before_sub: $launch_pub_before_sub/g" $CONFIG_FILE
+                            sed -i "s/pub_qos_depth:.*/pub_qos_depth: $pub_qos_depth/g" $CONFIG_FILE
+                            sed -i "s/pub_transient_local:.*/pub_transient_local: $pub_transient_local/g" $CONFIG_FILE
+                            sed -i "s/sub_qos_depth:.*/sub_qos_depth: $sub_qos_depth/g" $CONFIG_FILE
+                            sed -i "s/sub_transient_local:.*/sub_transient_local: $sub_transient_local/g" $CONFIG_FILE
+                            sed -i "s/use_take_sub:.*/use_take_sub: $use_take_sub/g" $CONFIG_FILE
+                            echo "====================== $COUNT / 64 ======================"
+                            show_config
+                            launch_test src/e2e_test/test/test_1to1_with_ros2sub.py
+
+                            if [ $? -ne 0 ]; then
+                                echo "Test failed."
+                                if [ "$CONTINUE_ON_FAILURE" = true ]; then
+                                    FAILURE_COUNT=$((FAILURE_COUNT + 1))
+                                else
+                                    exit 1
+                                fi
+                            fi
+                        done
+                    done
+                done
+            done
+        done
+    done
+fi
+
+if [ "$FAILURE_COUNT" -gt 0 ]; then
+    echo "$FAILURE_COUNT / $COUNT tests failed"
+    exit 1
+else
+    echo "All tests passed!!"
+fi
+
+# Reset config file
+git checkout -- "$CONFIG_FILE"

src/e2e_test/CMakeLists.txt

@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 3.14)
+project(e2e_test)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(agnocastlib REQUIRED)
+find_package(launch_testing_ament_cmake REQUIRED)
+
+add_library(test_talker_component SHARED src/test_publisher.cpp)
+ament_target_dependencies(test_talker_component rclcpp rclcpp_components std_msgs agnocastlib)
+target_include_directories(test_talker_component PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+rclcpp_components_register_node(
+  test_talker_component
+  PLUGIN "TestPublisher"
+  EXECUTABLE test_talker
+)
+
+add_library(test_listener_component SHARED src/test_subscriber.cpp)
+ament_target_dependencies(test_listener_component rclcpp rclcpp_components std_msgs agnocastlib)
+target_include_directories(test_listener_component PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+rclcpp_components_register_node(
+  test_listener_component
+  PLUGIN "TestSubscriber"
+  EXECUTABLE test_listener
+)
+
+add_library(test_taker_component SHARED src/test_take_subscriber.cpp)
+ament_target_dependencies(test_taker_component rclcpp rclcpp_components std_msgs agnocastlib)
+target_include_directories(test_taker_component PRIVATE
+  ${agnocastlib_INCLUDE_DIRS}
+)
+rclcpp_components_register_node(
+  test_taker_component
+  PLUGIN "TestTakeSubscriber"
+  EXECUTABLE test_taker
+)
+
+add_library(test_ros2_listener_component SHARED src/test_ros2_subscriber.cpp)
+ament_target_dependencies(test_ros2_listener_component rclcpp rclcpp_components std_msgs)
+rclcpp_components_register_node(
+  test_ros2_listener_component
+  PLUGIN "TestROS2Subscriber"
+  EXECUTABLE test_ros2_listener
+)
+
+ament_export_targets(export_test_talker_component)
+install(TARGETS test_talker_component
+        EXPORT export_test_talker_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_test_listener_component)
+install(TARGETS test_listener_component
+        EXPORT export_test_listener_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_test_taker_component)
+install(TARGETS test_taker_component
+        EXPORT export_test_taker_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_test_ros2_listener_component)
+install(TARGETS test_ros2_listener_component
+        EXPORT export_test_ros2_listener_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY test
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/e2e_test/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>e2e_test</name>
+  <version>0.0.0</version>
+  <description>E2E Test</description>
+  <maintainer email="atsushi.yano.2@tier4.jp">atsuhi421</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_component</depend>
+  <depend>std_msgs</depend>
+
+  <depend>agnocastlib</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/e2e_test/src/test_publisher.cpp

@@ -0,0 +1,95 @@
+#include "agnocast.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/int64.hpp"
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+class TestPublisher : public rclcpp::Node
+{
+  rclcpp::TimerBase::SharedPtr timer_;
+  agnocast::Publisher<std_msgs::msg::Int64>::SharedPtr publisher_;
+  int64_t count_;
+  int64_t target_pub_num_;
+  int64_t planned_sub_count_;
+  size_t planned_pub_count_;
+  bool is_ready_ = false;
+
+  bool is_ready()
+  {
+    if (is_ready_) {
+      return true;
+    }
+
+    if (
+      publisher_->get_subscription_count() < planned_sub_count_ ||
+      this->count_publishers("/test_topic") < planned_pub_count_) {
+      return false;
+    }
+
+    sleep(2);  // HACK: wait subscribing transient local messages
+    is_ready_ = true;
+    return true;
+  }
+
+  void timer_callback()
+  {
+    if (!is_ready()) {
+      return;
+    }
+
+    agnocast::ipc_shared_ptr<std_msgs::msg::Int64> message = publisher_->borrow_loaned_message();
+    message->data = count_;
+    publisher_->publish(std::move(message));
+    RCLCPP_INFO(this->get_logger(), "Publishing %ld.", count_);
+    count_++;
+
+    if (count_ == target_pub_num_) {
+      RCLCPP_INFO(this->get_logger(), "All messages published. Shutting down.");
+      std::cout << std::flush;
+      timer_->cancel();
+      rclcpp::shutdown();
+    }
+  }
+
+public:
+  explicit TestPublisher(const rclcpp::NodeOptions & options) : Node("test_publisher", options)
+  {
+    this->declare_parameter<int64_t>("qos_depth", 10);
+    this->declare_parameter<bool>("transient_local", true);
+    this->declare_parameter<int64_t>("init_pub_num", 10);
+    this->declare_parameter<int64_t>("pub_num", 10);
+    this->declare_parameter<int64_t>("planned_sub_count", 1);
+    this->declare_parameter<int64_t>("planned_pub_count", 1);
+    int64_t qos_depth = this->get_parameter("qos_depth").as_int();
+    bool transient_local = this->get_parameter("transient_local").as_bool();
+    int64_t init_pub_num = this->get_parameter("init_pub_num").as_int();
+    int64_t pub_num = this->get_parameter("pub_num").as_int();
+    planned_sub_count_ = this->get_parameter("planned_sub_count").as_int();
+    planned_pub_count_ = static_cast<size_t>(this->get_parameter("planned_pub_count").as_int());
+
+    rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth));
+    if (transient_local) {
+      qos.transient_local();
+    }
+    publisher_ = agnocast::create_publisher<std_msgs::msg::Int64>(this, "/test_topic", qos);
+    count_ = 0;
+    target_pub_num_ = init_pub_num + pub_num;
+
+    // Initial publish
+    for (int i = 0; i < init_pub_num; i++) {
+      agnocast::ipc_shared_ptr<std_msgs::msg::Int64> message = publisher_->borrow_loaned_message();
+      message->data = count_;
+      publisher_->publish(std::move(message));
+      RCLCPP_INFO(this->get_logger(), "Publishing %ld.", count_);
+      count_++;
+    }
+
+    timer_ = this->create_wall_timer(10ms, std::bind(&TestPublisher::timer_callback, this));
+  }
+};
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(TestPublisher)

src/e2e_test/src/test_ros2_subscriber.cpp

@@ -0,0 +1,48 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/int64.hpp"
+
+using std::placeholders::_1;
+
+class TestROS2Subscriber : public rclcpp::Node
+{
+  rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr sub_;
+  int64_t count_;
+  int64_t sub_num_;
+
+  void callback(const std_msgs::msg::Int64 & message)
+  {
+    RCLCPP_INFO(this->get_logger(), "Receiving %ld.", message.data);
+
+    count_++;
+    if (count_ == sub_num_) {
+      RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down.");
+      std::cout << std::flush;
+      rclcpp::shutdown();
+    }
+  }
+
+public:
+  explicit TestROS2Subscriber(const rclcpp::NodeOptions & options)
+  : Node("test_ros2_subscription", options)
+  {
+    this->declare_parameter<int64_t>("qos_depth", 10);
+    this->declare_parameter<bool>("transient_local", true);
+    this->declare_parameter<int64_t>("sub_num", 10);
+    int64_t qos_depth = this->get_parameter("qos_depth").as_int();
+    bool transient_local = this->get_parameter("transient_local").as_bool();
+
+    rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth));
+    if (transient_local) {
+      qos.transient_local();
+    }
+
+    count_ = 0;
+    sub_num_ = this->get_parameter("sub_num").as_int();
+    sub_ = this->create_subscription<std_msgs::msg::Int64>(
+      "/test_topic", qos, std::bind(&TestROS2Subscriber::callback, this, _1));
+  }
+};
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(TestROS2Subscriber)