Update referee system information in the roborts_base and roborts_msgs

roborts_base/CMakeLists.txt

@@ -17,6 +17,7 @@ add_executable(roborts_base_node
   roborts_base_node.cpp
   chassis/chassis.cpp
   gimbal/gimbal.cpp
+  referee_system/referee_system.cpp
   roborts_sdk/dispatch/execution.cpp
   roborts_sdk/dispatch/handle.cpp
   roborts_sdk/protocol/protocol.cpp

roborts_base/chassis/chassis.cpp

@@ -24,7 +24,28 @@ Chassis::Chassis(std::shared_ptr<roborts_sdk::Handle> handle):
   SDK_Init();
   ROS_Init();
 }
+Chassis::~Chassis(){
+  if(heartbeat_thread_.joinable()){
+    heartbeat_thread_.join();
+  }
+}
 void Chassis::SDK_Init(){
+
+  verison_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id,roborts_sdk::cmd_version_id>
+      (UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
+       MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+  roborts_sdk::cmd_version_id version_cmd;
+  version_cmd.version_id=0;
+  auto version = std::make_shared<roborts_sdk::cmd_version_id>(version_cmd);
+  verison_client_->AsyncSendRequest(version,
+                                    [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                                                           roborts_sdk::cmd_version_id>::SharedFuture future) {
+                                      LOG_INFO << "Chassis Firmware Version: " << int(future.get()->version_id>>24&0xFF) <<"."
+                                               <<int(future.get()->version_id>>16&0xFF)<<"."
+                                               <<int(future.get()->version_id>>8&0xFF)<<"."
+                                               <<int(future.get()->version_id&0xFF);
+                                    });
+
   handle_->CreateSubscriber<roborts_sdk::cmd_chassis_info>(CHASSIS_CMD_SET, CMD_PUSH_CHASSIS_INFO,
                                                            CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
                                                            std::bind(&Chassis::ChassisInfoCallback, this, std::placeholders::_1));
@@ -37,6 +58,19 @@ void Chassis::SDK_Init(){
   chassis_spd_acc_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_chassis_spd_acc>(CHASSIS_CMD_SET, CMD_SET_CHASSIS_SPD_ACC,
                                                                                     MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
 
+  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
+                                                                           MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+  heartbeat_thread_ = std::thread([this]{
+                                        roborts_sdk::cmd_heartbeat heartbeat;
+                                        heartbeat.heartbeat=0;
+                                        while(ros::ok()){
+                                          heartbeat_pub_->Publish(heartbeat);
+                                          std::this_thread::sleep_for(std::chrono::milliseconds(300));
+                                        }
+                                      }
+  );
+
+
 }
 void Chassis::ROS_Init(){
   //ros publisher

roborts_base/chassis/chassis.h

@@ -35,7 +35,7 @@ class Chassis {
   /**
    * @brief Destructor of chassis
    */
-  ~Chassis() = default;
+  ~Chassis();
 
  private:
   /**
@@ -74,6 +74,15 @@ class Chassis {
 
   //! sdk handler
   std::shared_ptr<roborts_sdk::Handle> handle_;
+  //! sdk version client
+  std::shared_ptr<roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                                      roborts_sdk::cmd_version_id>> verison_client_;
+
+  //! sdk heartbeat thread
+  std::thread heartbeat_thread_;
+  //! sdk publisher for heartbeat
+  std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_heartbeat>> heartbeat_pub_;
+
   //! sdk publisher for chassis speed control
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_chassis_speed>> chassis_speed_pub_;
   //! sdk publisher for chassis speed and acceleration control

roborts_base/gimbal/gimbal.cpp

@@ -25,20 +25,53 @@ Gimbal::Gimbal(std::shared_ptr<roborts_sdk::Handle> handle):
   ROS_Init();
 }
 
+Gimbal::~Gimbal(){
+  if(heartbeat_thread_.joinable()){
+    heartbeat_thread_.join();
+  }
+}
+
 void Gimbal::SDK_Init(){
+
+  verison_client_ = handle_->CreateClient<roborts_sdk::cmd_version_id,roborts_sdk::cmd_version_id>
+      (UNIVERSAL_CMD_SET, CMD_REPORT_VERSION,
+       MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  roborts_sdk::cmd_version_id version_cmd;
+  version_cmd.version_id=0;
+  auto version = std::make_shared<roborts_sdk::cmd_version_id>(version_cmd);
+  verison_client_->AsyncSendRequest(version,
+                                    [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                                                           roborts_sdk::cmd_version_id>::SharedFuture future) {
+                                      LOG_INFO << "Gimbal Firmware Version: " << int(future.get()->version_id>>24&0xFF) <<"."
+                                               <<int(future.get()->version_id>>16&0xFF)<<"."
+                                               <<int(future.get()->version_id>>8&0xFF)<<"."
+                                               <<int(future.get()->version_id&0xFF);
+                                    });
+
   handle_->CreateSubscriber<roborts_sdk::cmd_gimbal_info>(GIMBAL_CMD_SET, CMD_PUSH_GIMBAL_INFO,
                                                           GIMBAL_ADDRESS, BROADCAST_ADDRESS,
                                                           std::bind(&Gimbal::GimbalInfoCallback, this, std::placeholders::_1));
 
   gimbal_angle_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_gimbal_angle>(GIMBAL_CMD_SET, CMD_SET_GIMBAL_ANGLE,
                                                                               MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  gimbal_mode_pub_= handle_->CreatePublisher<roborts_sdk::gimbal_mode_e>(GIMBAL_CMD_SET, CMD_SET_GIMBAL_MODE,
-                                                                         MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  fric_wheel_pub_= handle_->CreatePublisher<roborts_sdk::cmd_fric_wheel_speed>(GIMBAL_CMD_SET, CMD_SET_FRIC_WHEEL_SPEED,
-                                                                               MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-  gimbal_shoot_pub_= handle_->CreatePublisher<roborts_sdk::cmd_shoot_info>(GIMBAL_CMD_SET, CMD_SET_SHOOT_INFO,
-                                                                             MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
-
+  gimbal_mode_pub_ = handle_->CreatePublisher<roborts_sdk::gimbal_mode_e>(GIMBAL_CMD_SET, CMD_SET_GIMBAL_MODE,
+                                                                          MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  fric_wheel_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_fric_wheel_speed>(GIMBAL_CMD_SET, CMD_SET_FRIC_WHEEL_SPEED,
+                                                                                MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  gimbal_shoot_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_shoot_info>(GIMBAL_CMD_SET, CMD_SET_SHOOT_INFO,
+                                                                            MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+
+  heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
+                                                                        MANIFOLD2_ADDRESS, GIMBAL_ADDRESS);
+  heartbeat_thread_ = std::thread([this]{
+                                    roborts_sdk::cmd_heartbeat heartbeat;
+                                    heartbeat.heartbeat=0;
+                                    while(ros::ok()){
+                                      heartbeat_pub_->Publish(heartbeat);
+                                      std::this_thread::sleep_for(std::chrono::milliseconds(300));
+                                    }
+                                  }
+  );
 }
 
 void Gimbal::ROS_Init(){

roborts_base/gimbal/gimbal.h

@@ -34,7 +34,7 @@ class Gimbal {
     /**
    * @brief Destructor of gimbal
    */
-  ~Gimbal() = default;
+  ~Gimbal();
  private:
   /**
    * @brief Initialization of sdk
@@ -83,6 +83,16 @@ class Gimbal {
 
   //! sdk handler
   std::shared_ptr<roborts_sdk::Handle> handle_;
+  //! sdk version client
+  std::shared_ptr<roborts_sdk::Client<roborts_sdk::cmd_version_id,
+                                      roborts_sdk::cmd_version_id>> verison_client_;
+
+  //! sdk heartbeat thread
+  std::thread heartbeat_thread_;
+  //! sdk publisher for heartbeat
+  std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_heartbeat>> heartbeat_pub_;
+
+
   //! sdk publisher for gimbal angle control
   std::shared_ptr<roborts_sdk::Publisher<roborts_sdk::cmd_gimbal_angle>>     gimbal_angle_pub_;
   //! sdk publisher for gimbal mode set

roborts_base/referee_system/referee_system.cpp

@@ -0,0 +1,203 @@
+#include "referee_system.h"
+namespace roborts_base {
+RefereeSystem::RefereeSystem(std::shared_ptr<roborts_sdk::Handle> handle) :
+    handle_(handle) {
+  SDK_Init();
+  ROS_Init();
+}
+void RefereeSystem::SDK_Init() {
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_state>(REFEREE_GAME_CMD_SET, CMD_GAME_STATUS,
+                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                         std::bind(&RefereeSystem::GameStateCallback,
+                                                                   this,
+                                                                   std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_result>(REFEREE_GAME_CMD_SET, CMD_GAME_RESULT,
+                                                          CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                          std::bind(&RefereeSystem::GameResultCallback,
+                                                                    this,
+                                                                    std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_survivors>(REFEREE_GAME_CMD_SET, CMD_GAME_SURVIVAL,
+                                                                   CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                                   std::bind(&RefereeSystem::GameSurvivorCallback,
+                                                                             this,
+                                                                             std::placeholders::_1));
+
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_event_data>(REFEREE_BATTLEFIELD_CMD_SET, CMD_BATTLEFIELD_EVENT,
+                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                         std::bind(&RefereeSystem::GameEventCallback,
+                                                                   this,
+                                                                   std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_supply_projectile_action>(REFEREE_BATTLEFIELD_CMD_SET, CMD_SUPPLIER_ACTION,
+                                                                       CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                                       std::bind(&RefereeSystem::SupplierStatusCallback,
+                                                                                 this,
+                                                                                 std::placeholders::_1));
+
+
+  handle_->CreateSubscriber<roborts_sdk::cmd_game_robot_state>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_STATUS,
+                                                               CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                               std::bind(&RefereeSystem::RobotStatusCallback,
+                                                                         this,
+                                                                         std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_power_heat_data>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_POWER_HEAT,
+                                                              CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                              std::bind(&RefereeSystem::RobotHeatCallback,
+                                                                        this,
+                                                                        std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_buff_musk>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_BUFF,
+                                                        CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                        std::bind(&RefereeSystem::RobotBonusCallback,
+                                                                  this,
+                                                                  std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_robot_hurt>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_HURT,
+                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                         std::bind(&RefereeSystem::RobotDamageCallback,
+                                                                   this,
+                                                                   std::placeholders::_1));
+  handle_->CreateSubscriber<roborts_sdk::cmd_shoot_data>(REFEREE_ROBOT_CMD_SET, CMD_ROBOT_SHOOT,
+                                                         CHASSIS_ADDRESS, MANIFOLD2_ADDRESS,
+                                                         std::bind(&RefereeSystem::RobotShootCallback,
+                                                                   this,
+                                                                   std::placeholders::_1));
+
+
+
+  projectile_supply_pub_ =
+      handle_->CreatePublisher<roborts_sdk::cmd_supply_projectile_booking>(REFEREE_SEND_CMD_SET, CMD_REFEREE_SEND_DATA,
+                                                                           MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+
+
+
+}
+void RefereeSystem::ROS_Init() {
+  //ros publisher
+  ros_game_status_pub_ = ros_nh_.advertise<roborts_msgs::GameStatus>("game_status", 30);
+  ros_game_result_pub_ = ros_nh_.advertise<roborts_msgs::GameResult>("game_result", 30);
+  ros_game_survival_pub_ = ros_nh_.advertise<roborts_msgs::GameSurvivor>("game_survivor", 30);
+
+  ros_bonus_status_pub_ = ros_nh_.advertise<roborts_msgs::BonusStatus>("field_bonus_status", 30);
+  ros_supplier_status_pub_ = ros_nh_.advertise<roborts_msgs::SupplierStatus>("field_supplier_status", 30);
+
+  ros_robot_status_pub_ = ros_nh_.advertise<roborts_msgs::RobotStatus>("robot_status", 30);
+  ros_robot_heat_pub_ = ros_nh_.advertise<roborts_msgs::RobotHeat>("robot_heat", 30);
+  ros_robot_bonus_pub_ = ros_nh_.advertise<roborts_msgs::RobotBonus>("robot_bonus", 30);
+  ros_robot_damage_pub_ = ros_nh_.advertise<roborts_msgs::RobotDamage>("robot_damage", 30);
+  ros_robot_shoot_pub_ = ros_nh_.advertise<roborts_msgs::RobotShoot>("robot_shoot", 30);
+
+  //ros subscriber
+  ros_sub_projectile_supply_ = ros_nh_.subscribe("projectile_supply", 1, &RefereeSystem::ProjectileSupplyCallback, this);
+
+}
+
+void RefereeSystem::GameStateCallback(const std::shared_ptr<roborts_sdk::cmd_game_state> raw_game_status){
+  roborts_msgs::GameStatus game_status;
+  game_status.game_status = raw_game_status->game_progress;
+  game_status.remaining_time = raw_game_status->stage_remain_time;
+  ros_game_status_pub_.publish(game_status);
+}
+
+void RefereeSystem::GameResultCallback(const std::shared_ptr<roborts_sdk::cmd_game_result> raw_game_result){
+  roborts_msgs::GameResult game_result;
+  game_result.result = raw_game_result->winner;
+  ros_game_result_pub_.publish(game_result);
+}
+
+void RefereeSystem::GameSurvivorCallback(const std::shared_ptr<roborts_sdk::cmd_game_robot_survivors> raw_game_survivor){
+  roborts_msgs::GameSurvivor game_survivor;
+  game_survivor.red3 = raw_game_survivor->robot_legion>>2&1;
+  game_survivor.red4 = raw_game_survivor->robot_legion>>3&1;
+  game_survivor.blue3 = raw_game_survivor->robot_legion>>10&1;
+  game_survivor.blue4 = raw_game_survivor->robot_legion>>11&1;
+  ros_game_survival_pub_.publish(game_survivor);
+}
+
+void RefereeSystem::GameEventCallback(const std::shared_ptr<roborts_sdk::cmd_event_data> raw_game_event){
+  roborts_msgs::BonusStatus bonus_status;
+  bonus_status.red_bonus = raw_game_event->event_type>>12&3;
+  bonus_status.blue_bonus = raw_game_event->event_type>>14&3;
+  ros_bonus_status_pub_.publish(bonus_status);
+}
+
+void RefereeSystem::SupplierStatusCallback(const std::shared_ptr<roborts_sdk::cmd_supply_projectile_action> raw_supplier_status){
+  roborts_msgs::SupplierStatus supplier_status;
+  supplier_status.status = raw_supplier_status->supply_projectile_step;
+  ros_supplier_status_pub_.publish(supplier_status);
+}
+
+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;
+  }
+
+  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;
+  }
+  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;
+  robot_status.heat_cooling_limit = raw_robot_status->shooter_heat0_cooling_limit;
+  robot_status.heat_cooling_rate = raw_robot_status->shooter_heat0_cooling_rate;
+  robot_status.chassis_output = raw_robot_status->mains_power_chassis_output;
+  robot_status.gimbal_output = raw_robot_status->mains_power_gimbal_output;
+  robot_status.shooter_output = raw_robot_status->mains_power_shooter_output;
+  ros_robot_status_pub_.publish(robot_status);
+}
+
+void RefereeSystem::RobotHeatCallback(const std::shared_ptr<roborts_sdk::cmd_power_heat_data> raw_robot_heat){
+  roborts_msgs::RobotHeat robot_heat;
+  robot_heat.chassis_volt = raw_robot_heat->chassis_volt;
+  robot_heat.chassis_current = raw_robot_heat->chassis_current;
+  robot_heat.chassis_power = raw_robot_heat->chassis_power;
+  robot_heat.chassis_power_buffer = raw_robot_heat->chassis_power_buffer;
+  ros_robot_heat_pub_.publish(robot_heat);
+}
+
+void RefereeSystem::RobotBonusCallback(const std::shared_ptr<roborts_sdk::cmd_buff_musk> raw_robot_bonus){
+  roborts_msgs::RobotBonus robot_bonus;
+  robot_bonus.bonus = raw_robot_bonus->power_rune_buff>>2&1;
+  ros_robot_heat_pub_.publish(robot_bonus);
+}
+
+void RefereeSystem::RobotDamageCallback(const std::shared_ptr<roborts_sdk::cmd_robot_hurt> raw_robot_damage){
+  roborts_msgs::RobotDamage robot_damage;
+  robot_damage.damage_type = raw_robot_damage->hurt_type;
+  robot_damage.damage_source = raw_robot_damage->armor_id;
+  ros_robot_damage_pub_.publish(robot_damage);
+}
+
+void RefereeSystem::RobotShootCallback(const std::shared_ptr<roborts_sdk::cmd_shoot_data> raw_robot_shoot){
+  roborts_msgs::RobotShoot robot_shoot;
+  robot_shoot.frequency = raw_robot_shoot->bullet_freq;
+  robot_shoot.speed = raw_robot_shoot->bullet_speed;
+  ros_robot_damage_pub_.publish(robot_shoot);
+}
+
+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) {
+    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;
+  projectile_supply_pub_->Publish(raw_projectile_booking);
+}
+
+}