diff --git a/.gitignore b/.gitignore index 01b9655..2bb4ecf 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,5 @@ build/ .cache/ .vscode/settings.json +log/ +install/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 313e94e..3ac3273 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -165,13 +165,13 @@ if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME}) endif() endif() -setup_cactus_rt_target_options(cactus_rt) - # ROS 2 build support if (ENABLE_ROS2) include(cmake/ros2.cmake) endif() +setup_cactus_rt_target_options(cactus_rt) + # Build tests, examples, docs, only if this project is not embedded in another # project. if(${CMAKE_PROJECT_NAME} STREQUAL ${PROJECT_NAME}) diff --git a/cmake/ros2.cmake b/cmake/ros2.cmake index 752579b..3e76606 100644 --- a/cmake/ros2.cmake +++ b/cmake/ros2.cmake @@ -1,6 +1,11 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +target_sources(cactus_rt + PRIVATE + src/cactus_rt/ros2/ros2_adapter.cc +) + ament_target_dependencies(cactus_rt PUBLIC rclcpp diff --git a/examples/ros2_example/CMakeLists.txt b/examples/ros2_example/CMakeLists.txt index 8942035..fa9be3d 100644 --- a/examples/ros2_example/CMakeLists.txt +++ b/examples/ros2_example/CMakeLists.txt @@ -1,3 +1,5 @@ +find_package(std_msgs REQUIRED) + add_executable(rt_ros2_example main.cc ) @@ -9,6 +11,11 @@ target_link_libraries(rt_ros2_example setup_cactus_rt_target_options(rt_ros2_example) +ament_target_dependencies(rt_ros2_example + PUBLIC + std_msgs +) + install( TARGETS rt_ros2_example DESTINATION lib/${PROJECT_NAME} diff --git a/examples/ros2_example/main.cc b/examples/ros2_example/main.cc index 4cce7f6..a2bb159 100644 --- a/examples/ros2_example/main.cc +++ b/examples/ros2_example/main.cc @@ -1,3 +1,90 @@ -int main() { +#include +#include + +#include +#include +#include + +using cactus_rt::App; +using cactus_rt::CyclicThread; + +struct RealtimeData { + int64_t data; + + RealtimeData() = default; + RealtimeData(int64_t d) : data(d) {} +}; +using RosData = std_msgs::msg::Int64; + +namespace { +void RealtimeToROS2ConverterFunc(const RealtimeData& rt_data, RosData& ros_data) { + // A bit of a silly example, but gets the point across. + ros_data.data = rt_data.data; +} +} // namespace + +/** + * This is a no-op thread that does nothing at 1 kHz. + */ +class ExampleRTThread : public CyclicThread { + int64_t loop_counter_ = 0; + std::shared_ptr ros2_adapter_; + std::shared_ptr> publisher_; + + static cactus_rt::CyclicThreadConfig CreateThreadConfig() { + cactus_rt::CyclicThreadConfig thread_config; + thread_config.period_ns = 1'000'000; + thread_config.cpu_affinity = std::vector{2}; + thread_config.SetFifoScheduler(80); + + // thread_config.tracer_config.trace_sleep = true; + thread_config.tracer_config.trace_wakeup_latency = true; + return thread_config; + } + + public: + ExampleRTThread(const char* name, std::shared_ptr ros2_adapter) : CyclicThread(name, CreateThreadConfig()), ros2_adapter_(ros2_adapter) { + publisher_ = ros2_adapter_->CreatePublisher("/hello", rclcpp::QoS(10), RealtimeToROS2ConverterFunc); + } + + int64_t GetLoopCounter() const { + return loop_counter_; + } + + protected: + bool Loop(int64_t /*now*/) noexcept final { + loop_counter_++; + if (loop_counter_ % 1000 == 0) { + LOG_INFO(Logger(), "Loop {}", loop_counter_); + publisher_->Publish(loop_counter_); + } + return false; + } +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + App app; + app.StartTraceSession("build/data.perfetto"); + + auto ros2_adapter = std::make_shared("cactus_rt_ros2_example", cactus_rt::ros2::Ros2Adapter::Config{}); + auto thread = std::make_shared("ExampleRTThread", ros2_adapter); + + app.RegisterThread(thread); + + constexpr unsigned int time = 5; + std::cout << "Testing RT loop for " << time << " seconds.\n"; + + app.Start(); + + rclcpp::spin(ros2_adapter->Node()); + rclcpp::shutdown(); + + // std::this_thread::sleep_for(std::chrono::seconds(time)); + app.RequestStop(); + app.Join(); + + std::cout << "Number of loops executed: " << thread->GetLoopCounter() << "\n"; return 0; } diff --git a/include/cactus_rt/ros2/app.h b/include/cactus_rt/ros2/app.h new file mode 100644 index 0000000..c692497 --- /dev/null +++ b/include/cactus_rt/ros2/app.h @@ -0,0 +1,17 @@ +#ifndef CACTUS_RT_ROS2_APP_H_ +#define CACTUS_RT_ROS2_APP_H_ +#include + +#include "../app.h" +#include "ros2_adapter.h" + +namespace cactus_rt::ros2 { +class App : cactus_rt::App { + std::shared_ptr ros2_adapter; + + public: + explicit App(std::string name = "RTROS2App", cactus_rt::AppConfig config = cactus_rt::AppConfig()); + ~App() override; +}; +} // namespace cactus_rt::ros2 +#endif diff --git a/include/cactus_rt/ros2/ros2_adapter.h b/include/cactus_rt/ros2/ros2_adapter.h index 69149fa..d249a38 100644 --- a/include/cactus_rt/ros2/ros2_adapter.h +++ b/include/cactus_rt/ros2/ros2_adapter.h @@ -1,4 +1,154 @@ +#ifndef CACTUS_RT_ROS2_ROS2_ADAPTER_H_ +#define CACTUS_RT_ROS2_ROS2_ADAPTER_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + namespace cactus_rt::ros2 { -class Ros2Adapter { + +class Ros2Adapter; + +template +using RealtimeToROS2Converter = std::function; + +template +using Ros2ToRealtimeConverter = std::function; + +class IPublisher { + friend class Ros2Adapter; + + // TODO: there are other rclcpp::Publisher::publish methods that can implement. + virtual void PublishToRos(const void* rt_msg) const = 0; + virtual std::pair DequeueFromRT() = 0; + + public: + virtual ~IPublisher() = 0; +}; + +template +class Publisher : public IPublisher { + typename rclcpp::Publisher::SharedPtr publisher_; + std::optional> converter_; + moodycamel::ReaderWriterQueue queue_; + + // This is used in dequeue from RT to + RealtimeT data_; + + void PublishToRos(const void* rt_msg) const override { + const auto* rt_msg_typed = static_cast(rt_msg); + + if (converter_) { + auto loaned_msg = publisher_->borrow_loaned_message(); + converter_.value()(*rt_msg_typed, loaned_msg.get()); + publisher_->publish(std::move(loaned_msg)); + } else { + if constexpr (std::is_same_v) { + const auto* ros_msg_typed = static_cast(rt_msg_typed); + publisher_->publish(*ros_msg_typed); + } else { + throw std::invalid_argument{"converter not specified but RealtimeT and RosT are not the same?!"}; + } + } + } + + std::pair DequeueFromRT() override { + // One copy + bool ok = queue_.try_dequeue(data_); + return std::make_pair(ok, &data_); + } + + public: + /** + * Constructs a publisher. Shouldn't be called directly. Only public to enable make_shared. + * + * @private + */ + Publisher( + typename rclcpp::Publisher::SharedPtr publisher, + std::optional> converter, + moodycamel::ReaderWriterQueue&& queue + ) : publisher_(publisher), converter_(converter), queue_(std::move(queue)) {} + + ~Publisher() override = default; + + template + bool Publish(Args&&... args) noexcept { + bool success = queue_.try_emplace(std::forward(args)...); + // TODO: Keep track of success/failed messages and expose that to be queried + return success; + } +}; + +class Ros2Adapter : private rclcpp::Node { + public: + struct Config { + /** + * The time interval for which the adapter node is polling for publisher data. + */ + std::chrono::microseconds timer_interval = std::chrono::microseconds(200'000); + }; + + private: + std::string node_name_; + + // Protects the vector of subscribers, publishers, and services + // This means that during the timer callback, no subscribers, publishers, services, and etc. can be changed. + std::mutex mut_; + + // Timer callback id + + // Publisher data + // TODO: the shared_ptr is supposed to give this to the real-time thread, but not sure if this is the best idea. + std::vector> publishers_; + + public: + Ros2Adapter(const std::string& name_, const Config& config); + + std::shared_ptr Node() { + return shared_from_this(); + } + + template + std::shared_ptr> CreatePublisher( + const std::string& topic_name, + const rclcpp::QoS& qos, + std::optional> converter, + size_t rt_queue_size = 1000 + ) { + if (!converter) { + if constexpr (!(std::is_trivial_v && std::is_standard_layout_v && std::is_same_v)) { + throw std::invalid_argument("RosT and RealtimeT must be the same and must be a plain object for converter not to be specified"); + } + } + + typename rclcpp::Publisher::SharedPtr publisher = this->create_publisher(topic_name, qos); + typename moodycamel::ReaderWriterQueue queue{rt_queue_size}; + + auto publisher_bundle = std::make_shared>( + std::move(publisher), + converter, + std::move(queue) + ); + + const std::scoped_lock lock(mut_); + publishers_.push_back(publisher_bundle); + return publisher_bundle; + } + + private: + void TimerCallback(); }; } // namespace cactus_rt::ros2 + +#endif diff --git a/package.xml b/package.xml index 6196c81..f25f185 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,8 @@ ament_cmake + + std_msgs protobuf-dev protobuf gtest diff --git a/src/cactus_rt/ros2/ros2_adapter.cc b/src/cactus_rt/ros2/ros2_adapter.cc new file mode 100644 index 0000000..7345fe0 --- /dev/null +++ b/src/cactus_rt/ros2/ros2_adapter.cc @@ -0,0 +1,29 @@ +#include "cactus_rt/ros2/ros2_adapter.h" + +#include + +namespace cactus_rt::ros2 { + +IPublisher::~IPublisher() = default; + +Ros2Adapter::Ros2Adapter(const std::string& name, const Ros2Adapter::Config& config) : rclcpp::Node(name + "_ros_adapter") { + this->create_wall_timer(config.timer_interval, [this] { TimerCallback(); }); +} + +void Ros2Adapter::TimerCallback() { + for (const auto& publisher : publishers_) { + // TODO: hopefully the thread is not publishing so quickly that a single + // publisher monopolizes all resources. That said, if that happens the + // program is likely in bigger trouble anyway. + while (true) { + const auto [has_data, rt_msg_ptr] = publisher->DequeueFromRT(); + if (!has_data) { + break; + } + + publisher->PublishToRos(rt_msg_ptr); + } + } +} + +} // namespace cactus_rt::ros2