Auto commit at 2025-05-27 12:55:24

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()

src/ctrl_arbiter/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ctrl_arbiter</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="zxwl@56.com">root</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>mc</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ctrl_arbiter/src/ctrl_arbiter.cpp

@@ -0,0 +1,118 @@
+#include "rclcpp/rclcpp.hpp"
+#include "mc/msg/mc_ctrl.hpp"
+#include <chrono>
+#include <mutex>
+
+using namespace std::chrono_literals;
+
+class ArbitrationNode : public rclcpp::Node
+{
+public:
+    ArbitrationNode()
+        : Node("control_arbitrator")
+    {
+        timeout_ms_ = 200; // 200毫秒超时阈值，可调整
+
+        publisher_ = this->create_publisher<mc::msg::McCtrl>("/mc_ctrl", 10);
+
+        sub_radio_ = this->create_subscription<mc::msg::McCtrl>(
+            "/radio_mc_ctrl", 10,
+            [this](const mc::msg::McCtrl::SharedPtr msg)
+            {
+                std::lock_guard<std::mutex> lock(mutex_);
+                radio_msg_ = *msg;
+                radio_last_time_ = this->now();
+                radio_valid_ = true;
+            });
+
+        sub_remote_ = this->create_subscription<mc::msg::McCtrl>(
+            "/remote_mc_ctrl", 10,
+            [this](const mc::msg::McCtrl::SharedPtr msg)
+            {
+                std::lock_guard<std::mutex> lock(mutex_);
+                remote_msg_ = *msg;
+                remote_last_time_ = this->now();
+                remote_valid_ = true;
+            });
+
+        sub_auto_ = this->create_subscription<mc::msg::McCtrl>(
+            "/auto_mc_ctrl", 10,
+            [this](const mc::msg::McCtrl::SharedPtr msg)
+            {
+                std::lock_guard<std::mutex> 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<std::mutex> 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<mc::msg::McCtrl>::SharedPtr publisher_;
+
+    rclcpp::Subscription<mc::msg::McCtrl>::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<ArbitrationNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/radio_ctrl/src/radio_ctrl.cpp

@@ -23,7 +23,7 @@ public:
         }
 
         // 发布控制指令消息的发布器
-        pub_ = this->create_publisher<mc::msg::McCtrl>("mc_ctrl", 10);
+        pub_ = this->create_publisher<mc::msg::McCtrl>("radio_mc_ctrl", 10);
 
         // 订阅CAN反馈的回调函数
         can_sub_ = this->create_subscription<mc::msg::CanFrame>(
@@ -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; // 下次循环，等待数据安全
         }
     }