From 8cee2d0789836851bf9557b3b7da6f2440bd62de Mon Sep 17 00:00:00 2001 From: 2006wu Date: Fri, 13 Mar 2026 22:46:22 +0800 Subject: [PATCH 1/3] sima game over pub --- src/startup/include/startup.hpp | 5 ++++- src/startup/params/robot_config_black.yaml | 1 + src/startup/params/robot_config_default.yaml | 1 + src/startup/params/robot_config_white.yaml | 1 + src/startup/src/startup_new.cpp | 13 +++++++++++++ 5 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/startup/include/startup.hpp b/src/startup/include/startup.hpp index 55fce5b..136d17a 100644 --- a/src/startup/include/startup.hpp +++ b/src/startup/include/startup.hpp @@ -77,9 +77,11 @@ class StartUp : public rclcpp::Node { // game timer rclcpp::TimerBase::SharedPtr timer; rclcpp::Publisher::SharedPtr game_time_pub; + rclcpp::Publisher::SharedPtr sima_game_over_pub; std::shared_ptr rate; int game_time; - + bool sima_game_over_sent; + // State checker for other groups rclcpp::Publisher::SharedPtr are_you_ready_pub; rclcpp::Publisher::SharedPtr plan_file_pub; @@ -123,6 +125,7 @@ class StartUp : public rclcpp::Node { std::vector yellow_start_pose; int time_rate; int game_time_limit; + int sima_game_over_trigger_sec; int sima_tick_threshold; int group_num; }; \ No newline at end of file diff --git a/src/startup/params/robot_config_black.yaml b/src/startup/params/robot_config_black.yaml index 80e9034..191a329 100644 --- a/src/startup/params/robot_config_black.yaml +++ b/src/startup/params/robot_config_black.yaml @@ -6,6 +6,7 @@ time_rate: 100 # Timer rate in microseconds (100 = 10kHz) game_time: 100 # Total game time in seconds sima_tick_threshold: 85 # Seconds before sima starts + sima_game_over_trigger_sec: 99 # Seconds tell sima to stop group_num: 5 # Total number of groups (including index 0) # Plan Configuration diff --git a/src/startup/params/robot_config_default.yaml b/src/startup/params/robot_config_default.yaml index ff03404..f2763d7 100644 --- a/src/startup/params/robot_config_default.yaml +++ b/src/startup/params/robot_config_default.yaml @@ -6,6 +6,7 @@ time_rate: 100 # Timer rate in microseconds (100 = 10kHz) game_time: 100 # Total game time in seconds sima_tick_threshold: 85 # Seconds before sima starts + sima_game_over_trigger_sec: 99 # Seconds tell sima to stop group_num: 5 # Total number of groups (including index 0) # Plan Configuration diff --git a/src/startup/params/robot_config_white.yaml b/src/startup/params/robot_config_white.yaml index d78915d..1effe18 100644 --- a/src/startup/params/robot_config_white.yaml +++ b/src/startup/params/robot_config_white.yaml @@ -6,6 +6,7 @@ time_rate: 100 # Timer rate in microseconds (100 = 10kHz) game_time: 100 # Total game time in seconds sima_tick_threshold: 85 # Seconds before sima starts + sima_game_over_trigger_sec: 99 # Seconds tell sima to stop group_num: 5 # Total number of groups (including index 0) # Plan Configuration diff --git a/src/startup/src/startup_new.cpp b/src/startup/src/startup_new.cpp index 8342aca..c1fe6d3 100644 --- a/src/startup/src/startup_new.cpp +++ b/src/startup/src/startup_new.cpp @@ -13,6 +13,7 @@ StartUp::StartUp() : Node("startup_node"){ game_time_pub = this->create_publisher("/robot/startup/game_time", 2); rate = std::make_shared(time_rate); + sima_game_over_pub = this->create_publisher("/robot/startup/sima_game_over", 1); // State checker for other groups are_you_ready_pub = this->create_publisher("/robot/startup/are_you_ready", 2); @@ -50,6 +51,7 @@ StartUp::StartUp() : Node("startup_node"){ is_plugged = false; end_logged = false; game_time = 0; + sima_game_over_sent = false; } void StartUp::initParam() { @@ -57,6 +59,7 @@ void StartUp::initParam() { this->declare_parameter("time_rate", 100); this->declare_parameter("game_time", 100); this->declare_parameter("sima_tick_threshold", 85); + this->declare_parameter("sima_game_over_trigger_sec", 99); this->declare_parameter("group_num", 5); // Robot parameters @@ -69,6 +72,7 @@ void StartUp::initParam() { this->get_parameter("time_rate", time_rate); this->get_parameter("game_time", game_time_limit); this->get_parameter("sima_tick_threshold", sima_tick_threshold); + this->get_parameter("sima_game_over_trigger_sec", sima_game_over_trigger_sec); this->get_parameter("group_num", group_num); // Get robot parameters @@ -288,6 +292,15 @@ void StartUp::publishTime() { cur_time_msg.data = cur_time - start_time; game_time = cur_time_msg.data; game_time_pub->publish(cur_time_msg); + + if (!sima_game_over_sent && game_time >= sima_game_over_trigger_sec) { + std_msgs::msg::Bool msg; + msg.data = true; + sima_game_over_pub->publish(msg); + sima_game_over_sent = true; + RCLCPP_INFO(this->get_logger(), "[StartUp]: Sima game over signal sent at game time %f", game_time); + } + tickLittleSima(game_time); } From e607ba24ddab04161f721e97c313081a8edbe677 Mon Sep 17 00:00:00 2001 From: 2006wu Date: Sat, 14 Mar 2026 16:36:16 +0800 Subject: [PATCH 2/3] add log in MissionChecker & FirmwareReceiver --- src/bt_core/src/MissionChecker.cpp | 4 +++- src/sensors/src/FirmwareReceiver.cpp | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/bt_core/src/MissionChecker.cpp b/src/bt_core/src/MissionChecker.cpp index 3973c88..e9b6cd6 100644 --- a/src/bt_core/src/MissionChecker.cpp +++ b/src/bt_core/src/MissionChecker.cpp @@ -64,7 +64,7 @@ BT::NodeStatus MissionChecker::onRunning() { if (checkCondition()) { auto elapsed = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time_).count(); - MC_INFO(node_, "%s confirmed by vision on side %d (took %ld ms)", + MC_INFO(node_, "[MissionChecker]: %s confirmed by vision on side %d (took %ld ms)", actionTypeToString(action_).c_str(), side_idx_, elapsed); return BT::NodeStatus::SUCCESS; } @@ -81,10 +81,12 @@ BT::NodeStatus MissionChecker::onRunning() { if(action_ == ActionType::TAKE){ robot_side_status[side_idx_] = FieldStatus::OCCUPIED; blackboard_->set>("robot_side_status", robot_side_status); + MC_INFO(node_, "[MissionChecker]: Marking side %d as OCCUPIED due to TAKE timeout", side_idx_); } else if(action_ == ActionType::PUT){ robot_side_status[side_idx_] = FieldStatus::EMPTY; blackboard_->set>("robot_side_status", robot_side_status); + MC_INFO(node_, "[MissionChecker]: Marking side %d as EMPTY due to PUT timeout", side_idx_); } return BT::NodeStatus::SUCCESS; // Don't block the game } diff --git a/src/sensors/src/FirmwareReceiver.cpp b/src/sensors/src/FirmwareReceiver.cpp index 8146748..54b6b99 100644 --- a/src/sensors/src/FirmwareReceiver.cpp +++ b/src/sensors/src/FirmwareReceiver.cpp @@ -100,7 +100,7 @@ void FirmwareReceiver::putFinishCallback(const std_msgs::msg::Int16::SharedPtr m robot_sides[side_idx] = FieldStatus::EMPTY; blackboard_->set>("robot_side_status", robot_sides); - FR_INFO(node_, "Put finish - set robot_side_status[%d] to EMPTY", side_idx); + FR_INFO(node_, "[FirmwareReceiver]: Put finish - set robot_side_status[%d] to EMPTY", side_idx); } void FirmwareReceiver::takeFinishCallback(const std_msgs::msg::Int16::SharedPtr msg) { @@ -120,10 +120,10 @@ void FirmwareReceiver::takeFinishCallback(const std_msgs::msg::Int16::SharedPtr robot_sides[side_idx] = FieldStatus::OCCUPIED; blackboard_->set>("robot_side_status", robot_sides); - FR_INFO(node_, "Take finish - set robot_side_status[%d] to OCCUPIED", side_idx); + FR_INFO(node_, "[FirmwareReceiver]: Take finish - set robot_side_status[%d] to OCCUPIED", side_idx); } BT::NodeStatus FirmwareReceiver::tick() { - RCLCPP_DEBUG(node_->get_logger(), "FirmwareReceiver tick"); + RCLCPP_DEBUG(node_->get_logger(), "[FirmwareReceiver]: tick"); return BT::NodeStatus::SUCCESS; } From bd7920b6fc86a92a990fcd038d037b854fbfe909 Mon Sep 17 00:00:00 2001 From: 2006wu Date: Thu, 26 Mar 2026 11:18:05 +0800 Subject: [PATCH 3/3] remove unnecessary log in missionchecker & firmwarechecker --- src/bt_core/src/MissionChecker.cpp | 6 +++--- src/sensors/src/FirmwareReceiver.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/bt_core/src/MissionChecker.cpp b/src/bt_core/src/MissionChecker.cpp index e9b6cd6..27da713 100644 --- a/src/bt_core/src/MissionChecker.cpp +++ b/src/bt_core/src/MissionChecker.cpp @@ -64,7 +64,7 @@ BT::NodeStatus MissionChecker::onRunning() { if (checkCondition()) { auto elapsed = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time_).count(); - MC_INFO(node_, "[MissionChecker]: %s confirmed by vision on side %d (took %ld ms)", + MC_INFO(node_, "%s confirmed by vision on side %d (took %ld ms)", actionTypeToString(action_).c_str(), side_idx_, elapsed); return BT::NodeStatus::SUCCESS; } @@ -81,12 +81,12 @@ BT::NodeStatus MissionChecker::onRunning() { if(action_ == ActionType::TAKE){ robot_side_status[side_idx_] = FieldStatus::OCCUPIED; blackboard_->set>("robot_side_status", robot_side_status); - MC_INFO(node_, "[MissionChecker]: Marking side %d as OCCUPIED due to TAKE timeout", side_idx_); + MC_INFO(node_, "Marking side %d as OCCUPIED due to TAKE timeout", side_idx_); } else if(action_ == ActionType::PUT){ robot_side_status[side_idx_] = FieldStatus::EMPTY; blackboard_->set>("robot_side_status", robot_side_status); - MC_INFO(node_, "[MissionChecker]: Marking side %d as EMPTY due to PUT timeout", side_idx_); + MC_INFO(node_, "Marking side %d as EMPTY due to PUT timeout", side_idx_); } return BT::NodeStatus::SUCCESS; // Don't block the game } diff --git a/src/sensors/src/FirmwareReceiver.cpp b/src/sensors/src/FirmwareReceiver.cpp index 54b6b99..88d75ee 100644 --- a/src/sensors/src/FirmwareReceiver.cpp +++ b/src/sensors/src/FirmwareReceiver.cpp @@ -100,7 +100,7 @@ void FirmwareReceiver::putFinishCallback(const std_msgs::msg::Int16::SharedPtr m robot_sides[side_idx] = FieldStatus::EMPTY; blackboard_->set>("robot_side_status", robot_sides); - FR_INFO(node_, "[FirmwareReceiver]: Put finish - set robot_side_status[%d] to EMPTY", side_idx); + FR_INFO(node_, "Put finish - set robot_side_status[%d] to EMPTY", side_idx); } void FirmwareReceiver::takeFinishCallback(const std_msgs::msg::Int16::SharedPtr msg) { @@ -120,7 +120,7 @@ void FirmwareReceiver::takeFinishCallback(const std_msgs::msg::Int16::SharedPtr robot_sides[side_idx] = FieldStatus::OCCUPIED; blackboard_->set>("robot_side_status", robot_sides); - FR_INFO(node_, "[FirmwareReceiver]: Take finish - set robot_side_status[%d] to OCCUPIED", side_idx); + FR_INFO(node_, "Take finish - set robot_side_status[%d] to OCCUPIED", side_idx); } BT::NodeStatus FirmwareReceiver::tick() {