diff --git a/src/bt_core/src/MissionChecker.cpp b/src/bt_core/src/MissionChecker.cpp index 3973c88..27da713 100644 --- a/src/bt_core/src/MissionChecker.cpp +++ b/src/bt_core/src/MissionChecker.cpp @@ -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_, "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_, "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..88d75ee 100644 --- a/src/sensors/src/FirmwareReceiver.cpp +++ b/src/sensors/src/FirmwareReceiver.cpp @@ -124,6 +124,6 @@ void FirmwareReceiver::takeFinishCallback(const std_msgs::msg::Int16::SharedPtr } BT::NodeStatus FirmwareReceiver::tick() { - RCLCPP_DEBUG(node_->get_logger(), "FirmwareReceiver tick"); + RCLCPP_DEBUG(node_->get_logger(), "[FirmwareReceiver]: tick"); return BT::NodeStatus::SUCCESS; } 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); }