Supply projectiles with expected number(50 or 100 each time for AI Ch…

…allenge) and Fix the potential bug for map operation in different threads

roborts_base/referee_system/referee_system.cpp

@@ -128,23 +128,35 @@ void RefereeSystem::SupplierStatusCallback(const std::shared_ptr<roborts_sdk::cm
 void RefereeSystem::RobotStatusCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_state> raw_robot_status){
   roborts_msgs::RobotStatus robot_status;
 
-  if(robot_id_ == 0xFF){
-    robot_id_ = raw_robot_status->robot_id;
+  if(robot_id_ != raw_robot_status->robot_id){
+
+    switch (raw_robot_status->robot_id){
+      case 3:
+        robot_status.id = 3;
+        robot_id_ = 3;
+        break;
+      case 4:
+        robot_status.id = 4;
+        robot_id_ = 4;
+        break;
+      case 13:
+        robot_status.id = 13;
+        robot_id_ = 13;
+        break;
+      case 14:
+        robot_status.id = 14;
+        robot_id_ = 14;
+        break;
+      default:
+        robot_status.id = raw_robot_status->robot_id;
+        LOG_ERROR<<"For AI challenge, please set robot id to Blue3/4 or Red3/4 in the referee system main control module";
+        return;
+    }
   }
-
-  switch (raw_robot_status->robot_id){
-    case 3:
-      robot_status.id = 3;break;
-    case 4:
-      robot_status.id = 4;break;
-    case 13:
-      robot_status.id = 13;break;
-    case 14:
-      robot_status.id = 14;break;
-    default:
-      LOG_ERROR<<"For AI challenge, please set robot id to Blue3/4 or Red3/4 in the referee system main control module";
-      return;
+  else{
+    robot_status.id = raw_robot_status->robot_id;
   }
+
   robot_status.level = raw_robot_status->robot_level;
   robot_status.remain_hp = raw_robot_status->remain_HP;
   robot_status.max_hp = raw_robot_status->max_HP;
@@ -187,17 +199,14 @@ void RefereeSystem::RobotShootCallback(const std::shared_ptr<roborts_sdk::cmd_sh
 }
 
 void RefereeSystem::ProjectileSupplyCallback(const roborts_msgs::ProjectileSupply::ConstPtr projectile_supply){
-  if(!projectile_supply->supply){
-    ROS_WARN("Projectile supply command is invalid, supply flag is false.");
-    return;
-  } else if (robot_id_ == 0xFF) {
+  if (robot_id_ == 0xFF) {
     ROS_ERROR("Can not get robot id before requesting for projectile supply.");
     return;
   }
   roborts_sdk::cmd_supply_projectile_booking raw_projectile_booking;
   raw_projectile_booking.supply_projectile_id = 1;
   raw_projectile_booking.supply_robot_id = robot_id_;
-  raw_projectile_booking.supply_num = 50;
+  raw_projectile_booking.supply_num = projectile_supply->number/50*50;
   projectile_supply_pub_->Publish(raw_projectile_booking);
 }
 

roborts_base/roborts_sdk/protocol/protocol.cpp

@@ -128,6 +128,7 @@ void Protocol::ReceivePool() {
     start_time = std::chrono::steady_clock::now();
     RecvContainer *container_ptr = Receive();
     if (container_ptr) {
+      std::lock_guard<std::mutex> lock(mutex_);
       if (buffer_pool_map_.count(std::make_pair(container_ptr->command_info.cmd_set,
                                                 container_ptr->command_info.cmd_id)) == 0) {
         buffer_pool_map_[std::make_pair(container_ptr->command_info.cmd_set,
@@ -158,19 +159,23 @@ void Protocol::ReceivePool() {
 bool Protocol::Take(const CommandInfo *command_info,
                     MessageHeader *message_header,
                     void *message_data) {
+
+  std::lock_guard<std::mutex> lock(mutex_);
   if (buffer_pool_map_.count(std::make_pair(command_info->cmd_set,
                                             command_info->cmd_id)) == 0) {
 //    DLOG_ERROR<<"take failed";
     return false;
   } else {
     //1 time copy
     RecvContainer container;
+
     if (!buffer_pool_map_[std::make_pair(command_info->cmd_set,
                                          command_info->cmd_id)]->Pop(container)) {
 //      DLOG_EVERY_N(ERROR, 100)<<"nothing to take";
       return false;
     }
 
+
     bool mismatch = false;
 
     if (int(container.command_info.need_ack) != int(command_info->need_ack)){
@@ -210,6 +215,7 @@ bool Protocol::Take(const CommandInfo *command_info,
                  <<", Get length: "<< int(container.command_info.length);
       mismatch = true;
     }
+
     if(mismatch){
       buffer_pool_map_[std::make_pair(command_info->cmd_set,
                                       command_info->cmd_id)]->Push(container);

roborts_base/roborts_sdk/protocol/protocol.h

@@ -514,6 +514,8 @@ class Protocol {
   std::thread send_poll_thread_;
   //! receive pool thread
   std::thread receive_pool_thread_;
+  //! mutex for map
+  std::mutex mutex_;
 };
 }
 #endif //ROBORTS_SDK_PROTOCOL_H

roborts_msgs/msg/referee_system/ProjectileSupply.msg

@@ -1,2 +1,2 @@
 #projectile supply
-bool supply
+uint8 number