Reworked subscriber examples

examples/ros2/subscriber/CMakeLists.txt

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

examples/ros2/subscriber/complex_data.cc

@@ -0,0 +1,104 @@
+#include <cactus_rt/config.h>
+#include <cactus_rt/cyclic_thread.h>
+#include <cactus_rt/ros2/app.h>
+#include <quill/detail/LogMacros.h>
+
+#include <chrono>
+#include <cstdint>
+#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 RTROS2SubscriberThread : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+ int64_t last_msg_id_ = 0;
+ int64_t run_duration_;
+
+ std::shared_ptr<cactus_rt::ros2::SubscriptionLatest<RealtimeType, RosType>> subscription_;
+
+ 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 RTROS2SubscriberThread(std::chrono::nanoseconds run_duration = std::chrono::seconds(30))
+ : cactus_rt::CyclicThread("RTROS2Subscriber", CreateThreadConfig()),
+ run_duration_(run_duration.count()) {}
+
+ void InitializeForRos2() override {
+ subscription_ = ros2_adapter_->CreateSubscriptionForLatestValue<RealtimeType, RosType>("/cactus_rt/complex", rclcpp::QoS(100));
+ }
+
+ protected:
+ bool Loop(int64_t elapsed_ns) noexcept override {
+ cactus_rt::ros2::StampedValue<MyCoefficientData> msg;
+ {
+ const auto span = Tracer().WithSpan("Subscription::ReadLatest");
+ msg = subscription_->ReadLatest();
+ }
+
+ if (msg.id != last_msg_id_) {
+ LOG_INFO(Logger(), "Got new data at {}: {} [{}, {}, {}, {}]", elapsed_ns, msg.id, msg.value.a, msg.value.b, msg.value.c, msg.value.d);
+ last_msg_id_ = msg.id;
+ }
+
+ return elapsed_ns > run_duration_;
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+
+ cactus_rt::ros2::App app("SimpleDataROS2Subscriber");
+ app.StartTraceSession("build/subscriber.perfetto");
+
+ constexpr std::chrono::seconds time(30);
+ std::cout << "Testing RT loop for " << time.count() << " seconds.\n";
+
+ auto thread = app.CreateROS2EnabledThread<RTROS2SubscriberThread>(time);
+ app.RegisterThread(thread);
+
+ app.Start();
+
+ std::this_thread::sleep_for(time);
+
+ app.RequestStop();
+ app.Join();
+
+ std::cout << "Done\n";
+ return 0;
+}

examples/ros2/subscriber/simple_data.cc

@@ -0,0 +1,79 @@
+#include <cactus_rt/config.h>
+#include <cactus_rt/cyclic_thread.h>
+#include <cactus_rt/ros2/app.h>
+#include <quill/detail/LogMacros.h>
+
+#include <chrono>
+#include <cstdint>
+#include <std_msgs/msg/int64.hpp>
+
+using RealtimeType = std_msgs::msg::Int64;
+using RosType = std_msgs::msg::Int64;
+
+class RTROS2SubscriberThread : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+ int64_t last_msg_id_ = 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::SubscriptionLatest<RealtimeType, RosType, false>> subscription_;
+
+ 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 RTROS2SubscriberThread(std::chrono::nanoseconds run_duration = std::chrono::seconds(30))
+ : cactus_rt::CyclicThread("RTROS2Subscriber", CreateThreadConfig()),
+ run_duration_(run_duration.count()) {}
+
+ void InitializeForRos2() override {
+ subscription_ = ros2_adapter_->CreateSubscriptionForLatestValue<RealtimeType, RosType, false>("/cactus_rt/simple", rclcpp::QoS(100));
+ }
+
+ protected:
+ bool Loop(int64_t elapsed_ns) noexcept override {
+ cactus_rt::ros2::StampedValue<std_msgs::msg::Int64> msg;
+ {
+ const auto span = Tracer().WithSpan("Subscription::ReadLatest");
+ msg = subscription_->ReadLatest();
+ }
+
+ if (msg.id != last_msg_id_) {
+ LOG_INFO(Logger(), "Got new data at {}: {} {}", elapsed_ns, msg.id, msg.value.data);
+ last_msg_id_ = msg.id;
+ }
+
+ return elapsed_ns > run_duration_;
+ }
+};
+
+int main(int argc, char* argv[]) {
+ rclcpp::init(argc, argv);
+
+ cactus_rt::ros2::App app("SimpleDataROS2Subscriber");
+ app.StartTraceSession("build/subscriber.perfetto");
+
+ constexpr std::chrono::seconds time(30);
+ std::cout << "Testing RT loop for " << time.count() << " seconds.\n";
+
+ auto thread = app.CreateROS2EnabledThread<RTROS2SubscriberThread>(time);
+ app.RegisterThread(thread);
+
+ app.Start();
+
+ std::this_thread::sleep_for(time);
+
+ app.RequestStop();
+ app.Join();
+
+ std::cout << "Done\n";
+ return 0;
+}

include/cactus_rt/ros2/ros2_adapter.h

@@ -59,12 +59,12 @@ class Ros2Adapter {
  return publisher;
  }
 
- template <typename RealtimeT, typename RosT>
- std::shared_ptr<SubscriptionLatest<RealtimeT, RosT>> CreateSubscriptionForLatestValue(
+ template <typename RealtimeT, typename RosT, bool CheckForTrivialRealtimeT = true>
+ std::shared_ptr<SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>> CreateSubscriptionForLatestValue(
  const std::string& topic_name,
  const rclcpp::QoS& qos
  ) {
- auto subscriber = SubscriptionLatest<RealtimeT, RosT>::Create(*this->ros_node_, topic_name, qos);
+ auto subscriber = SubscriptionLatest<RealtimeT, RosT, CheckForTrivialRealtimeT>::Create(*this->ros_node_, topic_name, qos);
 
  const std::scoped_lock lock(mut_);
  subscriptions_.push_back(subscriber);