diff --git a/rm_msgs/msg/referee/ClientMapSendData.msg b/rm_msgs/msg/referee/ClientMapSendData.msg index 628adb38..591fc67f 100644 --- a/rm_msgs/msg/referee/ClientMapSendData.msg +++ b/rm_msgs/msg/referee/ClientMapSendData.msg @@ -1,14 +1,17 @@ uint8 KEY_A = 65 +uint8 KEY_E = 69 uint8 KEY_H = 72 uint8 KEY_K = 75 uint8 KEY_L = 76 uint8 KEY_N = 78 uint8 KEY_O = 79 +uint8 KEY_Q = 81 +uint8 KEY_R = 82 +uint8 KEY_W = 87 uint8 KEY_U = 85 float32 target_position_x float32 target_position_y -float32 target_position_z uint8 command_keyboard uint16 target_robot_ID uint8 cmd_source diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index ece8bafd..c704f22e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -36,6 +36,8 @@ class RefereeBase const ros::Time& last_get_data_time); virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time); virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data); + virtual void supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data); + virtual void updateShootDataDataCallBack(const rm_msgs::ShootData& msg); // sub call back virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state); @@ -59,6 +61,7 @@ class RefereeBase virtual void sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data); virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data); + virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); // send ui void sendSerialDataCallback(); @@ -86,6 +89,7 @@ class RefereeBase ros::Subscriber sentry_cmd_sub_; ros::Subscriber radar_cmd_sub_; ros::Subscriber sentry_state_sub_; + ros::Subscriber drone_pose_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; @@ -93,8 +97,8 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; - BulletTimeChangeUi* bullet_time_change_ui_{}; + BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; EffortTimeChangeUi* effort_time_change_ui_{}; ProgressTimeChangeUi* progress_time_change_ui_{}; @@ -106,6 +110,7 @@ class RefereeBase JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{}, *engineer_joint3_time_change_ui{}; TargetDistanceTimeChangeUi* target_distance_time_change_ui_{}; + DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{}; StringTriggerChangeUi *step_name_trigger_change_ui_{}, *servo_mode_trigger_change_ui_{}, *reversal_state_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, *joint_temperature_trigger_change_ui_{}, *drag_state_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{}; @@ -115,6 +120,7 @@ class RefereeBase CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; + ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; InteractiveSender* enemy_hero_state_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index b0a39ebd..98603ec3 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -64,4 +64,20 @@ class HeroHitFlashUi : public FlashUi bool hitted_; rm_msgs::GameRobotHp last_hp_msg_; }; + +class ExceedBulletSpeedFlashUi : public FlashUi +{ +public: + explicit ExceedBulletSpeedFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashUi(rpc_value, base, "exceed_bullet_speed", graph_queue, character_queue) + { + } + void updateShootData(const rm_msgs::ShootData& msg); + +private: + void display(const ros::Time& time) override; + rm_msgs::ShootData shoot_data_; +}; + } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/time_change_ui.h b/rm_referee/include/rm_referee/ui/time_change_ui.h index 9364a182..911d9264 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -300,4 +300,38 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi void updateConfig() override; double target_distance_; }; + +class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) + { + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + ori_x_ = static_cast(data["ori_x"]); + ori_y_ = static_cast(data["ori_y"]); + } + else + ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); + + graph_vector_.insert( + std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); + }; + void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); + +private: + void updateConfig() override; + int ori_x_, ori_y_; + double angle_; + int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, + right_line_y2_; +}; + } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/trigger_change_ui.h b/rm_referee/include/rm_referee/ui/trigger_change_ui.h index 1d3064f6..90ae3381 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -238,4 +238,5 @@ class StringTriggerChangeUi : public TriggerChangeUi void update() override; std::string data_; }; + } // namespace rm_referee diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 1e552c25..33ae97c0 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -215,6 +215,7 @@ int Referee::unpack(uint8_t* rx_data) supply_projectile_action_data.supply_robot_id = supply_projectile_action_ref.supply_robot_id; supply_projectile_action_data.stamp = last_get_data_time_; + referee_ui_.supplyBulletDataCallBack(supply_projectile_action_data); supply_projectile_action_pub_.publish(supply_projectile_action_data); break; } @@ -331,6 +332,7 @@ int Referee::unpack(uint8_t* rx_data) shoot_data.shooter_id = shoot_data_ref.shooter_id; shoot_data.stamp = last_get_data_time_; + referee_ui_.updateShootDataDataCallBack(shoot_data); shoot_data_pub_.publish(shoot_data); break; } diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 9bed0804..898ec405 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -44,6 +44,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); RefereeBase::sentry_state_sub_ = nh.subscribe("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this); + RefereeBase::drone_pose_sub_ = + nh.subscribe("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -123,6 +125,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "target_distance") target_distance_time_change_ui_ = new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "drone_towards") + drone_towards_time_change_group_ui_ = + new DroneTowardsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -137,6 +142,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "hero_hit") hero_hit_flash_ui_ = new HeroHitFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "exceed_bullet_speed") + exceed_bullet_speed_flash_ui_ = + new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -209,6 +217,8 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); + if (drone_towards_time_change_group_ui_) + drone_towards_time_change_group_ui_->addForQueue(); // if (drag_state_trigger_change_ui_) // drag_state_trigger_change_ui_->addForQueue(); if (gripper_state_trigger_change_ui_) @@ -533,4 +543,20 @@ void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data) sentry_state_sender_->sendCustomInfoData(converter.from_bytes(data->data)); } +void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data) +{ +} + +void RefereeBase::dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data) +{ + if (drone_towards_time_change_group_ui_ && !is_adding_) + drone_towards_time_change_group_ui_->updateTowardsData(data); +} + +void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg) +{ + if (exceed_bullet_speed_flash_ui_ && !is_adding_) + exceed_bullet_speed_flash_ui_->updateShootData(msg); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index f4574694..c0adbafc 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -86,5 +86,17 @@ void HeroHitFlashUi::display(const ros::Time& time) FlashUi::updateFlashUiForQueue(time, hitted_, false); } +void ExceedBulletSpeedFlashUi::display(const ros::Time& time) +{ + if (shoot_data_.bullet_speed <= 30) + graph_->setOperation(rm_referee::GraphOperation::DELETE); + FlashUi::updateFlashUiForQueue(time, shoot_data_.bullet_speed > 30, true); +} + +void ExceedBulletSpeedFlashUi::updateShootData(const rm_msgs::ShootData& msg) +{ + shoot_data_ = msg; +} + } // namespace rm_referee // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index fdc54823..5a1b1ab1 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -398,4 +398,46 @@ void TargetDistanceTimeChangeUi::updateConfig() UiBase::transferInt(std::floor(target_distance_ * 1000)); } +void DroneTowardsTimeChangeGroupUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) +{ + angle_ = yawFromQuat(data->pose.orientation) - M_PI / 2; + mid_line_x2_ = ori_x_ + 60 * cos(angle_ - M_PI / 2); + mid_line_y2_ = ori_y_ + 60 * sin(angle_ - M_PI / 2); + mid_line_x1_ = ori_x_ + 60 * cos(angle_ + M_PI / 2); + mid_line_y1_ = ori_y_ + 60 * sin(angle_ + M_PI / 2); + left_line_x2_ = ori_x_ + 40 * cos(angle_ + (5 * M_PI) / 6); + left_line_y2_ = ori_y_ + 40 * sin(angle_ + (5 * M_PI) / 6); + right_line_x2_ = ori_x_ + 40 * cos(angle_ + M_PI / 6); + right_line_y2_ = ori_y_ + 40 * sin(angle_ + M_PI / 6); + updateForQueue(); +} + +void DroneTowardsTimeChangeGroupUi::updateConfig() +{ + for (auto it : graph_vector_) + { + if (it.first == "drone_towards_mid") + { + it.second->setStartX(mid_line_x2_); + it.second->setStartY(mid_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + else if (it.first == "drone_towards_left") + { + it.second->setStartX(left_line_x2_); + it.second->setStartY(left_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + else if (it.first == "drone_towards_right") + { + it.second->setStartX(right_line_x2_); + it.second->setStartY(right_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + } +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index eafb1d89..15c39700 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -436,4 +436,5 @@ void StringTriggerChangeUi::update() graph_->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(true); } + } // namespace rm_referee