From d2da99e0a0c1f6fbb6b37d9cbf06df47007e79e9 Mon Sep 17 00:00:00 2001 From: MrBearing Date: Tue, 24 Sep 2024 16:56:35 +0900 Subject: [PATCH] add test between each type. --- .../humble_test_mros_to_mros_string.yaml | 75 +++++++++++ .../humble_test_mros_to_mros_twist.yaml | 75 +++++++++++ .../humble_test_with_nativeROS_string.yaml | 78 ++++++++++++ .../humble_test_with_nativeROS_twist.yaml | 86 +++++++++++++ update_ip.sh | 27 ++++ .../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, 696 insertions(+) create mode 100644 .github/workflows/humble_test_mros_to_mros_string.yaml create mode 100644 .github/workflows/humble_test_mros_to_mros_twist.yaml create mode 100644 .github/workflows/humble_test_with_nativeROS_string.yaml create mode 100644 .github/workflows/humble_test_with_nativeROS_twist.yaml 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_test_mros_to_mros_string.yaml b/.github/workflows/humble_test_mros_to_mros_string.yaml new file mode 100644 index 0000000..6384997 --- /dev/null +++ b/.github/workflows/humble_test_mros_to_mros_string.yaml @@ -0,0 +1,75 @@ +name: Humble_test_mros_to_mros_String + +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] + 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 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: Run IP update script + run: | + chmod +x update_ip.sh + ./update_ip.sh + - name: Build echoback mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_string + mv cmake_build/ test_echoback_string/ + - name: Build reply mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_string_responder + - name: Run mROS and Native ROS + shell: bash + run : | + ./test_echoback_string/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 diff --git a/.github/workflows/humble_test_mros_to_mros_twist.yaml b/.github/workflows/humble_test_mros_to_mros_twist.yaml new file mode 100644 index 0000000..685d695 --- /dev/null +++ b/.github/workflows/humble_test_mros_to_mros_twist.yaml @@ -0,0 +1,75 @@ +name: Humble_test_mros_to_mros_String + +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] + 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 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: Run IP update script + run: | + chmod +x update_ip.sh + ./update_ip.sh + - name: Build echoback mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_twist + mv cmake_build/ cmake_build_test_echoback_twist/ + - name: Build reply mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_twist_responder + - name: Run mROS and Native ROS + shell: bash + run : | + ./cmake_build_test_echoback_twist/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 diff --git a/.github/workflows/humble_test_with_nativeROS_string.yaml b/.github/workflows/humble_test_with_nativeROS_string.yaml new file mode 100644 index 0000000..63c7969 --- /dev/null +++ b/.github/workflows/humble_test_with_nativeROS_string.yaml @@ -0,0 +1,78 @@ +name: Humble_test_with_nativeROS_String + +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] + 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 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: Run IP update script + run: | + chmod +x update_ip.sh + ./update_ip.sh + - name: Build mROS + shell: bash + run: | + bash build.bash clean + bash build.bash all test_echoback_string + - name: Build Native ROS + shell: bash + run: | + cd ws_host/ + source /opt/ros/humble/setup.bash + colcon build --packages-select mros2_echoreply_string + - name: Run mROS and Native ROS + shell: bash + run : | + ./cmake_build/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_string 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 diff --git a/.github/workflows/humble_test_with_nativeROS_twist.yaml b/.github/workflows/humble_test_with_nativeROS_twist.yaml new file mode 100644 index 0000000..6358d7f --- /dev/null +++ b/.github/workflows/humble_test_with_nativeROS_twist.yaml @@ -0,0 +1,86 @@ +name: Humble_test_with_nativeROS_twist + +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] + 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 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: Run IP update script + run: | + chmod +x update_ip.sh + ./update_ip.sh + - name: Generate Header files + shell: bash + run: | + cd workspace + python3 ../mros2/mros2_header_generator/header_generator.py geometry_msgs/msg/Twist.msg + cd - + - name: Build mROS Programs + shell: bash + run: | + pwd + ls -la + bash build.bash clean + bash build.bash all test_echoback_twist + - name: Build Native ROS + shell: bash + run: | + cd ws_host/ + source /opt/ros/humble/setup.bash + colcon build --packages-select mros2_echoreply_twist + - name: Run mROS and Native ROS + shell: bash + run : | + ./cmake_build/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_twist 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 diff --git a/update_ip.sh b/update_ip.sh new file mode 100755 index 0000000..f84f531 --- /dev/null +++ b/update_ip.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# IPアドレスを取得 +IP_ADDRESS=$(hostname -I | awk '{print $1}') + +# IPアドレスをドットで分割 +IFS='.' read -r -a IP_PARTS <<< "$IP_ADDRESS" + +# 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 + +# 結果を表示して確認 +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);