From 47c88aa441be234dce9b6d99d1461ef049bfd7c5 Mon Sep 17 00:00:00 2001 From: Shuhao Wu Date: Fri, 23 Aug 2024 01:08:49 -0400 Subject: [PATCH 1/2] Super ROS2 header --- include/cactus_rt/ros2.h | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 include/cactus_rt/ros2.h diff --git a/include/cactus_rt/ros2.h b/include/cactus_rt/ros2.h new file mode 100644 index 0000000..56cc22f --- /dev/null +++ b/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" From 8fc3b62c176e9764436c002616d2453907a05727 Mon Sep 17 00:00:00 2001 From: Shuhao Wu Date: Mon, 14 Oct 2024 12:02:07 -0400 Subject: [PATCH 2/2] Add examples from ROScon 2024 slides --- cmake/ros2.cmake | 1 + examples/ros2/roscon2024/CMakeLists.txt | 33 +++++ examples/ros2/roscon2024/ros_example.cc | 135 +++++++++++++++++++++ examples/ros2/roscon2024/simple_example.cc | 35 ++++++ package.xml | 2 +- 5 files changed, 205 insertions(+), 1 deletion(-) create mode 100644 examples/ros2/roscon2024/CMakeLists.txt create mode 100644 examples/ros2/roscon2024/ros_example.cc create mode 100644 examples/ros2/roscon2024/simple_example.cc diff --git a/cmake/ros2.cmake b/cmake/ros2.cmake index 5b4e7f7..10acdef 100644 --- a/cmake/ros2.cmake +++ b/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() diff --git a/examples/ros2/roscon2024/CMakeLists.txt b/examples/ros2/roscon2024/CMakeLists.txt new file mode 100644 index 0000000..18aeaef --- /dev/null +++ b/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} +) diff --git a/examples/ros2/roscon2024/ros_example.cc b/examples/ros2/roscon2024/ros_example.cc new file mode 100644 index 0000000..12de7cb --- /dev/null +++ b/examples/ros2/roscon2024/ros_example.cc @@ -0,0 +1,135 @@ +#include +#include +#include + +#include +#include + +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 { + 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> cmd_vel_sub_; + std::shared_ptr> 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( + "/cmd_vel", rclcpp::QoS(10) + ); + + feedback_pub_ = ros2_adapter.CreatePublisher( + "/cmd_vel_achieved", rclcpp::QoS(10) + ); + } + + protected: + LoopControl Loop(int64_t /*elapsed_ns*/) noexcept final { + StampedValue 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(rand_engine_) - 0.5); + } + + if (vy != 0.0) { + achieved_vel.vy = vy + (RealNumber(rand_engine_) - 0.5); + } + + if (w != 0.0) { + achieved_vel.w = w + ((RealNumber(rand_engine_) - 0.5) * 0.1); + } + + // Waste between 100 - 200 us, uniform + WasteTime(std::chrono::microseconds(static_cast(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(); + + app.StartTraceSession("build/ros.perfetto"); + app.Start(); + app.Join(); + + return 0; +} diff --git a/examples/ros2/roscon2024/simple_example.cc b/examples/ros2/roscon2024/simple_example.cc new file mode 100644 index 0000000..5823181 --- /dev/null +++ b/examples/ros2/roscon2024/simple_example.cc @@ -0,0 +1,35 @@ +#include + +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(); + + app.Start(); + app.Join(); + + return 0; +} diff --git a/package.xml b/package.xml index f25f185..3c29f77 100644 --- a/package.xml +++ b/package.xml @@ -11,8 +11,8 @@ ament_cmake - std_msgs + geometry_msgs protobuf-dev protobuf gtest