From 5fef6a39a3bfdd40d3ef93c665ffb65682669e25 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 09:57:59 +0900 Subject: [PATCH 01/24] test: add system test for 1to1 with ros2sub Signed-off-by: atsushi421 --- .gitignore | 1 + scripts/run_e2e_test | 41 +++++ src/e2e_test/CMakeLists.txt | 43 +++++ src/e2e_test/package.xml | 20 +++ src/e2e_test/src/test_publisher.cpp | 80 +++++++++ src/e2e_test/src/test_ros2_subscriber.cpp | 51 ++++++ src/e2e_test/src/test_subscriber.cpp | 55 +++++++ src/e2e_test/src/test_take_subscriber.cpp | 62 +++++++ .../test/config_test_1to1_with_ros2sub.yaml | 6 + src/e2e_test/test/test_1to1_with_ros2sub.py | 154 ++++++++++++++++++ 10 files changed, 513 insertions(+) create mode 100644 scripts/run_e2e_test create mode 100644 src/e2e_test/CMakeLists.txt create mode 100644 src/e2e_test/package.xml create mode 100644 src/e2e_test/src/test_publisher.cpp create mode 100644 src/e2e_test/src/test_ros2_subscriber.cpp create mode 100644 src/e2e_test/src/test_subscriber.cpp create mode 100644 src/e2e_test/src/test_take_subscriber.cpp create mode 100644 src/e2e_test/test/config_test_1to1_with_ros2sub.yaml create mode 100644 src/e2e_test/test/test_1to1_with_ros2sub.py diff --git a/.gitignore b/.gitignore index 6c2c3bfb..ab813a39 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ log coverage_report_agnocastlib .vscode agnocast_kmod_coverage_report +*.pyc diff --git a/scripts/run_e2e_test b/scripts/run_e2e_test new file mode 100644 index 00000000..3c468dfb --- /dev/null +++ b/scripts/run_e2e_test @@ -0,0 +1,41 @@ +#!/bin/bash + +# Setup +rm -rf build/e2e_test install/e2e_test +colcon build --symlink-install --packages-select e2e_test --cmake-args -DCMAKE_BUILD_TYPE=Release +source install/setup.bash + +# Configurations +CONFIG_FILE="src/e2e_test/test/config_test_1to1_with_ros2sub.yaml" +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) + +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 + 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 + launch_test src/e2e_test/test/test_1to1_with_ros2sub.py + + if [ $? -ne 0 ]; then + echo "Test failed" + exit 1 + fi + done + done + done + done + done +done +echo "All tests passed" diff --git a/src/e2e_test/CMakeLists.txt b/src/e2e_test/CMakeLists.txt new file mode 100644 index 00000000..26aeee48 --- /dev/null +++ b/src/e2e_test/CMakeLists.txt @@ -0,0 +1,43 @@ +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(std_msgs REQUIRED) +find_package(agnocastlib REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +add_executable(test_talker src/test_publisher.cpp) +ament_target_dependencies(test_talker rclcpp std_msgs agnocastlib) +target_include_directories(test_talker PRIVATE + ${agnocastlib_INCLUDE_DIRS} +) + +add_executable(test_listener src/test_subscriber.cpp) +ament_target_dependencies(test_listener rclcpp std_msgs agnocastlib) +target_include_directories(test_listener PRIVATE + ${agnocastlib_INCLUDE_DIRS} +) + +add_executable(test_taker src/test_take_subscriber.cpp) +ament_target_dependencies(test_taker rclcpp std_msgs agnocastlib) +target_include_directories(test_taker PRIVATE + ${agnocastlib_INCLUDE_DIRS} +) + +add_executable(test_ros2_listener src/test_ros2_subscriber.cpp) +ament_target_dependencies(test_ros2_listener rclcpp std_msgs) + +install(TARGETS test_talker test_listener test_taker test_ros2_listener + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY test + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/e2e_test/package.xml b/src/e2e_test/package.xml new file mode 100644 index 00000000..eb3ec66d --- /dev/null +++ b/src/e2e_test/package.xml @@ -0,0 +1,20 @@ + + + + e2e_test + 0.0.0 + E2E Test + atsuhi421 + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + + agnocastlib + + + ament_cmake + + diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp new file mode 100644 index 00000000..b23e96f2 --- /dev/null +++ b/src/e2e_test/src/test_publisher.cpp @@ -0,0 +1,80 @@ +#include "agnocast.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/int64.hpp" + +#include + +using namespace std::chrono_literals; + +class TestPublisher : public rclcpp::Node +{ + rclcpp::TimerBase::SharedPtr timer_; + agnocast::Publisher::SharedPtr publisher_; + int64_t count_; + int64_t pub_num_; + + void timer_callback() + { + if (publisher_->get_subscription_count() < 2) { + return; + } + + agnocast::ipc_shared_ptr message = publisher_->borrow_loaned_message(); + message->data = count_; + publisher_->publish(std::move(message)); + RCLCPP_INFO(this->get_logger(), "%ld", count_); + count_++; + + if (count_ == pub_num_) { + RCLCPP_INFO(this->get_logger(), "All messages published. Shutting down."); + timer_->cancel(); + rclcpp::shutdown(); + } + } + +public: + TestPublisher() : Node("test_publisher") + { + this->declare_parameter("qos_depth", 10); + this->declare_parameter("transient_local", true); + this->declare_parameter("init_pub_num", 10); + this->declare_parameter("pub_num", 10); + 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(); + + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth)); + if (transient_local) { + qos.transient_local(); + } + publisher_ = agnocast::create_publisher(this, "/test_topic", qos); + count_ = 0; + pub_num_ = init_pub_num + pub_num; + + // Initial publish + for (int i = 0; i < init_pub_num; i++) { + agnocast::ipc_shared_ptr message = publisher_->borrow_loaned_message(); + message->data = count_; + publisher_->publish(std::move(message)); + RCLCPP_INFO(this->get_logger(), "%ld", count_); + count_++; + } + + timer_ = this->create_wall_timer(10ms, std::bind(&TestPublisher::timer_callback, this)); + sleep(3); // wait for the subscription to be established + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + agnocast::SingleThreadedAgnocastExecutor executor; + executor.add_node(std::make_shared()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/e2e_test/src/test_ros2_subscriber.cpp b/src/e2e_test/src/test_ros2_subscriber.cpp new file mode 100644 index 00000000..590bef10 --- /dev/null +++ b/src/e2e_test/src/test_ros2_subscriber.cpp @@ -0,0 +1,51 @@ +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/int64.hpp" + +using std::placeholders::_1; + +class TestROS2Subscriber : public rclcpp::Node +{ + rclcpp::Subscription::SharedPtr sub_; + int64_t count_; + int64_t sub_num_; + + void callback(const std_msgs::msg::Int64 & message) + { + RCLCPP_INFO(this->get_logger(), "%ld", message.data); + + count_++; + if (count_ == sub_num_) { + RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + rclcpp::shutdown(); + } + } + +public: + TestROS2Subscriber() : Node("test_ros2_subscription") + { + this->declare_parameter("qos_depth", 10); + this->declare_parameter("transient_local", true); + this->declare_parameter("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( + "/test_topic", qos, std::bind(&TestROS2Subscriber::callback, this, _1)); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/e2e_test/src/test_subscriber.cpp b/src/e2e_test/src/test_subscriber.cpp new file mode 100644 index 00000000..c0911d01 --- /dev/null +++ b/src/e2e_test/src/test_subscriber.cpp @@ -0,0 +1,55 @@ +#include "agnocast.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/int64.hpp" + +using std::placeholders::_1; + +class TestSubscriber : public rclcpp::Node +{ + agnocast::Subscription::SharedPtr sub_; + uint64_t count_; + uint64_t sub_num_; + + void callback(const agnocast::ipc_shared_ptr & message) + { + RCLCPP_INFO(this->get_logger(), "%ld", message->data); + + count_++; + if (count_ == sub_num_) { + RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + rclcpp::shutdown(); + } + } + +public: + TestSubscriber() : Node("test_subscription") + { + this->declare_parameter("qos_depth", 10); + this->declare_parameter("transient_local", true); + this->declare_parameter("sub_num", 10); + + int64_t qos_depth = this->get_parameter("qos_depth").as_int(); + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth)); + if (this->get_parameter("transient_local").as_bool()) { + qos.transient_local(); + } + + count_ = 0; + sub_num_ = this->get_parameter("sub_num").as_int(); + sub_ = agnocast::create_subscription( + this, "/test_topic", qos, std::bind(&TestSubscriber::callback, this, _1)); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + agnocast::SingleThreadedAgnocastExecutor executor; + executor.add_node(std::make_shared()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/e2e_test/src/test_take_subscriber.cpp b/src/e2e_test/src/test_take_subscriber.cpp new file mode 100644 index 00000000..cc4dd80d --- /dev/null +++ b/src/e2e_test/src/test_take_subscriber.cpp @@ -0,0 +1,62 @@ +#include "agnocast.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/int64.hpp" + +#include +using namespace std::chrono_literals; + +class TestTakeSubscriber : public rclcpp::Node +{ + rclcpp::TimerBase::SharedPtr timer_; + agnocast::TakeSubscription::SharedPtr sub_; + uint64_t count_; + uint64_t sub_num_; + + void timer_callback() + { + auto message = sub_->take(); + if (message) { + RCLCPP_INFO(this->get_logger(), "%ld", message->data); + count_++; + } + + if (count_ == sub_num_) { + RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + timer_->cancel(); + rclcpp::shutdown(); + } + } + +public: + TestTakeSubscriber() : Node("test_take_subscription") + { + this->declare_parameter("qos_depth", 10); + this->declare_parameter("transient_local", true); + this->declare_parameter("sub_num", 10); + + int64_t qos_depth = this->get_parameter("qos_depth").as_int(); + rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth)); + if (this->get_parameter("transient_local").as_bool()) { + qos.transient_local(); + } + + count_ = 0; + sub_num_ = this->get_parameter("sub_num").as_int(); + sub_ = + std::make_shared>(this, "/test_topic", qos); + timer_ = this->create_wall_timer(10ms, std::bind(&TestTakeSubscriber::timer_callback, this)); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + agnocast::SingleThreadedAgnocastExecutor executor; + executor.add_node(std::make_shared()); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml new file mode 100644 index 00000000..8570b173 --- /dev/null +++ b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml @@ -0,0 +1,6 @@ +launch_pub_before_sub: true +pub_qos_depth: 1 +pub_transient_local: true +sub_qos_depth: 10 +sub_transient_local: false +use_take_sub: false diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py new file mode 100644 index 00000000..7a0b19f9 --- /dev/null +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -0,0 +1,154 @@ +import os +import unittest + +import launch_testing +import launch_testing.asserts +import launch_testing.markers +import yaml +from launch import LaunchDescription +from launch.actions import TimerAction +from launch_ros.actions import Node + +CONFIG_PATH = os.path.join(os.path.dirname(__file__), 'config_test_1to1_with_ros2sub.yaml') + +EXPECT_INIT_PUB_NUM: int +EXPECT_PUB_NUM: int +EXPECT_SUB_NUM: int +EXPECT_ROS2_SUB_NUM: int + + +def calc_expect_pub_sub_num(config: dict) -> None: + global EXPECT_PUB_NUM, EXPECT_INIT_PUB_NUM, EXPECT_SUB_NUM, EXPECT_ROS2_SUB_NUM + + EXPECT_INIT_PUB_NUM = config['pub_qos_depth'] if config['launch_pub_before_sub'] and config['pub_transient_local'] else 0 + EXPECT_PUB_NUM = config['pub_qos_depth'] + + base_num = min(EXPECT_PUB_NUM, config['sub_qos_depth']) + if config['launch_pub_before_sub'] and config['sub_transient_local']: + EXPECT_ROS2_SUB_NUM = base_num * 2 + EXPECT_SUB_NUM = base_num if config['use_take_sub'] else base_num * 2 + else: + EXPECT_ROS2_SUB_NUM = base_num + EXPECT_SUB_NUM = base_num + + +@launch_testing.markers.keep_alive +def generate_test_description(): + with open(CONFIG_PATH, 'r') as f: + config = yaml.safe_load(f) + calc_expect_pub_sub_num(config) + + pub_node = TimerAction( + period=0.0 if config['launch_pub_before_sub'] else 1.0, + actions=[ + Node( + package='e2e_test', + executable='test_talker', + name='test_talker_node', + parameters=[ + { + "qos_depth": config['pub_qos_depth'], + "transient_local": config['pub_transient_local'], + "init_pub_num": EXPECT_INIT_PUB_NUM, + "pub_num": EXPECT_PUB_NUM + } + ], + output="screen", + additional_env={ + 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", + 'MEMPOOL_SIZE': '134217728', + } + ) + ] + ) + + sub_nodes_actions = [ + Node( + package='e2e_test', executable='test_ros2_listener', + name='test_ros2_listener_node', + parameters=[{"qos_depth": config['sub_qos_depth'], + "transient_local": config + ['sub_transient_local'] + if config['pub_transient_local'] else False, + "sub_num": EXPECT_ROS2_SUB_NUM}], + output="screen",)] + if config['use_take_sub']: + sub_nodes_actions.append( + Node( + package='e2e_test', + executable='test_taker', + name='test_taker_node', + parameters=[ + { + "qos_depth": config['sub_qos_depth'], + "transient_local": config['sub_transient_local'], + "sub_num": EXPECT_SUB_NUM + } + ], + output="screen", + additional_env={ + 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", + 'MEMPOOL_SIZE': '134217728', + } + ) + ) + else: + sub_nodes_actions.append( + Node( + package='e2e_test', + executable='test_listener', + name='test_listener_node', + parameters=[ + { + "qos_depth": config['sub_qos_depth'], + "transient_local": config['sub_transient_local'], + "sub_num": EXPECT_SUB_NUM + } + ], + output="screen", + additional_env={ + 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", + 'MEMPOOL_SIZE': '134217728', + } + ) + ) + + sub_nodes = TimerAction( + period=1.0 if config['launch_pub_before_sub'] else 0.0, + actions=sub_nodes_actions + ) + + return ( + LaunchDescription( + [pub_node, + sub_nodes, + TimerAction(period=5.0, actions=[launch_testing.actions.ReadyToTest()])] + ), + { + 'test_pub': pub_node.actions[0], + 'test_ros2_sub': sub_nodes.actions[0], + 'test_sub': sub_nodes.actions[1], + } + ) + + +@ launch_testing.post_shutdown_test() +class Test1To1(unittest.TestCase): + + def test_pub(self, proc_output, test_pub): + with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_pub) as cm: + for i in range(EXPECT_INIT_PUB_NUM + EXPECT_PUB_NUM): + cm.assertInStdout(f"{i}") + cm.assertInStdout("All messages published. Shutting down.") + + def test_sub(self, proc_output, test_sub): + with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_sub) as cm: + for i in range(EXPECT_SUB_NUM): + cm.assertInStdout(f"{i}") + cm.assertInStdout("All messages received. Shutting down.") + + def test_ros2_sub(self, proc_output, test_ros2_sub): + with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_ros2_sub) as cm: + for i in range(EXPECT_ROS2_SUB_NUM): + cm.assertInStdout(f"{i}") + cm.assertInStdout("All messages received. Shutting down.") From 5d2cf78d7cc06ffc6065f1a52f16d110c8b9379c Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 10:01:11 +0900 Subject: [PATCH 02/24] rename --- scripts/{run_e2e_test => e2e_test_1to1_with_ros2sub} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename scripts/{run_e2e_test => e2e_test_1to1_with_ros2sub} (100%) diff --git a/scripts/run_e2e_test b/scripts/e2e_test_1to1_with_ros2sub similarity index 100% rename from scripts/run_e2e_test rename to scripts/e2e_test_1to1_with_ros2sub From 78bfb9552e1bfdb15be47368c571645cb7a2acae Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 10:15:14 +0900 Subject: [PATCH 03/24] fix --- src/e2e_test/src/test_publisher.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index b23e96f2..c7120fc7 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -16,10 +16,6 @@ class TestPublisher : public rclcpp::Node void timer_callback() { - if (publisher_->get_subscription_count() < 2) { - return; - } - agnocast::ipc_shared_ptr message = publisher_->borrow_loaned_message(); message->data = count_; publisher_->publish(std::move(message)); @@ -62,8 +58,12 @@ class TestPublisher : public rclcpp::Node count_++; } + // wait for the subscription to be established + while (publisher_->get_subscription_count() < 2) { + sleep(1); + } + timer_ = this->create_wall_timer(10ms, std::bind(&TestPublisher::timer_callback, this)); - sleep(3); // wait for the subscription to be established } }; From d0256a0cc6f797cd9c5251baec99876255eb9455 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 14:17:55 +0900 Subject: [PATCH 04/24] fix: convert to composable nodes Signed-off-by: atsushi421 --- src/e2e_test/CMakeLists.txt | 76 +++++++++-- src/e2e_test/package.xml | 1 + src/e2e_test/src/test_publisher.cpp | 16 +-- src/e2e_test/src/test_ros2_subscriber.cpp | 12 +- src/e2e_test/src/test_subscriber.cpp | 15 +-- src/e2e_test/src/test_take_subscriber.cpp | 16 +-- .../test/config_test_1to1_with_ros2sub.yaml | 6 +- src/e2e_test/test/test_1to1_with_ros2sub.py | 126 ++++++++++++------ 8 files changed, 164 insertions(+), 104 deletions(-) diff --git a/src/e2e_test/CMakeLists.txt b/src/e2e_test/CMakeLists.txt index 26aeee48..bb089543 100644 --- a/src/e2e_test/CMakeLists.txt +++ b/src/e2e_test/CMakeLists.txt @@ -8,33 +8,83 @@ 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_executable(test_talker src/test_publisher.cpp) -ament_target_dependencies(test_talker rclcpp std_msgs agnocastlib) -target_include_directories(test_talker PRIVATE +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_executable(test_listener src/test_subscriber.cpp) -ament_target_dependencies(test_listener rclcpp std_msgs agnocastlib) -target_include_directories(test_listener PRIVATE +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_executable(test_taker src/test_take_subscriber.cpp) -ament_target_dependencies(test_taker rclcpp std_msgs agnocastlib) -target_include_directories(test_taker PRIVATE +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_executable(test_ros2_listener src/test_ros2_subscriber.cpp) -ament_target_dependencies(test_ros2_listener rclcpp std_msgs) +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 +) -install(TARGETS test_talker test_listener test_taker test_ros2_listener - DESTINATION lib/${PROJECT_NAME}) +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}/ diff --git a/src/e2e_test/package.xml b/src/e2e_test/package.xml index eb3ec66d..6031da48 100644 --- a/src/e2e_test/package.xml +++ b/src/e2e_test/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + rclcpp_component std_msgs agnocastlib diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index c7120fc7..6cd7ded5 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -30,7 +30,7 @@ class TestPublisher : public rclcpp::Node } public: - TestPublisher() : Node("test_publisher") + explicit TestPublisher(const rclcpp::NodeOptions & options) : Node("test_publisher", options) { this->declare_parameter("qos_depth", 10); this->declare_parameter("transient_local", true); @@ -62,19 +62,11 @@ class TestPublisher : public rclcpp::Node while (publisher_->get_subscription_count() < 2) { sleep(1); } + sleep(2); // HACK: wait subscribing transient local messages timer_ = this->create_wall_timer(10ms, std::bind(&TestPublisher::timer_callback, this)); } }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - agnocast::SingleThreadedAgnocastExecutor executor; - executor.add_node(std::make_shared()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TestPublisher) diff --git a/src/e2e_test/src/test_ros2_subscriber.cpp b/src/e2e_test/src/test_ros2_subscriber.cpp index 590bef10..836e54ce 100644 --- a/src/e2e_test/src/test_ros2_subscriber.cpp +++ b/src/e2e_test/src/test_ros2_subscriber.cpp @@ -22,7 +22,8 @@ class TestROS2Subscriber : public rclcpp::Node } public: - TestROS2Subscriber() : Node("test_ros2_subscription") + explicit TestROS2Subscriber(const rclcpp::NodeOptions & options) + : Node("test_ros2_subscription", options) { this->declare_parameter("qos_depth", 10); this->declare_parameter("transient_local", true); @@ -42,10 +43,5 @@ class TestROS2Subscriber : public rclcpp::Node } }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TestROS2Subscriber) diff --git a/src/e2e_test/src/test_subscriber.cpp b/src/e2e_test/src/test_subscriber.cpp index c0911d01..869ebb35 100644 --- a/src/e2e_test/src/test_subscriber.cpp +++ b/src/e2e_test/src/test_subscriber.cpp @@ -23,7 +23,7 @@ class TestSubscriber : public rclcpp::Node } public: - TestSubscriber() : Node("test_subscription") + explicit TestSubscriber(const rclcpp::NodeOptions & options) : Node("test_subscription", options) { this->declare_parameter("qos_depth", 10); this->declare_parameter("transient_local", true); @@ -42,14 +42,5 @@ class TestSubscriber : public rclcpp::Node } }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - agnocast::SingleThreadedAgnocastExecutor executor; - executor.add_node(std::make_shared()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TestSubscriber) diff --git a/src/e2e_test/src/test_take_subscriber.cpp b/src/e2e_test/src/test_take_subscriber.cpp index cc4dd80d..7aadcd86 100644 --- a/src/e2e_test/src/test_take_subscriber.cpp +++ b/src/e2e_test/src/test_take_subscriber.cpp @@ -29,7 +29,8 @@ class TestTakeSubscriber : public rclcpp::Node } public: - TestTakeSubscriber() : Node("test_take_subscription") + explicit TestTakeSubscriber(const rclcpp::NodeOptions & options) + : Node("test_take_subscription", options) { this->declare_parameter("qos_depth", 10); this->declare_parameter("transient_local", true); @@ -49,14 +50,5 @@ class TestTakeSubscriber : public rclcpp::Node } }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - agnocast::SingleThreadedAgnocastExecutor executor; - executor.add_node(std::make_shared()); - executor.spin(); - - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TestTakeSubscriber) diff --git a/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml index 8570b173..47a474f3 100644 --- a/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml +++ b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml @@ -1,6 +1,6 @@ -launch_pub_before_sub: true -pub_qos_depth: 1 -pub_transient_local: true +launch_pub_before_sub: false +pub_qos_depth: 10 +pub_transient_local: false sub_qos_depth: 10 sub_transient_local: false use_take_sub: false diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 7a0b19f9..ccefec84 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -7,7 +7,8 @@ import yaml from launch import LaunchDescription from launch.actions import TimerAction -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode CONFIG_PATH = os.path.join(os.path.dirname(__file__), 'config_test_1to1_with_ros2sub.yaml') @@ -41,19 +42,27 @@ def generate_test_description(): pub_node = TimerAction( period=0.0 if config['launch_pub_before_sub'] else 1.0, actions=[ - Node( - package='e2e_test', - executable='test_talker', - name='test_talker_node', - parameters=[ - { - "qos_depth": config['pub_qos_depth'], - "transient_local": config['pub_transient_local'], - "init_pub_num": EXPECT_INIT_PUB_NUM, - "pub_num": EXPECT_PUB_NUM - } + ComposableNodeContainer( + name='test_talker_container', + namespace='', + package='agnocastlib', + executable='agnocast_component_container', + composable_node_descriptions=[ + ComposableNode( + package='e2e_test', + plugin='TestPublisher', + name='test_talker_node', + parameters=[ + { + "qos_depth": config['pub_qos_depth'], + "transient_local": config['pub_transient_local'], + "init_pub_num": EXPECT_INIT_PUB_NUM, + "pub_num": EXPECT_PUB_NUM + } + ], + ) ], - output="screen", + output='screen', additional_env={ 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", 'MEMPOOL_SIZE': '134217728', @@ -63,29 +72,50 @@ def generate_test_description(): ) sub_nodes_actions = [ - Node( - package='e2e_test', executable='test_ros2_listener', - name='test_ros2_listener_node', - parameters=[{"qos_depth": config['sub_qos_depth'], - "transient_local": config - ['sub_transient_local'] - if config['pub_transient_local'] else False, - "sub_num": EXPECT_ROS2_SUB_NUM}], - output="screen",)] + ComposableNodeContainer( + name='test_ros2_lister_container', + namespace='', + package='agnocastlib', + executable='agnocast_component_container', + composable_node_descriptions=[ + ComposableNode( + package='e2e_test', + plugin='TestROS2Subscriber', + name='test_ros2_listener_node', + parameters=[ + {"qos_depth": config['sub_qos_depth'], + "transient_local": config + ['sub_transient_local'] + if config['pub_transient_local'] else False, + "sub_num": EXPECT_ROS2_SUB_NUM} + ], + ) + ], + output='screen', + ) + ] if config['use_take_sub']: sub_nodes_actions.append( - Node( - package='e2e_test', - executable='test_taker', - name='test_taker_node', - parameters=[ - { - "qos_depth": config['sub_qos_depth'], - "transient_local": config['sub_transient_local'], - "sub_num": EXPECT_SUB_NUM - } + ComposableNodeContainer( + name='test_taker_container', + namespace='', + package='agnocastlib', + executable='agnocast_component_container', + composable_node_descriptions=[ + ComposableNode( + package='e2e_test', + plugin='TestTakeSubscriber', + name='test_taker_node', + parameters=[ + { + "qos_depth": config['sub_qos_depth'], + "transient_local": config['sub_transient_local'], + "sub_num": EXPECT_SUB_NUM + } + ], + ) ], - output="screen", + output='screen', additional_env={ 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", 'MEMPOOL_SIZE': '134217728', @@ -94,18 +124,26 @@ def generate_test_description(): ) else: sub_nodes_actions.append( - Node( - package='e2e_test', - executable='test_listener', - name='test_listener_node', - parameters=[ - { - "qos_depth": config['sub_qos_depth'], - "transient_local": config['sub_transient_local'], - "sub_num": EXPECT_SUB_NUM - } + ComposableNodeContainer( + name='test_lister_container', + namespace='', + package='agnocastlib', + executable='agnocast_component_container', + composable_node_descriptions=[ + ComposableNode( + package='e2e_test', + plugin='TestSubscriber', + name='test_listener_node', + parameters=[ + { + "qos_depth": config['sub_qos_depth'], + "transient_local": config['sub_transient_local'], + "sub_num": EXPECT_SUB_NUM + } + ], + ) ], - output="screen", + output='screen', additional_env={ 'LD_PRELOAD': f"libagnocast_heaphook.so:{os.getenv('LD_PRELOAD', '')}", 'MEMPOOL_SIZE': '134217728', From 555c2608d48bf1cfedfbd86a933117ca0d952205 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 14:34:25 +0900 Subject: [PATCH 05/24] fix --- src/e2e_test/src/test_publisher.cpp | 4 ++-- src/e2e_test/src/test_ros2_subscriber.cpp | 2 +- src/e2e_test/src/test_subscriber.cpp | 2 +- src/e2e_test/src/test_take_subscriber.cpp | 2 +- src/e2e_test/test/test_1to1_with_ros2sub.py | 6 +++--- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index 6cd7ded5..c15f7e5f 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -19,7 +19,7 @@ class TestPublisher : public rclcpp::Node agnocast::ipc_shared_ptr message = publisher_->borrow_loaned_message(); message->data = count_; publisher_->publish(std::move(message)); - RCLCPP_INFO(this->get_logger(), "%ld", count_); + RCLCPP_INFO(this->get_logger(), "Publishing %ld.", count_); count_++; if (count_ == pub_num_) { @@ -54,7 +54,7 @@ class TestPublisher : public rclcpp::Node agnocast::ipc_shared_ptr message = publisher_->borrow_loaned_message(); message->data = count_; publisher_->publish(std::move(message)); - RCLCPP_INFO(this->get_logger(), "%ld", count_); + RCLCPP_INFO(this->get_logger(), "Publishing %ld.", count_); count_++; } diff --git a/src/e2e_test/src/test_ros2_subscriber.cpp b/src/e2e_test/src/test_ros2_subscriber.cpp index 836e54ce..9d862238 100644 --- a/src/e2e_test/src/test_ros2_subscriber.cpp +++ b/src/e2e_test/src/test_ros2_subscriber.cpp @@ -12,7 +12,7 @@ class TestROS2Subscriber : public rclcpp::Node void callback(const std_msgs::msg::Int64 & message) { - RCLCPP_INFO(this->get_logger(), "%ld", message.data); + RCLCPP_INFO(this->get_logger(), "Receiving %ld.", message.data); count_++; if (count_ == sub_num_) { diff --git a/src/e2e_test/src/test_subscriber.cpp b/src/e2e_test/src/test_subscriber.cpp index 869ebb35..c653d66d 100644 --- a/src/e2e_test/src/test_subscriber.cpp +++ b/src/e2e_test/src/test_subscriber.cpp @@ -13,7 +13,7 @@ class TestSubscriber : public rclcpp::Node void callback(const agnocast::ipc_shared_ptr & message) { - RCLCPP_INFO(this->get_logger(), "%ld", message->data); + RCLCPP_INFO(this->get_logger(), "Receiving %ld.", message->data); count_++; if (count_ == sub_num_) { diff --git a/src/e2e_test/src/test_take_subscriber.cpp b/src/e2e_test/src/test_take_subscriber.cpp index 7aadcd86..c01f6e22 100644 --- a/src/e2e_test/src/test_take_subscriber.cpp +++ b/src/e2e_test/src/test_take_subscriber.cpp @@ -17,7 +17,7 @@ class TestTakeSubscriber : public rclcpp::Node { auto message = sub_->take(); if (message) { - RCLCPP_INFO(this->get_logger(), "%ld", message->data); + RCLCPP_INFO(this->get_logger(), "Receiving %ld.", message->data); count_++; } diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index ccefec84..3a914d1e 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -176,17 +176,17 @@ class Test1To1(unittest.TestCase): def test_pub(self, proc_output, test_pub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_pub) as cm: for i in range(EXPECT_INIT_PUB_NUM + EXPECT_PUB_NUM): - cm.assertInStdout(f"{i}") + cm.assertInStdout(f"Publishing {i}.") cm.assertInStdout("All messages published. Shutting down.") def test_sub(self, proc_output, test_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_sub) as cm: for i in range(EXPECT_SUB_NUM): - cm.assertInStdout(f"{i}") + cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") def test_ros2_sub(self, proc_output, test_ros2_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_ros2_sub) as cm: for i in range(EXPECT_ROS2_SUB_NUM): - cm.assertInStdout(f"{i}") + cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") From 10ef254479a813ec8afbb41de974f1ed5e744981 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 15:18:08 +0900 Subject: [PATCH 06/24] fix --- src/e2e_test/test/test_1to1_with_ros2sub.py | 29 ++++++++++++--------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 3a914d1e..53c0450f 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -14,23 +14,28 @@ EXPECT_INIT_PUB_NUM: int EXPECT_PUB_NUM: int +EXPECT_INIT_SUB_NUM: int EXPECT_SUB_NUM: int +EXPECT_INIT_ROS2_SUB_NUM: int EXPECT_ROS2_SUB_NUM: int def calc_expect_pub_sub_num(config: dict) -> None: - global EXPECT_PUB_NUM, EXPECT_INIT_PUB_NUM, EXPECT_SUB_NUM, EXPECT_ROS2_SUB_NUM + global EXPECT_PUB_NUM, EXPECT_INIT_PUB_NUM, EXPECT_INIT_SUB_NUM, EXPECT_SUB_NUM, EXPECT_INIT_ROS2_SUB_NUM, EXPECT_ROS2_SUB_NUM - EXPECT_INIT_PUB_NUM = config['pub_qos_depth'] if config['launch_pub_before_sub'] and config['pub_transient_local'] else 0 + EXPECT_INIT_PUB_NUM = config['pub_qos_depth'] if ( + config['launch_pub_before_sub'] and config['pub_transient_local']) else 0 EXPECT_PUB_NUM = config['pub_qos_depth'] - base_num = min(EXPECT_PUB_NUM, config['sub_qos_depth']) + base_sub_num = min(EXPECT_PUB_NUM, config['sub_qos_depth']) + EXPECT_ROS2_SUB_NUM = base_sub_num + EXPECT_SUB_NUM = base_sub_num if config['launch_pub_before_sub'] and config['sub_transient_local']: - EXPECT_ROS2_SUB_NUM = base_num * 2 - EXPECT_SUB_NUM = base_num if config['use_take_sub'] else base_num * 2 + EXPECT_INIT_ROS2_SUB_NUM = base_sub_num + EXPECT_INIT_SUB_NUM = 0 if config['use_take_sub'] else base_sub_num else: - EXPECT_ROS2_SUB_NUM = base_num - EXPECT_SUB_NUM = base_num + EXPECT_INIT_ROS2_SUB_NUM = 0 + EXPECT_INIT_SUB_NUM = 0 @launch_testing.markers.keep_alive @@ -87,7 +92,7 @@ def generate_test_description(): "transient_local": config ['sub_transient_local'] if config['pub_transient_local'] else False, - "sub_num": EXPECT_ROS2_SUB_NUM} + "sub_num": EXPECT_INIT_ROS2_SUB_NUM + EXPECT_ROS2_SUB_NUM} ], ) ], @@ -110,7 +115,7 @@ def generate_test_description(): { "qos_depth": config['sub_qos_depth'], "transient_local": config['sub_transient_local'], - "sub_num": EXPECT_SUB_NUM + "sub_num": EXPECT_INIT_SUB_NUM + EXPECT_SUB_NUM } ], ) @@ -138,7 +143,7 @@ def generate_test_description(): { "qos_depth": config['sub_qos_depth'], "transient_local": config['sub_transient_local'], - "sub_num": EXPECT_SUB_NUM + "sub_num": EXPECT_INIT_SUB_NUM + EXPECT_SUB_NUM } ], ) @@ -181,12 +186,12 @@ def test_pub(self, proc_output, test_pub): def test_sub(self, proc_output, test_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_sub) as cm: - for i in range(EXPECT_SUB_NUM): + for i in range(EXPECT_INIT_PUB_NUM - EXPECT_INIT_SUB_NUM, EXPECT_SUB_NUM): cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") def test_ros2_sub(self, proc_output, test_ros2_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_ros2_sub) as cm: - for i in range(EXPECT_ROS2_SUB_NUM): + for i in range(EXPECT_INIT_PUB_NUM - EXPECT_INIT_ROS2_SUB_NUM, EXPECT_ROS2_SUB_NUM): cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") From 38bd1bf5fe955a2a49a0fc516d359f8ff86886b3 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 15:28:41 +0900 Subject: [PATCH 07/24] fix --- scripts/e2e_test_1to1_with_ros2sub | 8 ++++++++ src/e2e_test/test/config_test_1to1_with_ros2sub.yaml | 8 ++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index 3c468dfb..ecb26862 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -39,3 +39,11 @@ for LAUNCH_PUB_BEFORE_SUB in ${LAUNCH_PUB_BEFORE_SUB[@]}; do done done echo "All tests passed" + +# Reset config file +sed -i "s/launch_pub_before_sub:.*/launch_pub_before_sub: true/g" $CONFIG_FILE +sed -i "s/pub_qos_depth:.*/pub_qos_depth: 10/g" $CONFIG_FILE +sed -i "s/pub_transient_local:.*/pub_transient_local: true/g" $CONFIG_FILE +sed -i "s/sub_qos_depth:.*/sub_qos_depth: 10/g" $CONFIG_FILE +sed -i "s/sub_transient_local:.*/sub_transient_local: true/g" $CONFIG_FILE +sed -i "s/use_take_sub:.*/use_take_sub: true/g" $CONFIG_FILE diff --git a/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml index 47a474f3..fdeef329 100644 --- a/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml +++ b/src/e2e_test/test/config_test_1to1_with_ros2sub.yaml @@ -1,6 +1,6 @@ -launch_pub_before_sub: false +launch_pub_before_sub: true pub_qos_depth: 10 -pub_transient_local: false +pub_transient_local: true sub_qos_depth: 10 -sub_transient_local: false -use_take_sub: false +sub_transient_local: true +use_take_sub: true From f2c54d87b4e6b72c6ae15348d4009de0fd520428 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 16:19:09 +0900 Subject: [PATCH 08/24] fix --- src/e2e_test/test/test_1to1_with_ros2sub.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 53c0450f..b31a744c 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -6,7 +6,7 @@ import launch_testing.markers import yaml from launch import LaunchDescription -from launch.actions import TimerAction +from launch.actions import SetEnvironmentVariable, TimerAction from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -163,9 +163,12 @@ def generate_test_description(): return ( LaunchDescription( - [pub_node, - sub_nodes, - TimerAction(period=5.0, actions=[launch_testing.actions.ReadyToTest()])] + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '0'), + pub_node, + sub_nodes, + TimerAction(period=5.0, actions=[launch_testing.actions.ReadyToTest()]) + ] ), { 'test_pub': pub_node.actions[0], @@ -175,7 +178,7 @@ def generate_test_description(): ) -@ launch_testing.post_shutdown_test() +@launch_testing.post_shutdown_test() class Test1To1(unittest.TestCase): def test_pub(self, proc_output, test_pub): From 86e4016a19ff00f2bd94332be41de34da55ab53a Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 16:40:20 +0900 Subject: [PATCH 09/24] fix: add flush --- src/e2e_test/src/test_publisher.cpp | 1 + src/e2e_test/src/test_ros2_subscriber.cpp | 1 + src/e2e_test/src/test_subscriber.cpp | 1 + src/e2e_test/src/test_take_subscriber.cpp | 1 + 4 files changed, 4 insertions(+) diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index c15f7e5f..8e8212fd 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -24,6 +24,7 @@ class TestPublisher : public rclcpp::Node if (count_ == pub_num_) { RCLCPP_INFO(this->get_logger(), "All messages published. Shutting down."); + std::cout << std::flush; timer_->cancel(); rclcpp::shutdown(); } diff --git a/src/e2e_test/src/test_ros2_subscriber.cpp b/src/e2e_test/src/test_ros2_subscriber.cpp index 9d862238..c7db2f3e 100644 --- a/src/e2e_test/src/test_ros2_subscriber.cpp +++ b/src/e2e_test/src/test_ros2_subscriber.cpp @@ -17,6 +17,7 @@ class TestROS2Subscriber : public rclcpp::Node count_++; if (count_ == sub_num_) { RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + std::cout << std::flush; rclcpp::shutdown(); } } diff --git a/src/e2e_test/src/test_subscriber.cpp b/src/e2e_test/src/test_subscriber.cpp index c653d66d..66e48b38 100644 --- a/src/e2e_test/src/test_subscriber.cpp +++ b/src/e2e_test/src/test_subscriber.cpp @@ -18,6 +18,7 @@ class TestSubscriber : public rclcpp::Node count_++; if (count_ == sub_num_) { RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + std::cout << std::flush; rclcpp::shutdown(); } } diff --git a/src/e2e_test/src/test_take_subscriber.cpp b/src/e2e_test/src/test_take_subscriber.cpp index c01f6e22..b76b02b4 100644 --- a/src/e2e_test/src/test_take_subscriber.cpp +++ b/src/e2e_test/src/test_take_subscriber.cpp @@ -23,6 +23,7 @@ class TestTakeSubscriber : public rclcpp::Node if (count_ == sub_num_) { RCLCPP_INFO(this->get_logger(), "All messages received. Shutting down."); + std::cout << std::flush; timer_->cancel(); rclcpp::shutdown(); } From 12f29ae3118ab2689fb3122753b7e1e2980fd560 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 17:24:41 +0900 Subject: [PATCH 10/24] chore: add test info --- scripts/e2e_test_1to1_with_ros2sub | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index ecb26862..5076d9e1 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -14,12 +14,23 @@ 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)) + echo "=================== $COUNT / 64 ==================" + echo "launch_pub_before_sub: $LAUNCH_PUB_BEFORE_SUB" + echo "pub_qos_depth: $PUB_QOS_DEPTH" + echo "pub_transient_local: $PUB_TRANSIENT_LOCAL" + echo "sub_qos_depth: $SUB_QOS_DEPTH" + echo "sub_transient_local: $SUB_TRANSIENT_LOCAL" + echo "use_take_sub: $USE_TAKE_SUB" + echo "==============================================" + 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 From f5e1333230d4ecb222d2ff342402bdc1f398e88d Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 17:53:13 +0900 Subject: [PATCH 11/24] refactor: delay --- src/e2e_test/test/test_1to1_with_ros2sub.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index b31a744c..4c514363 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -38,14 +38,23 @@ def calc_expect_pub_sub_num(config: dict) -> None: EXPECT_INIT_SUB_NUM = 0 +def calc_action_delays(config: dict) -> tuple: + unit_delay = 1.0 + pub_delay = 0.0 if config['launch_pub_before_sub'] else unit_delay + sub_delay = 0.01 * EXPECT_INIT_PUB_NUM + unit_delay if config['launch_pub_before_sub'] else 0.0 + ready_delay = pub_delay + sub_delay + 4.0 + return pub_delay, sub_delay, ready_delay + + @launch_testing.markers.keep_alive def generate_test_description(): with open(CONFIG_PATH, 'r') as f: config = yaml.safe_load(f) calc_expect_pub_sub_num(config) + pub_delay, sub_delay, ready_delay = calc_action_delays(config) pub_node = TimerAction( - period=0.0 if config['launch_pub_before_sub'] else 1.0, + period=pub_delay, actions=[ ComposableNodeContainer( name='test_talker_container', @@ -157,7 +166,7 @@ def generate_test_description(): ) sub_nodes = TimerAction( - period=1.0 if config['launch_pub_before_sub'] else 0.0, + period=sub_delay, actions=sub_nodes_actions ) @@ -167,7 +176,7 @@ def generate_test_description(): SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '0'), pub_node, sub_nodes, - TimerAction(period=5.0, actions=[launch_testing.actions.ReadyToTest()]) + TimerAction(period=ready_delay, actions=[launch_testing.actions.ReadyToTest()]) ] ), { From 7444c334e2b9067b8f6ff4ca261118322a8f9723 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Tue, 28 Jan 2025 18:02:03 +0900 Subject: [PATCH 12/24] fix --- src/e2e_test/test/test_1to1_with_ros2sub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 4c514363..c31f42d1 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -42,7 +42,7 @@ def calc_action_delays(config: dict) -> tuple: unit_delay = 1.0 pub_delay = 0.0 if config['launch_pub_before_sub'] else unit_delay sub_delay = 0.01 * EXPECT_INIT_PUB_NUM + unit_delay if config['launch_pub_before_sub'] else 0.0 - ready_delay = pub_delay + sub_delay + 4.0 + ready_delay = pub_delay + sub_delay + 5.0 return pub_delay, sub_delay, ready_delay From a8d01f8c1a3c8f8caaccaba330a192f2a789d5a1 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Wed, 29 Jan 2025 07:08:50 +0900 Subject: [PATCH 13/24] fix --- scripts/e2e_test_1to1_with_ros2sub | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index 5076d9e1..2f7b3f7b 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -2,6 +2,7 @@ # 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 @@ -52,9 +53,4 @@ done echo "All tests passed" # Reset config file -sed -i "s/launch_pub_before_sub:.*/launch_pub_before_sub: true/g" $CONFIG_FILE -sed -i "s/pub_qos_depth:.*/pub_qos_depth: 10/g" $CONFIG_FILE -sed -i "s/pub_transient_local:.*/pub_transient_local: true/g" $CONFIG_FILE -sed -i "s/sub_qos_depth:.*/sub_qos_depth: 10/g" $CONFIG_FILE -sed -i "s/sub_transient_local:.*/sub_transient_local: true/g" $CONFIG_FILE -sed -i "s/use_take_sub:.*/use_take_sub: true/g" $CONFIG_FILE +git checkout -- "$CONFIG_FILE" From 2e65e5e801216d754817506b5f6664c47e78c1e4 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Thu, 30 Jan 2025 07:17:05 +0900 Subject: [PATCH 14/24] fix --- scripts/e2e_test_1to1_with_ros2sub | 36 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index 2f7b3f7b..cf08f5c1 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -16,28 +16,28 @@ 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 +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)) echo "=================== $COUNT / 64 ==================" - echo "launch_pub_before_sub: $LAUNCH_PUB_BEFORE_SUB" - echo "pub_qos_depth: $PUB_QOS_DEPTH" - echo "pub_transient_local: $PUB_TRANSIENT_LOCAL" - echo "sub_qos_depth: $SUB_QOS_DEPTH" - echo "sub_transient_local: $SUB_TRANSIENT_LOCAL" - echo "use_take_sub: $USE_TAKE_SUB" + echo "launch_pub_before_sub: $launch_pub_before_sub" + echo "pub_qos_depth: $pub_qos_depth" + echo "pub_transient_local: $pub_transient_local" + echo "sub_qos_depth: $sub_qos_depth" + echo "sub_transient_local: $sub_transient_local" + echo "use_take_sub: $use_take_sub" echo "==============================================" - 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 + 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 launch_test src/e2e_test/test/test_1to1_with_ros2sub.py if [ $? -ne 0 ]; then From dff97bfb0f85615edef18883ff7052d9a1f3e3ac Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Thu, 30 Jan 2025 08:31:02 +0900 Subject: [PATCH 15/24] fix and refactor script --- scripts/e2e_test_1to1_with_ros2sub | 139 +++++++++++++++++++++-------- 1 file changed, 100 insertions(+), 39 deletions(-) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index cf08f5c1..13d6eea0 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -1,56 +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 -# Configurations +# Run test CONFIG_FILE="src/e2e_test/test/config_test_1to1_with_ros2sub.yaml" -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)) - echo "=================== $COUNT / 64 ==================" - echo "launch_pub_before_sub: $launch_pub_before_sub" - echo "pub_qos_depth: $pub_qos_depth" - echo "pub_transient_local: $pub_transient_local" - echo "sub_qos_depth: $sub_qos_depth" - echo "sub_transient_local: $sub_transient_local" - echo "use_take_sub: $use_take_sub" - echo "==============================================" - - 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 - launch_test src/e2e_test/test/test_1to1_with_ros2sub.py - - if [ $? -ne 0 ]; then - echo "Test failed" - exit 1 - fi +show_config() { + echo "----------------------------------------------" + cat $CONFIG_FILE + echo "----------------------------------------------" +} + +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 + FAILURE_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 -done -echo "All tests passed" +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" From 039ff6253bb4b658986fc36c083583298a16f1d4 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Thu, 30 Jan 2025 08:41:23 +0900 Subject: [PATCH 16/24] fix expected num --- src/e2e_test/test/test_1to1_with_ros2sub.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index c31f42d1..350ba983 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -27,12 +27,13 @@ def calc_expect_pub_sub_num(config: dict) -> None: config['launch_pub_before_sub'] and config['pub_transient_local']) else 0 EXPECT_PUB_NUM = config['pub_qos_depth'] - base_sub_num = min(EXPECT_PUB_NUM, config['sub_qos_depth']) - EXPECT_ROS2_SUB_NUM = base_sub_num - EXPECT_SUB_NUM = base_sub_num - if config['launch_pub_before_sub'] and config['sub_transient_local']: - EXPECT_INIT_ROS2_SUB_NUM = base_sub_num - EXPECT_INIT_SUB_NUM = 0 if config['use_take_sub'] else base_sub_num + EXPECT_ROS2_SUB_NUM = min(EXPECT_PUB_NUM, config['sub_qos_depth']) + EXPECT_SUB_NUM = min(EXPECT_PUB_NUM, config['sub_qos_depth']) + if config['sub_transient_local']: + EXPECT_INIT_ROS2_SUB_NUM = min( + EXPECT_INIT_PUB_NUM, config['sub_qos_depth']) if config['pub_transient_local'] else 0 + EXPECT_INIT_SUB_NUM = 0 if config['use_take_sub'] else min( + EXPECT_INIT_PUB_NUM, config['sub_qos_depth']) else: EXPECT_INIT_ROS2_SUB_NUM = 0 EXPECT_INIT_SUB_NUM = 0 From f8cea4b71e86369654808c2d047bab86241f9e63 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Thu, 30 Jan 2025 16:11:54 +0900 Subject: [PATCH 17/24] fix: to assert the number of received messages --- src/e2e_test/test/test_1to1_with_ros2sub.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 350ba983..b08177da 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -199,12 +199,22 @@ def test_pub(self, proc_output, test_pub): def test_sub(self, proc_output, test_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_sub) as cm: + # Check the order of messages received for i in range(EXPECT_INIT_PUB_NUM - EXPECT_INIT_SUB_NUM, EXPECT_SUB_NUM): cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") + # Check the number of messages received + sub_count = "".join(cm._output).count("Receiving ") + self.assertEqual(sub_count, EXPECT_INIT_SUB_NUM + EXPECT_SUB_NUM) + def test_ros2_sub(self, proc_output, test_ros2_sub): with launch_testing.asserts.assertSequentialStdout(proc_output, process=test_ros2_sub) as cm: + # Check the order of messages received for i in range(EXPECT_INIT_PUB_NUM - EXPECT_INIT_ROS2_SUB_NUM, EXPECT_ROS2_SUB_NUM): cm.assertInStdout(f"Receiving {i}.") cm.assertInStdout("All messages received. Shutting down.") + + # Check the number of messages received + sub_count = "".join(cm._output).count("Receiving ") + self.assertEqual(sub_count, EXPECT_INIT_ROS2_SUB_NUM + EXPECT_ROS2_SUB_NUM) From 7754a61d9487a768e1f7b6f57da24cf86c6253ea Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Fri, 31 Jan 2025 08:31:14 +0900 Subject: [PATCH 18/24] refactor --- scripts/e2e_test_1to1_with_ros2sub | 2 +- src/e2e_test/src/test_publisher.cpp | 34 ++++++++++++++++++++++++++--- 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/scripts/e2e_test_1to1_with_ros2sub b/scripts/e2e_test_1to1_with_ros2sub index 13d6eea0..a9fcf176 100644 --- a/scripts/e2e_test_1to1_with_ros2sub +++ b/scripts/e2e_test_1to1_with_ros2sub @@ -56,6 +56,7 @@ show_config() { echo "----------------------------------------------" } +FAILURE_COUNT=0 if [ "$RUN_SINGLE" = true ]; then show_config launch_test src/e2e_test/test/test_1to1_with_ros2sub.py @@ -72,7 +73,6 @@ else USE_TAKE_SUB=(true false) COUNT=0 - FAILURE_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 diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index 8e8212fd..fd877361 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -12,17 +12,41 @@ class TestPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; agnocast::Publisher::SharedPtr publisher_; int64_t count_; - int64_t pub_num_; + 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 message = publisher_->borrow_loaned_message(); message->data = count_; publisher_->publish(std::move(message)); RCLCPP_INFO(this->get_logger(), "Publishing %ld.", count_); count_++; - if (count_ == pub_num_) { + if (count_ == target_pub_num_) { RCLCPP_INFO(this->get_logger(), "All messages published. Shutting down."); std::cout << std::flush; timer_->cancel(); @@ -37,10 +61,14 @@ class TestPublisher : public rclcpp::Node this->declare_parameter("transient_local", true); this->declare_parameter("init_pub_num", 10); this->declare_parameter("pub_num", 10); + this->declare_parameter("planned_sub_count", 1); + this->declare_parameter("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(this->get_parameter("planned_pub_count").as_int()); rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(qos_depth)); if (transient_local) { @@ -48,7 +76,7 @@ class TestPublisher : public rclcpp::Node } publisher_ = agnocast::create_publisher(this, "/test_topic", qos); count_ = 0; - pub_num_ = init_pub_num + pub_num; + target_pub_num_ = init_pub_num + pub_num; // Initial publish for (int i = 0; i < init_pub_num; i++) { From d250d09f612c44e8407e51fa7b298863d77fe97a Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Fri, 31 Jan 2025 08:33:20 +0900 Subject: [PATCH 19/24] fix --- src/e2e_test/src/test_publisher.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/e2e_test/src/test_publisher.cpp b/src/e2e_test/src/test_publisher.cpp index fd877361..ab013748 100644 --- a/src/e2e_test/src/test_publisher.cpp +++ b/src/e2e_test/src/test_publisher.cpp @@ -87,12 +87,6 @@ class TestPublisher : public rclcpp::Node count_++; } - // wait for the subscription to be established - while (publisher_->get_subscription_count() < 2) { - sleep(1); - } - sleep(2); // HACK: wait subscribing transient local messages - timer_ = this->create_wall_timer(10ms, std::bind(&TestPublisher::timer_callback, this)); } }; From 31884dc14215d7c89f16331652a04f33e5d09fb4 Mon Sep 17 00:00:00 2001 From: atsushi yano <55824710+atsushi421@users.noreply.github.com> Date: Fri, 31 Jan 2025 09:38:55 +0900 Subject: [PATCH 20/24] Update src/e2e_test/test/test_1to1_with_ros2sub.py Co-authored-by: Koichi Imai <45482193+Koichi98@users.noreply.github.com> --- src/e2e_test/test/test_1to1_with_ros2sub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index b08177da..0eb3900c 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -88,7 +88,7 @@ def generate_test_description(): sub_nodes_actions = [ ComposableNodeContainer( - name='test_ros2_lister_container', + name='test_ros2_listener_container', namespace='', package='agnocastlib', executable='agnocast_component_container', From cdcedd27a1bb4d9f7c15dcdf928b822d0a709607 Mon Sep 17 00:00:00 2001 From: atsushi yano <55824710+atsushi421@users.noreply.github.com> Date: Fri, 31 Jan 2025 09:39:02 +0900 Subject: [PATCH 21/24] Update src/e2e_test/test/test_1to1_with_ros2sub.py Co-authored-by: Koichi Imai <45482193+Koichi98@users.noreply.github.com> --- src/e2e_test/test/test_1to1_with_ros2sub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 0eb3900c..1c17d68f 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -140,7 +140,7 @@ def generate_test_description(): else: sub_nodes_actions.append( ComposableNodeContainer( - name='test_lister_container', + name='test_listener_container', namespace='', package='agnocastlib', executable='agnocast_component_container', From 0abd8e32ec215d5564de478615ae6ec683a7fe12 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Mon, 3 Feb 2025 11:03:58 +0900 Subject: [PATCH 22/24] fix: to use rclcpp_components --- src/e2e_test/test/test_1to1_with_ros2sub.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/e2e_test/test/test_1to1_with_ros2sub.py b/src/e2e_test/test/test_1to1_with_ros2sub.py index 1c17d68f..e5b4b667 100644 --- a/src/e2e_test/test/test_1to1_with_ros2sub.py +++ b/src/e2e_test/test/test_1to1_with_ros2sub.py @@ -90,8 +90,8 @@ def generate_test_description(): ComposableNodeContainer( name='test_ros2_listener_container', namespace='', - package='agnocastlib', - executable='agnocast_component_container', + package='rclcpp_components', + executable='component_container', composable_node_descriptions=[ ComposableNode( package='e2e_test', From 4447e440771541a235a7200c9987f6769b3bd6e9 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Mon, 3 Feb 2025 11:38:45 +0900 Subject: [PATCH 23/24] fix workflow --- .github/workflows/build-test.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-test.yaml b/.github/workflows/build-test.yaml index edb0fe62..0c582a89 100644 --- a/.github/workflows/build-test.yaml +++ b/.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 From 1e1ab1535ab27bc3f3b7746c8eb1bed4373fcddc Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Mon, 3 Feb 2025 11:39:45 +0900 Subject: [PATCH 24/24] fix: PR template --- .github/pull_request_template.md | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index d478bef9..aef850c0 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -6,5 +6,6 @@ - [ ] sample application (required) - [ ] Autoware (required) +- [ ] `bash scripts/e2e_test_1to1_with_ros2sub` ## Notes for reviewers