From 47f081f9e53fdc2c3a5bc1f7115c4f886a1ab8cc Mon Sep 17 00:00:00 2001 From: JustinShih Date: Fri, 8 May 2026 00:46:12 +0800 Subject: [PATCH 1/2] feat: add game score publishing and score callbacks --- src/startup/include/startup.hpp | 17 ++++++++++- src/startup/src/startup_new.cpp | 51 ++++++++++++++++++++++++++++++++- 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/src/startup/include/startup.hpp b/src/startup/include/startup.hpp index 7b89a66..546c3e8 100644 --- a/src/startup/include/startup.hpp +++ b/src/startup/include/startup.hpp @@ -69,6 +69,10 @@ class StartUp : public rclcpp::Node { void tickLittleSima(double game_time); void tickNinjaSima(); void publishTime(); + void publishGameScore(); + void hazelnutStorageScoreCallback(const std::shared_ptr msg); + void hazelnutBonusScoreCallback(const std::shared_ptr msg); + void simaPantryScoreCallback(const std::shared_ptr msg); bool gameOver(double game_time); // utils @@ -131,4 +135,15 @@ class StartUp : public rclcpp::Node { int sima_tick_threshold; int group_num; std::string sima_config_path; -}; \ No newline at end of file + + // score + rclcpp::Publisher::SharedPtr game_score_pub; + rclcpp::Subscription::SharedPtr hazelnut_storage_score_sub; + rclcpp::Subscription::SharedPtr hazelnut_bonus_score_sub; + rclcpp::Subscription::SharedPtr sima_pantry_score_sub; + int overall_score; + int hazelnut_storage_score; + int hazelnut_bonus_score; + int sima_pantry_score; + int game_score_publish_count; +}; diff --git a/src/startup/src/startup_new.cpp b/src/startup/src/startup_new.cpp index b66caaf..a3e2afc 100644 --- a/src/startup/src/startup_new.cpp +++ b/src/startup/src/startup_new.cpp @@ -1,6 +1,8 @@ #include "startup.hpp" #include +constexpr int SCORE_PUBLISH_TIME = 99; +constexpr int SCORE_PUBLISH_TIMES = 10; StartUp::StartUp() : Node("startup_node"){ // Load parameters first @@ -23,6 +25,18 @@ StartUp::StartUp() : Node("startup_node"){ plan_file_pub = this->create_publisher("/robot/startup/plan_file", 2); start_signal_client = this->create_client( "/robot/start_signal"); + + // score + game_score_pub = this->create_publisher("/game_score", 2); + hazelnut_storage_score_sub = this->create_subscription( + "/hazelnut_storage_score", 2, + std::bind(&StartUp::hazelnutStorageScoreCallback, this, std::placeholders::_1)); + hazelnut_bonus_score_sub = this->create_subscription( + "/hazelnut_bonus_score", 2, + std::bind(&StartUp::hazelnutBonusScoreCallback, this, std::placeholders::_1)); + sima_pantry_score_sub = this->create_subscription( + "/sima_pantry_score", 2, + std::bind(&StartUp::simaPantryScoreCallback, this, std::placeholders::_1)); rclcpp::QoS qos(1); qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); @@ -53,6 +67,11 @@ StartUp::StartUp() : Node("startup_node"){ end_logged = false; game_time = 0; sima_game_over_sent = false; + overall_score = 0; + hazelnut_storage_score = 0; + hazelnut_bonus_score = 0; + sima_pantry_score = 0; + game_score_publish_count = 0; } void StartUp::initParam() { @@ -349,9 +368,39 @@ void StartUp::publishTime() { RCLCPP_INFO(this->get_logger(), "[StartUp]: Sima game over signal sent at game time %f", static_cast(game_time)); } + if (game_time >= SCORE_PUBLISH_TIME && game_score_publish_count < SCORE_PUBLISH_TIMES) { + publishGameScore(); + } + tickLittleSima(game_time); } +void StartUp::publishGameScore() { + overall_score = CURSOR_POINT + HOME_POINT + hazelnut_storage_score + hazelnut_bonus_score + sima_pantry_score; + + std_msgs::msg::Int32 msg; + msg.data = overall_score; + game_score_pub->publish(msg); + game_score_publish_count++; + + RCLCPP_INFO( + this->get_logger(), + "[StartUp]: Game score published %d/%d: %d (cursor=%d, home=%d, hazelnut_storage=%d, hazelnut_bonus=%d, sima_pantry=%d)", + game_score_publish_count, SCORE_PUBLISH_TIMES, overall_score, CURSOR_POINT, HOME_POINT, hazelnut_storage_score, hazelnut_bonus_score, sima_pantry_score); +} + +void StartUp::hazelnutStorageScoreCallback(const std::shared_ptr msg) { + hazelnut_storage_score = msg->data; +} + +void StartUp::hazelnutBonusScoreCallback(const std::shared_ptr msg) { + hazelnut_bonus_score = msg->data; +} + +void StartUp::simaPantryScoreCallback(const std::shared_ptr msg) { + sima_pantry_score = msg->data; +} + bool StartUp::gameOver(double game_time) { if(game_time >= game_time_limit) { RCLCPP_INFO(this->get_logger(), "[StartUp]: Game over at game time: %f", game_time); @@ -372,4 +421,4 @@ int main(int argc, char** argv) { rclcpp::spin(startup_node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 512d033b3c34a7c86ad1dac8763b09bee626d53b Mon Sep 17 00:00:00 2001 From: JustinShih Date: Fri, 8 May 2026 00:52:30 +0800 Subject: [PATCH 2/2] fix: update game score calculation to use constants for cursor and home scores --- src/startup/src/startup_new.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/startup/src/startup_new.cpp b/src/startup/src/startup_new.cpp index a3e2afc..e256fb3 100644 --- a/src/startup/src/startup_new.cpp +++ b/src/startup/src/startup_new.cpp @@ -3,6 +3,8 @@ constexpr int SCORE_PUBLISH_TIME = 99; constexpr int SCORE_PUBLISH_TIMES = 10; +constexpr int CURSOR_SCORE = 10; +constexpr int HOME_SCORE = 10; StartUp::StartUp() : Node("startup_node"){ // Load parameters first @@ -376,7 +378,7 @@ void StartUp::publishTime() { } void StartUp::publishGameScore() { - overall_score = CURSOR_POINT + HOME_POINT + hazelnut_storage_score + hazelnut_bonus_score + sima_pantry_score; + overall_score = CURSOR_SCORE + HOME_SCORE + hazelnut_storage_score + hazelnut_bonus_score + sima_pantry_score; std_msgs::msg::Int32 msg; msg.data = overall_score; @@ -386,7 +388,7 @@ void StartUp::publishGameScore() { RCLCPP_INFO( this->get_logger(), "[StartUp]: Game score published %d/%d: %d (cursor=%d, home=%d, hazelnut_storage=%d, hazelnut_bonus=%d, sima_pantry=%d)", - game_score_publish_count, SCORE_PUBLISH_TIMES, overall_score, CURSOR_POINT, HOME_POINT, hazelnut_storage_score, hazelnut_bonus_score, sima_pantry_score); + game_score_publish_count, SCORE_PUBLISH_TIMES, overall_score, CURSOR_SCORE, HOME_SCORE, hazelnut_storage_score, hazelnut_bonus_score, sima_pantry_score); } void StartUp::hazelnutStorageScoreCallback(const std::shared_ptr msg) {