Merge pull request #144 from cactusdynamics/roscon2024-slides

Add examples from ROScon 2024 slides

cmake/ros2.cmake

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

examples/ros2/roscon2024/CMakeLists.txt

@@ -0,0 +1,33 @@
+add_executable(cactus_rt_roscon2024_simple_example
+  simple_example.cc
+)
+
+target_link_libraries(cactus_rt_roscon2024_simple_example
+  PRIVATE
+  cactus_rt
+)
+
+setup_cactus_rt_target_options(cactus_rt_roscon2024_simple_example)
+
+find_package(geometry_msgs REQUIRED)
+
+add_executable(cactus_rt_roscon2024_ros_example
+  ros_example.cc
+)
+
+target_link_libraries(cactus_rt_roscon2024_ros_example
+  PRIVATE
+  cactus_rt
+)
+
+setup_cactus_rt_target_options(cactus_rt_roscon2024_ros_example)
+
+ament_target_dependencies(cactus_rt_roscon2024_ros_example
+  PUBLIC
+  geometry_msgs
+)
+
+install(
+  TARGETS cactus_rt_roscon2024_ros_example
+  DESTINATION lib/${PROJECT_NAME}
+)

examples/ros2/roscon2024/ros_example.cc

@@ -0,0 +1,135 @@
+#include <cactus_rt/experimental/random.h>
+#include <cactus_rt/ros2.h>
+#include <cactus_rt/rt.h>
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <random>
+
+using namespace std::literals::chrono_literals;
+using cactus_rt::experimental::random::RealNumber;
+using cactus_rt::ros2::StampedValue;
+
+struct Velocity2D {
+  double vx;
+  double vy;
+  double w;
+};
+
+// For demonstration purposes.
+void WasteTime(std::chrono::microseconds duration) {
+  const auto start = cactus_rt::NowNs();
+  auto       duration_ns = duration.count() * 1000;
+  while (cactus_rt::NowNs() - start < duration_ns) {
+  }
+}
+
+template <>
+struct rclcpp::TypeAdapter<Velocity2D, geometry_msgs::msg::Twist> {
+  using is_specialized = std::true_type;
+  using custom_type = Velocity2D;
+  using ros_message_type = geometry_msgs::msg::Twist;
+
+  static void convert_to_ros_message(const custom_type& source, ros_message_type& destination) {
+    destination.linear.x = source.vx;
+    destination.linear.y = source.vy;
+    destination.angular.z = source.w;
+  }
+
+  static void convert_to_custom(const ros_message_type& source, custom_type& destination) {
+    destination.vx = source.linear.x;
+    destination.vy = source.linear.y;
+    destination.w = source.angular.z;
+  }
+};
+
+class RT1000 : public cactus_rt::CyclicThread, public cactus_rt::ros2::Ros2ThreadMixin {
+  static cactus_rt::CyclicThreadConfig MakeConfig() {
+    cactus_rt::CyclicThreadConfig config;
+    config.period_ns = 1'000'000;  // 1ms period, 1000 Hz
+    config.SetFifoScheduler(80);   // SCHED_FIFO, rtprio = 80
+    return config;
+  }
+
+  cactus_rt::experimental::random::Xorshift64Rand rand_engine_{std::random_device{}()};
+
+  std::shared_ptr<cactus_rt::ros2::SubscriptionLatest<Velocity2D, geometry_msgs::msg::Twist>> cmd_vel_sub_;
+  std::shared_ptr<cactus_rt::ros2::Publisher<Velocity2D, geometry_msgs::msg::Twist>>          feedback_pub_;
+
+ public:
+  RT1000() : CyclicThread("RT1000", MakeConfig()) {
+    Logger()->set_log_level(quill::LogLevel::Debug);
+  }
+
+  void InitializeForRos2(cactus_rt::ros2::Ros2Adapter& ros2_adapter) final {
+    cmd_vel_sub_ = ros2_adapter.CreateSubscriptionForLatestValue<Velocity2D, geometry_msgs::msg::Twist>(
+      "/cmd_vel", rclcpp::QoS(10)
+    );
+
+    feedback_pub_ = ros2_adapter.CreatePublisher<Velocity2D, geometry_msgs::msg::Twist>(
+      "/cmd_vel_achieved", rclcpp::QoS(10)
+    );
+  }
+
+ protected:
+  LoopControl Loop(int64_t /*elapsed_ns*/) noexcept final {
+    StampedValue<Velocity2D> msg = cmd_vel_sub_->ReadLatest();
+
+    Velocity2D achieved_vel;
+    {
+      auto span = Tracer().WithSpan("Drive");
+      achieved_vel = Drive(msg.value.vx, msg.value.vy, msg.value.w);
+    }
+
+    {
+      auto span = Tracer().WithSpan("Publish");
+      feedback_pub_->Publish(achieved_vel);
+    }
+
+    LOG_DEBUG(
+      Logger(),
+      "Received id = {}; vx, vy, w = {}, {}, {}; Achieved vx, vy, w = {}, {}, {}",
+      msg.id,
+      msg.value.vx,
+      msg.value.vy,
+      msg.value.w,
+      achieved_vel.vx,
+      achieved_vel.vy,
+      achieved_vel.w
+    );
+
+    return LoopControl::Continue;
+  }
+
+ private:
+  Velocity2D Drive(double vx, double vy, double w) {
+    Velocity2D achieved_vel;
+    if (vx != 0.0) {
+      achieved_vel.vx = vx + (RealNumber<double>(rand_engine_) - 0.5);
+    }
+
+    if (vy != 0.0) {
+      achieved_vel.vy = vy + (RealNumber<double>(rand_engine_) - 0.5);
+    }
+
+    if (w != 0.0) {
+      achieved_vel.w = w + ((RealNumber<double>(rand_engine_) - 0.5) * 0.1);
+    }
+
+    // Waste between 100 - 200 us, uniform
+    WasteTime(std::chrono::microseconds(static_cast<int64_t>(RealNumber(rand_engine_) * 100.0F + 100.0F)));
+
+    return achieved_vel;
+  }
+};
+
+int main(int argc, const char* argv[]) {
+  cactus_rt::ros2::App app(argc, argv);
+
+  auto thread = app.CreateROS2EnabledThread<RT1000>();
+
+  app.StartTraceSession("build/ros.perfetto");
+  app.Start();
+  app.Join();
+
+  return 0;
+}

examples/ros2/roscon2024/simple_example.cc

@@ -0,0 +1,35 @@
+#include <cactus_rt/rt.h>
+
+using namespace std::literals::chrono_literals;
+
+class RT1000 : public cactus_rt::CyclicThread {
+  static cactus_rt::CyclicThreadConfig MakeConfig() {
+    cactus_rt::CyclicThreadConfig config;
+    config.period_ns = 1'000'000;  // 1ms period, 1000 Hz
+    config.SetFifoScheduler(80);   // SCHED_FIFO, rtprio = 80
+    return config;
+  }
+
+ public:
+  RT1000() : CyclicThread("RT1000", MakeConfig()) {
+    Logger()->set_log_level(quill::LogLevel::Debug);
+  }
+
+ protected:
+  LoopControl Loop(int64_t elapsed_ns) noexcept final {
+    LOG_DEBUG(Logger(), "This logs every iteration. elapsed={}ns", elapsed_ns);
+    LOG_DEBUG_LIMIT(1s, Logger(), "This logs every 1s");
+    return LoopControl::Continue;
+  }
+};
+
+int main() {
+  cactus_rt::App app;
+
+  auto thread = app.CreateThread<RT1000>();
+
+  app.Start();
+  app.Join();
+
+  return 0;
+}

include/cactus_rt/ros2.h

@@ -0,0 +1,4 @@
+#include "ros2/app.h"
+#include "ros2/publisher.h"
+#include "ros2/ros2_adapter.h"
+#include "ros2/subscription.h"

package.xml

@@ -11,8 +11,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-
   <build_depend>std_msgs</build_depend> <!-- Needed for the example -->
+  <build_depend>geometry_msgs</build_depend> <!-- Needed for the example -->
   <build_depend>protobuf-dev</build_depend>
   <exec_depend>protobuf</exec_depend>
   <test_depend>gtest</test_depend>