Feat/score again

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<std_msgs::msg::Int32> msg);
+    void hazelnutBonusScoreCallback(const std::shared_ptr<std_msgs::msg::Int32> msg);
+    void simaPantryScoreCallback(const std::shared_ptr<std_msgs::msg::Int32> 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;
-};
+
+    // score
+    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr game_score_pub;
+    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr hazelnut_storage_score_sub;
+    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr hazelnut_bonus_score_sub;
+    rclcpp::Subscription<std_msgs::msg::Int32>::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;
+};

src/startup/src/startup_new.cpp

@@ -1,6 +1,10 @@
 #include "startup.hpp"
 #include <algorithm>
 
+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<std_msgs::msg::String>("/robot/startup/plan_file", 2);
     start_signal_client = this->create_client<std_srvs::srv::SetBool>(
         "/robot/start_signal");
+
+    // score
+    game_score_pub = this->create_publisher<std_msgs::msg::Int32>("/game_score", 2);
+    hazelnut_storage_score_sub = this->create_subscription<std_msgs::msg::Int32>(
+        "/hazelnut_storage_score", 2,
+        std::bind(&StartUp::hazelnutStorageScoreCallback, this, std::placeholders::_1));
+    hazelnut_bonus_score_sub = this->create_subscription<std_msgs::msg::Int32>(
+        "/hazelnut_bonus_score", 2,
+        std::bind(&StartUp::hazelnutBonusScoreCallback, this, std::placeholders::_1));
+    sima_pantry_score_sub = this->create_subscription<std_msgs::msg::Int32>(
+        "/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<double>(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<std_msgs::msg::Int32> msg) {
+    hazelnut_storage_score = msg->data;
+}
+
+void StartUp::hazelnutBonusScoreCallback(const std::shared_ptr<std_msgs::msg::Int32> msg) {
+    hazelnut_bonus_score = msg->data;
+}
+
+void StartUp::simaPantryScoreCallback(const std::shared_ptr<std_msgs::msg::Int32> 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;
-}
+}