diff --git a/src/ctrl_arbiter/CMakeLists.txt b/src/ctrl_arbiter/CMakeLists.txt new file mode 100644 index 0000000..0fac0db --- /dev/null +++ b/src/ctrl_arbiter/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(ctrl_arbiter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(mc REQUIRED) +find_package(std_msgs REQUIRED) + +file(GLOB SRC_FILES "src/*.cpp") + +add_executable(ctrl_arbiter_node ${SRC_FILES}) + +ament_target_dependencies(ctrl_arbiter_node + rclcpp + mc + std_msgs +) + +install(TARGETS + ctrl_arbiter_node + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/ctrl_arbiter/package.xml b/src/ctrl_arbiter/package.xml new file mode 100644 index 0000000..2b3e914 --- /dev/null +++ b/src/ctrl_arbiter/package.xml @@ -0,0 +1,22 @@ + + + + ctrl_arbiter + 0.0.0 + TODO: Package description + root + MIT + + ament_cmake + + rclcpp + mc + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/ctrl_arbiter/src/ctrl_arbiter.cpp new file mode 100644 index 0000000..89a8123 --- /dev/null +++ b/src/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -0,0 +1,118 @@ +#include "rclcpp/rclcpp.hpp" +#include "mc/msg/mc_ctrl.hpp" +#include +#include + +using namespace std::chrono_literals; + +class ArbitrationNode : public rclcpp::Node +{ +public: + ArbitrationNode() + : Node("control_arbitrator") + { + timeout_ms_ = 200; // 200毫秒超时阈值,可调整 + + publisher_ = this->create_publisher("/mc_ctrl", 10); + + sub_radio_ = this->create_subscription( + "/radio_mc_ctrl", 10, + [this](const mc::msg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + radio_msg_ = *msg; + radio_last_time_ = this->now(); + radio_valid_ = true; + }); + + sub_remote_ = this->create_subscription( + "/remote_mc_ctrl", 10, + [this](const mc::msg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + remote_msg_ = *msg; + remote_last_time_ = this->now(); + remote_valid_ = true; + }); + + sub_auto_ = this->create_subscription( + "/auto_mc_ctrl", 10, + [this](const mc::msg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + auto_msg_ = *msg; + auto_last_time_ = this->now(); + auto_valid_ = true; + }); + + timer_ = this->create_wall_timer(20ms, [this]() + { this->arbitrateAndPublish(); }); + } + +private: + void arbitrateAndPublish() + { + std::lock_guard lock(mutex_); + rclcpp::Time now = this->now(); + + // 检查每个控制源是否在线 + if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(radio_msg_); + return; + } + if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(remote_msg_); + return; + } + if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000) + { + publisher_->publish(auto_msg_); + return; + } + + // 所有控制源均失联,发布安全默认指令 + mc::msg::McCtrl safe_msg; + safe_msg.mcu_enabled = false; + safe_msg.brake = 1; + safe_msg.gear = 0; + safe_msg.rpm = 0; + safe_msg.brake_time_ms = 500; + safe_msg.angle = 0; + safe_msg.angle_speed = 120; + safe_msg.main_brush_lift = false; + safe_msg.edge_brush_lift = false; + safe_msg.vacuum = false; + safe_msg.spray = false; + safe_msg.mud_flap = false; + safe_msg.dust_shake = false; + safe_msg.main_brush_spin = false; + safe_msg.edge_brush_spin = false; + + publisher_->publish(safe_msg); + } + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::Subscription::SharedPtr sub_radio_, sub_remote_, sub_auto_; + rclcpp::TimerBase::SharedPtr timer_; + + mc::msg::McCtrl radio_msg_, remote_msg_, auto_msg_; + rclcpp::Time radio_last_time_, remote_last_time_, auto_last_time_; + bool radio_valid_ = false; + bool remote_valid_ = false; + bool auto_valid_ = false; + + std::mutex mutex_; + int timeout_ms_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 4fe46f8..287c3ea 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -23,7 +23,7 @@ public: } // 发布控制指令消息的发布器 - pub_ = this->create_publisher("mc_ctrl", 10); + pub_ = this->create_publisher("radio_mc_ctrl", 10); // 订阅CAN反馈的回调函数 can_sub_ = this->create_subscription( @@ -203,7 +203,7 @@ private: } else { - RCLCPP_INFO(this->get_logger(), "Waiting for remote control data..."); + RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); return; // 下次循环,等待数据安全 } }