Merge pull request #111 from cactusdynamics/ros2-subscriber

Implemented ROS2 subscriber and use TypeAdapter

.vscode/settings.json

@@ -5,6 +5,7 @@
  "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd"
  },
  "clangd.arguments": [
- "--compile-commands-dir=${workspaceFolder}/build/debug"
+ "--compile-commands-dir=${workspaceFolder}/build/debug",
+ "--header-insertion=never"
  ]
 }

cmake/ros2.cmake

@@ -13,5 +13,6 @@ ament_target_dependencies(cactus_rt
 )
 
 add_subdirectory(examples/ros2/publisher)
+add_subdirectory(examples/ros2/subscriber)
 
 ament_package()

examples/ros2/publisher/CMakeLists.txt

@@ -1,22 +1,43 @@
 find_package(std_msgs REQUIRED)
 
-add_executable(rt_ros2_publisher
- main.cc
+add_executable(rt_ros2_publisher_complex_data
+ complex_data.cc
 )
 
-target_link_libraries(rt_ros2_publisher
+target_link_libraries(rt_ros2_publisher_complex_data
  PRIVATE
  cactus_rt
 )
 
-setup_cactus_rt_target_options(rt_ros2_publisher)
+setup_cactus_rt_target_options(rt_ros2_publisher_complex_data)
 
-ament_target_dependencies(rt_ros2_publisher
+ament_target_dependencies(rt_ros2_publisher_complex_data
  PUBLIC
  std_msgs
 )
 
 install(
- TARGETS rt_ros2_publisher
+ TARGETS rt_ros2_publisher_complex_data
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+add_executable(rt_ros2_publisher_simple_data
+ simple_data.cc
+)
+
+target_link_libraries(rt_ros2_publisher_simple_data
+ PRIVATE
+ cactus_rt
+)
+
+setup_cactus_rt_target_options(rt_ros2_publisher_simple_data)
+
+ament_target_dependencies(rt_ros2_publisher_simple_data
+ PUBLIC
+ std_msgs
+)
+
+install(
+ TARGETS rt_ros2_publisher_simple_data
  DESTINATION lib/${PROJECT_NAME}
 )

examples/ros2/publisher/complex_data.cc

@@ -0,0 +1,122 @@
+#include <cactus_rt/ros2/app.h>
+#include <cactus_rt/rt.h>
+
+#include <chrono>
+#include <memory>
+#include <std_msgs/msg/float32_multi_array.hpp>
+
+struct MyCoefficientData {
+ float a;
+ float b;
+ float c;
+ float d;
+};
+
+using RealtimeType = MyCoefficientData;
+using RosType = std_msgs::msg::Float32MultiArray;
+
+template <>
+struct rclcpp::TypeAdapter<RealtimeType, RosType> {
+ using is_specialized = std::true_type;
+ using custom_type = RealtimeType;
+ using ros_message_type = RosType;
+
+ static void convert_to_ros_message(const custom_type& source, ros_message_type& destination) {
+ destination.data.reserve(4);
+ destination.data.push_back(source.a);
+ destination.data.push_back(source.b);
+ destination.data.push_back(source.c);
+ destination.data.push_back(source.d);
+ }
+
+ static void convert_to_custom(const ros_message_type& source, custom_type& destination) {
+ destination.a = source.data.at(0);
+ destination.b = source.data.at(1);
+ destination.c = source.data.at(2);
+ destination.d = source.data.at(3);
+ }
+};
+
+class RTROS2PublisherThread : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+ int64_t last_published_at_ = 0;
+ int64_t run_duration_;
+
+ std::shared_ptr<cactus_rt::ros2::Publisher<RealtimeType, RosType>> publisher_;
+
+ static cactus_rt::CyclicThreadConfig CreateThreadConfig() {
+ cactus_rt::CyclicThreadConfig thread_config;
+ thread_config.period_ns = 1'000'000;
+ thread_config.cpu_affinity = std::vector<size_t>{2};
+ thread_config.SetFifoScheduler(80);
+
+ return thread_config;
+ }
+
+ public:
+ explicit RTROS2PublisherThread(std::chrono::nanoseconds run_duration = std::chrono::seconds(30))
+ : cactus_rt::CyclicThread("RTROS2Publisher", CreateThreadConfig()),
+ run_duration_(run_duration.count()) {}
+
+ void InitializeForRos2() override {
+ publisher_ = ros2_adapter_->CreatePublisher<RealtimeType, RosType>("/cactus_rt/complex", rclcpp::QoS(100));
+ }
+
+ protected:
+ bool Loop(int64_t elapsed_ns) noexcept override {
+ if (elapsed_ns - last_published_at_ > 10'000'000) {
+ last_published_at_ = elapsed_ns;
+
+ const auto span = Tracer().WithSpan("Publish");
+
+ const auto elapsed_ns_f = static_cast<float>(elapsed_ns);
+
+ MyCoefficientData msg{
+ elapsed_ns_f,
+ elapsed_ns_f / 10.0F,
+ elapsed_ns_f / 100.0F,
+ elapsed_ns_f / 1000.0F
+ };
+
+ const auto success = publisher_->Publish(msg);
+ LOG_INFO(
+ Logger(),
+ "{} data {}, {}, {}, {}",
+ success ? "Published" : "Did not publish",
+ msg.a,
+ msg.b,
+ msg.c,
+ msg.d
+ );
+ }
+
+ return elapsed_ns > run_duration_;
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+
+ const cactus_rt::AppConfig app_config;
+
+ cactus_rt::ros2::Ros2Adapter::Config ros2_adapter_config;
+ ros2_adapter_config.timer_interval = std::chrono::milliseconds(50);
+
+ cactus_rt::ros2::App app("ComplexDataROS2Publisher", app_config, ros2_adapter_config);
+ app.StartTraceSession("build/publisher.perfetto");
+
+ constexpr std::chrono::seconds time(30);
+ std::cout << "Testing RT loop for " << time.count() << " seconds.\n";
+
+ auto thread = app.CreateROS2EnabledThread<RTROS2PublisherThread>(time);
+ app.RegisterThread(thread);
+
+ app.Start();
+
+ std::this_thread::sleep_for(time);
+
+ app.RequestStop();
+ app.Join();
+
+ std::cout << "Done\n";
+ return 0;
+}

examples/ros2/publisher/simple_data.cc

@@ -0,0 +1,82 @@
+#include <cactus_rt/ros2/app.h>
+#include <cactus_rt/rt.h>
+
+#include <chrono>
+#include <memory>
+#include <std_msgs/msg/int64.hpp>
+
+using RealtimeType = std_msgs::msg::Int64;
+using RosType = std_msgs::msg::Int64;
+
+class RTROS2PublisherThread : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+ int64_t last_published_at_ = 0;
+ int64_t run_duration_;
+
+ // We force turn off the trivial data check, because the ROS message data type
+ // has a defined constructor with code in it. That said, the code really is
+ // pretty trivial so it is safe to use in real-time. We thus disable the trivial
+ // type check manually.
+ std::shared_ptr<cactus_rt::ros2::Publisher<RealtimeType, RosType, false>> publisher_;
+
+ static cactus_rt::CyclicThreadConfig CreateThreadConfig() {
+ cactus_rt::CyclicThreadConfig thread_config;
+ thread_config.period_ns = 1'000'000;
+ thread_config.cpu_affinity = std::vector<size_t>{2};
+ thread_config.SetFifoScheduler(80);
+
+ return thread_config;
+ }
+
+ public:
+ explicit RTROS2PublisherThread(std::chrono::nanoseconds run_duration = std::chrono::seconds(30))
+ : cactus_rt::CyclicThread("RTROS2Publisher", CreateThreadConfig()),
+ run_duration_(run_duration.count()) {}
+
+ void InitializeForRos2() override {
+ publisher_ = ros2_adapter_->CreatePublisher<RealtimeType, RosType, false>("/cactus_rt/simple", rclcpp::QoS(100));
+ }
+
+ protected:
+ bool Loop(int64_t elapsed_ns) noexcept override {
+ if (elapsed_ns - last_published_at_ > 10'000'000) {
+ last_published_at_ = elapsed_ns;
+
+ const auto span = Tracer().WithSpan("Publish");
+
+ std_msgs::msg::Int64 msg;
+ msg.data = elapsed_ns;
+ const auto success = publisher_->Publish(msg);
+ LOG_INFO(Logger(), "{} integer {}", success ? "Published" : "Did not publish", msg.data);
+ }
+
+ return elapsed_ns > run_duration_;
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+
+ const cactus_rt::AppConfig app_config;
+
+ cactus_rt::ros2::Ros2Adapter::Config ros2_adapter_config;
+ ros2_adapter_config.timer_interval = std::chrono::milliseconds(50);
+
+ cactus_rt::ros2::App app("SimpleDataROS2Publisher", app_config, ros2_adapter_config);
+ app.StartTraceSession("build/publisher.perfetto");
+
+ constexpr std::chrono::seconds time(30);
+ std::cout << "Testing RT loop for " << time.count() << " seconds.\n";
+
+ auto thread = app.CreateROS2EnabledThread<RTROS2PublisherThread>(time);
+ app.RegisterThread(thread);
+
+ app.Start();
+
+ std::this_thread::sleep_for(time);
+
+ app.RequestStop();
+ app.Join();
+
+ std::cout << "Done\n";
+ return 0;
+}