Merge pull request #7 from DIT-ROBOTICS/feat/game_over_pub

Feat/game over pub

Author: JustinShih0918

src/bt_core/src/MissionChecker.cpp

@@ -83,10 +83,12 @@ BT::NodeStatus MissionChecker::onRunning() {
         if(action_ == ActionType::TAKE){
             robot_side_status[side_idx_] = FieldStatus::OCCUPIED;
             blackboard_->set<std::vector<FieldStatus>>("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<std::vector<FieldStatus>>("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
     }

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;
 }

src/startup/include/startup.hpp

@@ -77,9 +77,11 @@ class StartUp : public rclcpp::Node {
     // game timer
     rclcpp::TimerBase::SharedPtr timer;
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr game_time_pub;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr sima_game_over_pub;
     std::shared_ptr<rclcpp::Rate> rate;
     int game_time;
-
+    bool sima_game_over_sent;
+ 
     // State checker for other groups
     rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr are_you_ready_pub;   
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr plan_file_pub;
@@ -123,6 +125,7 @@ class StartUp : public rclcpp::Node {
     std::vector<double> yellow_start_pose;
     int time_rate;
     int game_time_limit;
+    int sima_game_over_trigger_sec;
     int sima_tick_threshold;
     int group_num;
 };

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)
     stop_time: 98           # Hard stop time limit in seconds
     go_home_time: 90        # Time in seconds to trigger go home logic

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

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)
     stop_time: 98           # Hard stop time limit in seconds
     go_home_time: 90        # Time in seconds to trigger go home logic

src/startup/src/startup_new.cpp

@@ -13,6 +13,7 @@ StartUp::StartUp() : Node("startup_node"){
 
     game_time_pub = this->create_publisher<std_msgs::msg::Float32>("/robot/startup/game_time", 2);
     rate = std::make_shared<rclcpp::Rate>(time_rate);
+    sima_game_over_pub = this->create_publisher<std_msgs::msg::Bool>("/robot/startup/sima_game_over", 1);
 
     // State checker for other groups
     are_you_ready_pub = this->create_publisher<std_msgs::msg::Bool>("/robot/startup/are_you_ready", 2);
@@ -50,13 +51,15 @@ StartUp::StartUp() : Node("startup_node"){
     is_plugged = false;
     end_logged = false;
     game_time = 0;
+    sima_game_over_sent = false;
 }
 
 void StartUp::initParam() {
     // Timing parameters
     this->declare_parameter<int>("time_rate", 100);
     this->declare_parameter<int>("game_time", 100);
     this->declare_parameter<int>("sima_tick_threshold", 85);
+    this->declare_parameter<int>("sima_game_over_trigger_sec", 99);
     this->declare_parameter<int>("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);
 }