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..e256fb3 100644 --- a/src/startup/src/startup_new.cpp +++ b/src/startup/src/startup_new.cpp @@ -1,6 +1,10 @@ #include "startup.hpp" #include +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 @@ -23,6 +27,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 +69,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 +370,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_SCORE + HOME_SCORE + 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_SCORE, HOME_SCORE, 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 +423,4 @@ int main(int argc, char** argv) { rclcpp::spin(startup_node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +}