From 8eca2c762b87e2919100200be25366f893981ecd Mon Sep 17 00:00:00 2001 From: MrBearing Date: Sat, 28 Sep 2024 02:09:33 +0900 Subject: [PATCH] add CI and Test --- .github/workflows/humble_docker_test.yaml | 115 +++++++++++++++++ .github/workflows/humble_test.yaml | 118 ++++++++++++++++++ README.md | 2 +- docker-compose.yml | 38 ++++++ update_ip.sh | 72 +++++++++++ .../test_echoback_string/Filelists.cmake | 3 + workspace/test_echoback_string/app.cpp | 97 ++++++++++++++ workspace/test_echoback_string/templates.hpp | 11 ++ .../Filelists.cmake | 3 + .../test_echoback_string_responder/app.cpp | 41 ++++++ .../templates.hpp | 11 ++ workspace/test_echoback_twist/Filelists.cmake | 3 + workspace/test_echoback_twist/app.cpp | 117 +++++++++++++++++ workspace/test_echoback_twist/templates.hpp | 11 ++ .../Filelists.cmake | 3 + .../test_echoback_twist_responder/app.cpp | 44 +++++++ .../templates.hpp | 11 ++ 17 files changed, 699 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/humble_docker_test.yaml create mode 100644 .github/workflows/humble_test.yaml create mode 100644 docker-compose.yml create mode 100755 update_ip.sh create mode 100644 workspace/test_echoback_string/Filelists.cmake create mode 100644 workspace/test_echoback_string/app.cpp create mode 100644 workspace/test_echoback_string/templates.hpp create mode 100644 workspace/test_echoback_string_responder/Filelists.cmake create mode 100644 workspace/test_echoback_string_responder/app.cpp create mode 100644 workspace/test_echoback_string_responder/templates.hpp create mode 100644 workspace/test_echoback_twist/Filelists.cmake create mode 100644 workspace/test_echoback_twist/app.cpp create mode 100644 workspace/test_echoback_twist/templates.hpp create mode 100644 workspace/test_echoback_twist_responder/Filelists.cmake create mode 100644 workspace/test_echoback_twist_responder/app.cpp create mode 100644 workspace/test_echoback_twist_responder/templates.hpp diff --git a/.github/workflows/humble_docker_test.yaml b/.github/workflows/humble_docker_test.yaml new file mode 100644 index 0000000..fb6b26b --- /dev/null +++ b/.github/workflows/humble_docker_test.yaml @@ -0,0 +1,115 @@ +name: Humble_test_with_docker_NativeROS_string + +on: + push: + branches: + - "main" + pull_request: + types: [opened, synchronize, labeled] +jobs: + ci: + runs-on: ${{ matrix.os }} + if: | + ((github.event.action == 'labeled') && (github.event.label.name == 'TESTING') && (github.base_ref == 'main' )) || + ((github.event.action == 'synchronize') && (github.base_ref == 'main') && contains(github.event.pull_request.labels.*.name, 'TESTING')) || + (github.ref_name == 'main') + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop + timeout-minutes: 5 + strategy: + fail-fast: false + matrix: + os: [ubuntu-22.04] + ros_distribution: [humble] + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + submodules: recursive + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + + - name: Install Docker Compose + run: sudo apt-get update && sudo apt-get install -y docker-compose + # docker-compose.yml を使ってコンテナを起動 + - name: Build and run containers + run: docker-compose up -d + - name: Wait for containers to start + run: sleep 20 # コンテナが確実に起動するのを待つ時間を長くする + - name: List running containers + run: docker ps -a # 全コンテナが起動しているか確認 + - name: Show container logs # コンテナのログを確認する + run: | + docker logs app1 + docker logs app2 + - name: Show IP addresses + run: | + docker exec app1 ip addr show eth0 + docker exec app2 ip addr show eth0 + - name: Test communication between containers (ping) + run: | + docker exec app1 ping -c 4 172.19.0.3 # app1 から app2 へのPing + docker exec app2 ping -c 4 172.19.0.2 # app2 から app1 へのPing + - name: Test TCP connection between containers (netcat) + run: | + # app2でポート8080をリスン + docker exec -d app2 bash -c "nc -l -p 8080" + # app1からapp2へのTCP接続を確認 + docker exec app1 bash -c "echo 'Test' | nc -w 3 172.19.0.3 8080" + # app1: ビルドの準備 (chmod +x update_ip.sh と ./update_ip.sh の実行) + - name: Prepare build in mros + run: | + docker cp $GITHUB_WORKSPACE app1:/root/ws_mros # ソースコードをapp1コンテナにコピー + docker exec app1 bash -c " + cd /root/ws_mros && + chmod +x update_ip.sh && # 権限変更 + ./update_ip.sh" # IPアドレス更新スクリプトの実行 + # app1: クリーンビルド + - name: Clean and build in mros + run: | + docker exec app1 bash -c " + cd /root/ws_mros && + pwd && + ls -la && + bash build.bash clean && # クリーンアップ + bash build.bash all test_echoback_string" + # app2: のコードをコンテナにコピーしてビルド + - name: Clone Native test stub source code to app2 and build and run + run: | + docker exec app2 bash -c " + source /opt/ros/humble/setup.bash && + cd && + pwd && + ls -la && + git clone https://github.com/mROS-base/mros2-host-examples && + cd mros2-host-examples && + colcon build --packages-select mros2_echoreply_string && + ls -la && + source install/setup.bash" + - name: Run mROS and Native ROS + shell: bash + run : | + # テスト対象をバックグラウンドで実行 + docker exec app2 bash -c "source /opt/ros/humble/setup.bash && + cd mros2-host-examples && + source install/setup.bash && + ros2 run mros2_echoreply_string echoreply_node" & + + docker exec app1 bash -c "cd /root/ws_mros && ./cmake_build/mros2-posix" & + mros_pid=$! # テストプログラムのプロセスIDを取得 + + docker ps + + # mROSが終了するまで待つ + wait $mros_pid + mros_status=$? + # 結果に基づいてCIの成否を判断 + if [ $mros_status -eq 0 ] ;then + echo "Succeed pub/sub test process between mros2 and Native ROS" + exit 0 + else + echo "Fail pub/sub test process between mros2 and Native ROS" + exit 1 + fi + - name: Tear down containers + run: docker-compose down diff --git a/.github/workflows/humble_test.yaml b/.github/workflows/humble_test.yaml new file mode 100644 index 0000000..0464e49 --- /dev/null +++ b/.github/workflows/humble_test.yaml @@ -0,0 +1,118 @@ +name: ci_humble + +on: + push: + branches: + - "main" + - "feat/add_ci_config" + pull_request: + types: [opened, synchronize, labeled] +jobs: + ci: + runs-on: ${{ matrix.os }} + if: | + ((github.event.action == 'labeled') && (github.event.label.name == 'TESTING') && (github.base_ref == 'main' )) || + ((github.event.action == 'synchronize') && (github.base_ref == 'main') && contains(github.event.pull_request.labels.*.name, 'TESTING')) || + (github.ref_name == 'main') + container: + image: osrf/ros:${{ matrix.ros_distribution }}-desktop + timeout-minutes: 3 + strategy: + fail-fast: false + matrix: + os: [ubuntu-22.04] + ros_distribution: [humble] + comm-target: [native , mros] + comm-data-type: [string, twist] + steps: + - uses: actions/checkout@v3 + with: + submodules: recursive + - name: update + run: sudo apt-get update + - name: setup and install tools + run: > + sudo apt-get install -y iproute2 git wget build-essential gcc g++ libssl-dev libreadline-dev + zlib1g-dev make autoconf automake cmake pkg-config curl net-tools netcat python3-jinja2 + - name: Clone Test Stub + uses: actions/checkout@v3 + with: + repository: mROS-base/mros2-host-examples + path: ws_host/src/mros2-host-examples + - name: Update IP address + run: | + chmod +x update_ip.sh + ./update_ip.sh + - name: Generate Twist Header files + if: matrix.comm-data-type == 'twist' + shell: bash + run: | + cd workspace + python3 ../mros2/mros2_header_generator/header_generator.py geometry_msgs/msg/Twist.msg + cd - + - name: Build mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_${{ matrix.comm-data-type }} + mv cmake_build/ test_echoback_${{ matrix.comm-data-type }}/ + - name: Build Native ROS responder + if: matrix.comm-target == 'native' + shell: bash + run: | + cd ws_host/ + source /opt/ros/humble/setup.bash + colcon build --packages-select mros2_echoreply_${{ matrix.comm-data-type }} + - name: Build mROS responder + if: matrix.comm-target == 'mros' + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_${{ matrix.comm-data-type }}_responder + - name: Run Testing mROS and Native ROS + if: matrix.comm-target == 'native' + shell: bash + run : | + ./test_echoback_${{ matrix.comm-data-type }}/mros2-posix & + mros_pid=$! # mROSのプロセスIDを取得 + + # Native ROSをバックグラウンドで実行 + cd ws_host/ + source /opt/ros/humble/setup.bash + source install/setup.bash + ros2 run mros2_echoreply_${{ matrix.comm-data-type }} echoreply_node & + + # mROSが終了するまで待つ + wait $mros_pid + mros_status=$? + + # 結果に基づいてCIの成否を判断 + if [ $mros_status -eq 0 ] ;then + echo "Succeed pub/sub test process between mros2 and Native ROS" + exit 0 + else + echo "Fail pub/sub test process between mros2 and Native ROS" + exit 1 + fi + - name: Run mROS and mROS + if: matrix.comm-target == 'mros' + shell: bash + run : | + ./test_echoback_${{ matrix.comm-data-type }}/mros2-posix & + mros_pid=$! # テストプログラムのプロセスIDを取得 + + # テスト対象をバックグラウンドで実行 + ./cmake_build/mros2-posix & + + # mROSが終了するまで待つ + wait $mros_pid + mros_status=$? + + # 結果に基づいてCIの成否を判断 + if [ $mros_status -eq 0 ] ;then + echo "Succeed pub/sub test process between mros2 and Native ROS" + exit 0 + else + echo "Fail pub/sub test process between mros2 and Native ROS" + exit 1 + fi \ No newline at end of file diff --git a/README.md b/README.md index 05ba7ba..dd01bca 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ sudo apt-get update && sudo apt-get install -y \ libssl-dev libreadline-dev zlib1g-dev \ make autoconf automake cmake \ pkg-config curl \ - net-tools netcat + net-tools netcat jinja2 ``` Please check the IP address and netmask of the execution environment. diff --git a/docker-compose.yml b/docker-compose.yml new file mode 100644 index 0000000..729fb10 --- /dev/null +++ b/docker-compose.yml @@ -0,0 +1,38 @@ +version: '3' +services: + app1: + container_name: app1 + image: osrf/ros:humble-desktop # ROS Humble の Ubuntu ベースイメージを使用 + command: > + bash -c "set -e && + apt-get update && + apt-get install -y iproute2 iputils-ping git wget build-essential gcc g++ libssl-dev libreadline-dev + zlib1g-dev make autoconf automake cmake pkg-config curl net-tools netcat python3-jinja2 && + echo 'IP address:' && + ip addr show eth0 && + sleep 600" + networks: + test_net: + ipv4_address: 172.19.0.2 + + app2: + container_name: app2 + image: osrf/ros:humble-desktop # ROS Humble の Ubuntu ベースイメージを使用 + command: > + bash -c "set -e && + apt-get update && + apt-get install -y iproute2 iputils-ping git wget build-essential gcc g++ libssl-dev libreadline-dev + zlib1g-dev make autoconf automake cmake pkg-config curl net-tools netcat python3-jinja2 && + echo 'IP address:' && + ip addr show eth0 && + sleep 600" + networks: + test_net: + ipv4_address: 172.19.0.3 + +networks: + test_net: + driver: bridge + ipam: + config: + - subnet: 172.19.0.0/16 \ No newline at end of file diff --git a/update_ip.sh b/update_ip.sh new file mode 100755 index 0000000..86300c9 --- /dev/null +++ b/update_ip.sh @@ -0,0 +1,72 @@ +#!/bin/bash + +# IPアドレスを取得 +IP_ADDRESS=$(hostname -I | awk '{print $1}') +if [ -z "$IP_ADDRESS" ]; then + echo "Error: Failed to retrieve IP address." + exit 1 +fi + +# ネットマスクを取得（IPアドレスに対応するネットマスクを抽出） +INTERFACE=$(ip -o addr show | grep "$IP_ADDRESS" | awk '{print $2}') +NETMASK=$(ip -o -f inet addr show $INTERFACE | awk '/inet/ {print $4}' | cut -d'/' -f2) +if [ -z "$NETMASK" ]; then + echo "Error: Failed to retrieve netmask." + exit 1 +fi + +# CIDRからネットマスクを計算 +function cidr_to_netmask() { + local cidr=$1 + local mask="" + local octets=$(( cidr / 8 )) + local bits=$(( cidr % 8 )) + + for (( i=0; i<4; i++ )); do + if [ $i -lt $octets ]; then + mask+="255" + elif [ $i -eq $octets ]; then + mask+=$(( 256 - 2**(8-bits) )) + else + mask+="0" + fi + + if [ $i -lt 3 ]; then + mask+="." + fi + done + echo "$mask" +} + +NETMASK=$(cidr_to_netmask $NETMASK) +echo "Retrieved Netmask for IP $IP_ADDRESS: $NETMASK" + +# IPアドレスをドットで分割 +IFS='.' read -r -a IP_PARTS <<< "$IP_ADDRESS" + +# ネットマスクをドットで分割 +IFS='.' read -r -a NETMASK_PARTS <<< "$NETMASK" +echo "Netmask parts: ${NETMASK_PARTS[0]}, ${NETMASK_PARTS[1]}, ${NETMASK_PARTS[2]}, ${NETMASK_PARTS[3]}" + +# include/rtps/config.h のIPアドレス置換 + +echo "Running sed on include/rtps/config.h" +sed -i "s/[[:space:]]*[0-9]\{1,3\},[[:space:]]*[0-9]\{1,3\},[[:space:]]*[0-9]\{1,3\},[[:space:]]*[0-9]\{1,3\}[[:space:]]*}; \ +\/\/ Needs to be set in lwipcfg.h too./\ +${IP_PARTS[0]}, ${IP_PARTS[1]}, ${IP_PARTS[2]}, ${IP_PARTS[3]}};\ +\/\/ Needs to be set in lwipcfg.h too./" \ +include/rtps/config.h + + +# include/netif.h のIPアドレスとネットマスク置換 +sed -i 's/#define NETIF_IPADDR ".*"/#define NETIF_IPADDR "'$IP_ADDRESS'"/' include/netif.h +sed -i 's/#define NETIF_NETMASK ".*"/#define NETIF_NETMASK "'$NETMASK'"/' include/netif.h + + + +# 結果を表示して確認 +echo "Updated IP Address: $IP_ADDRESS" +echo "Updated include/rtps/config.h:" +grep -E 'Needs to be set in lwipcfg.h too.' include/rtps/config.h +echo "Updated include/netif.h:" +grep -E 'NETIF_IPADDR' include/netif.h \ No newline at end of file diff --git a/workspace/test_echoback_string/Filelists.cmake b/workspace/test_echoback_string/Filelists.cmake new file mode 100644 index 0000000..6ef8d52 --- /dev/null +++ b/workspace/test_echoback_string/Filelists.cmake @@ -0,0 +1,3 @@ +set(apl_SRCS + ${PROJECT_SOURCE_DIR}/workspace/${MROS2_APPNAME}/app.cpp +) \ No newline at end of file diff --git a/workspace/test_echoback_string/app.cpp b/workspace/test_echoback_string/app.cpp new file mode 100644 index 0000000..35327c0 --- /dev/null +++ b/workspace/test_echoback_string/app.cpp @@ -0,0 +1,97 @@ + +/** + * このプログラムとechoreply_stringを使う + * 10回くらい送信して返ってくるかチェックする + */ + +#include "mros2.h" +#include "std_msgs/msg/string.hpp" + +#include "cmsis_os.h" +#include "netif.h" +#include "netif_posix_add.h" + +#include +#include +#include + +bool result = true; +auto TestMessagePreFix = "TestMessage"; +std::string expect = ""; +int sub_counter = 0; +pthread_mutex_t mutex_expect_sting; + +void userCallback(std_msgs::msg::String *msg) +{ + pthread_mutex_lock(&mutex_expect_sting); + sub_counter++; + printf("*test count : %d\r\n", sub_counter); + auto actual = msg->data; + // FIXME 受信データに改行や不要な文字が含まれている模様 + actual.erase(std::remove(actual.begin(), actual.end(), '\n'), actual.end()); + actual.erase(std::remove(actual.begin(), actual.end(), '\r'), actual.end()); + + MROS2_DEBUG("Sub result:\r\n expect : '%s',\r\n actural: '%s'", expect.c_str(), actual.c_str()); + MROS2_DEBUG("expect length: %lu, actual length: %lu", expect.size(), actual.size()); + if (strcmp(expect.c_str(), actual.c_str()) == 0) + { + MROS2_INFO("****SUCCEED****"); + result = result & true; + } + else + { + MROS2_INFO("****FAIL different data responsed.***"); + result = result & false; + } + pthread_mutex_unlock(&mutex_expect_sting); + if (sub_counter > 10) + { + if (result) + { + MROS2_INFO("All tests succeed !!"); + std::exit(0); + } + MROS2_INFO("Some tests faild."); + std::exit(-1); + } +} + + +int main(int argc, char *argv[]) +{ + netif_posix_add(NETIF_IPADDR, NETIF_NETMASK); + + osKernelStart(); + + MROS2_INFO("mros2-posix start!\r\n"); + MROS2_INFO("app name: echoback_string\r\n"); + mros2::init(0, NULL); + MROS2_DEBUG("mROS 2 initialization is completed\r\n"); + + mros2::Node node = mros2::Node::create_node("mros2_test_node"); + mros2::Publisher pub = node.create_publisher("to_linux", 10); + auto msg = std_msgs::msg::String(); + msg.data = "initial"; + pub.publish(msg); + osDelay(1000); + mros2::Subscriber sub = node.create_subscription("to_stm", 10, userCallback); + + MROS2_INFO("ready to pub/sub message\r\n"); + auto count = 0; + sub_counter = 0; + while (true) + { + auto msg = std_msgs::msg::String(); + pthread_mutex_lock(&mutex_expect_sting); + expect = TestMessagePreFix + std::to_string(count++); + msg.data = expect; + MROS2_INFO("publishing msg: '%s'", msg.data.c_str()); + pub.publish(msg); + pthread_mutex_unlock(&mutex_expect_sting); + + osDelay(1000); + } + + mros2::spin(); + return 0; +} diff --git a/workspace/test_echoback_string/templates.hpp b/workspace/test_echoback_string/templates.hpp new file mode 100644 index 0000000..780d817 --- /dev/null +++ b/workspace/test_echoback_string/templates.hpp @@ -0,0 +1,11 @@ + +#include "std_msgs/msg/string.hpp" + + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template void mros2::Publisher::publish(std_msgs::msg::String &msg); + + + +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::String*)); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); diff --git a/workspace/test_echoback_string_responder/Filelists.cmake b/workspace/test_echoback_string_responder/Filelists.cmake new file mode 100644 index 0000000..6ef8d52 --- /dev/null +++ b/workspace/test_echoback_string_responder/Filelists.cmake @@ -0,0 +1,3 @@ +set(apl_SRCS + ${PROJECT_SOURCE_DIR}/workspace/${MROS2_APPNAME}/app.cpp +) \ No newline at end of file diff --git a/workspace/test_echoback_string_responder/app.cpp b/workspace/test_echoback_string_responder/app.cpp new file mode 100644 index 0000000..8f82ffa --- /dev/null +++ b/workspace/test_echoback_string_responder/app.cpp @@ -0,0 +1,41 @@ +#include "mros2.h" +#include "std_msgs/msg/string.hpp" + +#include "cmsis_os.h" +#include "netif.h" +#include "netif_posix_add.h" + +#include +#include + + +mros2::Subscriber sub; +mros2::Publisher pub; + +void userCallback(std_msgs::msg::String *msg) +{ + printf("subscribed msg: '%s'\r\n", msg->data.c_str()); + printf("publishing msg: '%s'\r\n", msg->data.c_str()); + pub.publish(*msg); +} + +int main(int argc, char* argv[]) +{ + netif_posix_add(NETIF_IPADDR, NETIF_NETMASK); + + osKernelStart(); + + printf("mros2-posix start!\r\n"); + printf("app name: echoreply_string\r\n"); + mros2::init(0, NULL); + MROS2_DEBUG("mROS 2 initialization is completed\r\n"); + + mros2::Node node = mros2::Node::create_node("mros2_node"); + pub = node.create_publisher("to_stm", 10); + sub = node.create_subscription("to_linux", 10, userCallback); + osDelay(100); + MROS2_INFO("ready to pub/sub message\r\n"); + + mros2::spin(); + return 0; +} diff --git a/workspace/test_echoback_string_responder/templates.hpp b/workspace/test_echoback_string_responder/templates.hpp new file mode 100644 index 0000000..780d817 --- /dev/null +++ b/workspace/test_echoback_string_responder/templates.hpp @@ -0,0 +1,11 @@ + +#include "std_msgs/msg/string.hpp" + + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template void mros2::Publisher::publish(std_msgs::msg::String &msg); + + + +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(std_msgs::msg::String*)); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); diff --git a/workspace/test_echoback_twist/Filelists.cmake b/workspace/test_echoback_twist/Filelists.cmake new file mode 100644 index 0000000..6ef8d52 --- /dev/null +++ b/workspace/test_echoback_twist/Filelists.cmake @@ -0,0 +1,3 @@ +set(apl_SRCS + ${PROJECT_SOURCE_DIR}/workspace/${MROS2_APPNAME}/app.cpp +) \ No newline at end of file diff --git a/workspace/test_echoback_twist/app.cpp b/workspace/test_echoback_twist/app.cpp new file mode 100644 index 0000000..e7b9850 --- /dev/null +++ b/workspace/test_echoback_twist/app.cpp @@ -0,0 +1,117 @@ + +/** + * このプログラムとechoreply_twistを使う + * 10回くらい送信して返ってくるかチェックする + */ + +#include "mros2.h" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/twist.hpp" + +#include "cmsis_os.h" +#include "netif.h" +#include "netif_posix_add.h" + +#include +#include +#include +#include + +bool result = true; +auto TestMessagePreFix = "TestMessage"; +geometry_msgs::msg::Twist expect; +int sub_counter = 0; +pthread_mutex_t mutex_expect_twist; + +void userCallback(geometry_msgs::msg::Twist *msg) +{ + pthread_mutex_lock(&mutex_expect_twist); + sub_counter++; + MROS2_INFO("*test count : %d\r\n", sub_counter); + + auto actual = *msg; + bool pub_sub_result = true; + + MROS2_INFO("expect { linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }", + expect.linear.x, expect.linear.y, expect.linear.z, expect.angular.x, expect.angular.y, expect.angular.z); + MROS2_INFO("actual { linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }", + actual.linear.x, actual.linear.y, actual.linear.z, actual.angular.x, actual.angular.y, actual.angular.z); + + pub_sub_result = pub_sub_result & (abs(actual.linear.x - expect.linear.x) < 0.001); + pub_sub_result = pub_sub_result & (abs(actual.linear.y - expect.linear.y) < 0.001); + pub_sub_result = pub_sub_result & (abs(actual.linear.z - expect.linear.z) < 0.001); + pub_sub_result = pub_sub_result & (abs(actual.angular.x - expect.angular.x) < 0.001); + pub_sub_result = pub_sub_result & (abs(actual.angular.y - expect.angular.y) < 0.001); + pub_sub_result = pub_sub_result & (abs(actual.angular.z - expect.angular.z) < 0.001); + + if (pub_sub_result) + { + MROS2_INFO("****SUCCEED****'\r\n"); + result = result & true; + } + else + { + MROS2_INFO("FAIL different data responsed.\r\n"); + result = result & false; + } + pthread_mutex_unlock(&mutex_expect_twist); + MROS2_INFO("\r\n"); + + if (sub_counter > 10) + { + if (result) + { + MROS2_INFO("All tests succeed.\r\n"); + std::exit(0); + } + MROS2_INFO("Some tests faild.\r\n"); + std::exit(-1); + } +} + +int main(int argc, char *argv[]) +{ + netif_posix_add(NETIF_IPADDR, NETIF_NETMASK); + + osKernelStart(); + + MROS2_INFO("mros2-posix start!\r\n"); + MROS2_INFO("app name: echoback_string\r\n"); + mros2::init(0, NULL); + MROS2_DEBUG("mROS 2 initialization is completed\r\n"); + + mros2::Node node = mros2::Node::create_node("mros2_test_node"); + mros2::Publisher pub = node.create_publisher("to_linux", 10); + osDelay(1000); + mros2::Subscriber sub = node.create_subscription("to_stm", 10, userCallback); + + MROS2_INFO("ready to pub/sub message\r\n"); + + geometry_msgs::msg::Vector3 linear; + geometry_msgs::msg::Vector3 angular; + + auto count = 0; + sub_counter = 0; + auto publish_count = 0; + while (true) + { + pthread_mutex_lock(&mutex_expect_twist); + linear.x = publish_count/1.0; + linear.y = publish_count/1.0; + linear.z = publish_count/1.0; + angular.x = publish_count/1.0; + angular.y = publish_count/1.0; + angular.z = publish_count/1.0; + expect.linear = linear; + expect.angular = angular; + MROS2_INFO("publishing Twist msg!!"); + MROS2_INFO("{ linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }", linear.x, linear.y, linear.z, angular.x, angular.y, angular.z); + pub.publish(expect); + publish_count++; + pthread_mutex_unlock(&mutex_expect_twist); + osDelay(1000); + } + + mros2::spin(); + return 0; +} diff --git a/workspace/test_echoback_twist/templates.hpp b/workspace/test_echoback_twist/templates.hpp new file mode 100644 index 0000000..11c4a00 --- /dev/null +++ b/workspace/test_echoback_twist/templates.hpp @@ -0,0 +1,11 @@ + +#include "geometry_msgs/msg/twist.hpp" + + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template void mros2::Publisher::publish(geometry_msgs::msg::Twist &msg); + + + +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Twist*)); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange); diff --git a/workspace/test_echoback_twist_responder/Filelists.cmake b/workspace/test_echoback_twist_responder/Filelists.cmake new file mode 100644 index 0000000..6ef8d52 --- /dev/null +++ b/workspace/test_echoback_twist_responder/Filelists.cmake @@ -0,0 +1,3 @@ +set(apl_SRCS + ${PROJECT_SOURCE_DIR}/workspace/${MROS2_APPNAME}/app.cpp +) \ No newline at end of file diff --git a/workspace/test_echoback_twist_responder/app.cpp b/workspace/test_echoback_twist_responder/app.cpp new file mode 100644 index 0000000..44eeb07 --- /dev/null +++ b/workspace/test_echoback_twist_responder/app.cpp @@ -0,0 +1,44 @@ +#include "mros2.h" +#include "std_msgs/msg/string.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/twist.hpp" + + +#include "cmsis_os.h" +#include "netif.h" +#include "netif_posix_add.h" + +#include +#include + + +mros2::Subscriber sub; +mros2::Publisher pub; + +void userCallback(geometry_msgs::msg::Twist *msg) +{ + MROS2_INFO("expect { linear: {x: %f, y: %f, z: %f }, angular: {x: %f, y: %f, z: %f } }", + msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z); + pub.publish(*msg); +} + +int main(int argc, char* argv[]) +{ + netif_posix_add(NETIF_IPADDR, NETIF_NETMASK); + + osKernelStart(); + + printf("mros2-posix start!\r\n"); + printf("app name: echoreply_string\r\n"); + mros2::init(0, NULL); + MROS2_DEBUG("mROS 2 initialization is completed\r\n"); + + mros2::Node node = mros2::Node::create_node("mros2_node"); + pub = node.create_publisher("to_stm", 10); + sub = node.create_subscription("to_linux", 10, userCallback); + osDelay(100); + MROS2_INFO("ready to pub/sub message\r\n"); + + mros2::spin(); + return 0; +} diff --git a/workspace/test_echoback_twist_responder/templates.hpp b/workspace/test_echoback_twist_responder/templates.hpp new file mode 100644 index 0000000..11c4a00 --- /dev/null +++ b/workspace/test_echoback_twist_responder/templates.hpp @@ -0,0 +1,11 @@ + +#include "geometry_msgs/msg/twist.hpp" + + +template mros2::Publisher mros2::Node::create_publisher(std::string topic_name, int qos); +template void mros2::Publisher::publish(geometry_msgs::msg::Twist &msg); + + + +template mros2::Subscriber mros2::Node::create_subscription(std::string topic_name, int qos, void (*fp)(geometry_msgs::msg::Twist*)); +template void mros2::Subscriber::callback_handler(void *callee, const rtps::ReaderCacheChange &cacheChange);