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; // 下次循环,等待数据安全
}
}