Skip to content

Commit

Permalink
Merge branch 'master' into follow-feedforward
Browse files Browse the repository at this point in the history
  • Loading branch information
liyixin135 committed Jul 16, 2024
2 parents 71bb1eb + 3985d9b commit df15d63
Show file tree
Hide file tree
Showing 24 changed files with 737 additions and 268 deletions.
16 changes: 15 additions & 1 deletion rm_common/include/rm_common/decision/command_sender.h
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
max_angular_z_.init(xml_rpc_value);
std::string topic;
nh.getParam("power_limit_topic", topic);
target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.);
target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.);
chassis_power_limit_subscriber_ =
nh.subscribe<rm_msgs::ChassisCmd>(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this);
}
Expand All @@ -186,6 +186,15 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
vel_direction_ = 1.;
msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_;
};
void setAngularZVel(double scale, double limit)
{
if (track_data_.v_yaw > target_vel_yaw_threshold_)
vel_direction_ = -1.;
if (track_data_.v_yaw < -target_vel_yaw_threshold_)
vel_direction_ = 1.;
double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_);
msg_.angular.z = scale * angular_z * vel_direction_;
};
void set2DVel(double scale_x, double scale_y, double scale_z)
{
setLinearXVel(scale_x);
Expand Down Expand Up @@ -824,6 +833,11 @@ class DoubleBarrelCommandSender
shooter_ID1_cmd_sender_->updateSuggestFireData(data);
shooter_ID2_cmd_sender_->updateSuggestFireData(data);
}
void updateShootBeforehandCmd(const rm_msgs::ShootBeforehandCmd& data)
{
shooter_ID1_cmd_sender_->updateShootBeforehandCmd(data);
shooter_ID2_cmd_sender_->updateShootBeforehandCmd(data);
}

void setMode(int mode)
{
Expand Down
2 changes: 1 addition & 1 deletion rm_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ add_message_files(
SupplyProjectileAction.msg
DartRemainingTime.msg
RobotHurt.msg
CurrentSentryPosData.msg
SentryAttackingTarget.msg
ShootData.msg
BulletAllowance.msg
RfidStatus.msg
Expand Down
4 changes: 0 additions & 4 deletions rm_msgs/msg/referee/CurrentSentryPosData.msg

This file was deleted.

2 changes: 1 addition & 1 deletion rm_msgs/msg/referee/EngineerUi.msg
Original file line number Diff line number Diff line change
Expand Up @@ -4,5 +4,5 @@ string ex_arm_state
uint32 stone_num

string step_queue_name
bool symbol
uint32 symbol
time stamp
14 changes: 13 additions & 1 deletion rm_msgs/msg/referee/EventData.msg
Original file line number Diff line number Diff line change
@@ -1,3 +1,15 @@
uint32 event_data
bool forward_supply_station_state
bool inside_supply_station_state
bool supplier_zone_state
bool power_rune_activation_point_state
bool small_power_rune_state
bool large_power_rune_state
uint8 ring_elevated_ground_state
uint8 r3_state
uint8 r4_state
uint8 base_shield_value
uint16 be_hit_time
uint8 be_hit_target
uint8 central_point_state

time stamp
22 changes: 21 additions & 1 deletion rm_msgs/msg/referee/RfidStatus.msg
Original file line number Diff line number Diff line change
@@ -1,3 +1,23 @@
uint32 rfid_status
bool base_buff_point_state
bool own_ring_elevated_ground_state
bool enemy_ring_elevated_ground_state
bool own_r3_state
bool enemy_r3_state
bool own_r4_state
bool enemy_r4_state
bool power_rune_activation_point_state
bool forward_own_launch_ramp_buff_point_state
bool behind_own_launch_ramp_buff_point_state
bool forward_enemy_launch_ramp_buff_point_state
bool behind_enemy_launch_ramp_buff_point_state
bool own_outpost_buff_point
bool own_side_restoration_zone
bool own_sentry_patrol_zones
bool enemy_sentry_patrol_zones
bool own_large_resource_island_point
bool enemy_large_resource_island_point
bool own_exchange_zone
bool central_buff_point
uint32 reverse

time stamp
3 changes: 3 additions & 0 deletions rm_msgs/msg/referee/SentryAttackingTarget.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
uint8 target_robot_ID
float32 target_position_x
float32 target_position_y
3 changes: 1 addition & 2 deletions rm_referee/include/rm_referee/common/data.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,12 @@
#include <rm_msgs/ClientMapReceiveData.h>
#include <rm_msgs/SupplyProjectileAction.h>
#include <rm_msgs/IcraBuffDebuffZoneStatus.h>
#include <rm_msgs/SentryDeviate.h>
#include <rm_msgs/CurrentSentryPosData.h>
#include <rm_msgs/GameRobotPosData.h>
#include "rm_msgs/SentryInfo.h"
#include "rm_msgs/RadarInfo.h"
#include "rm_msgs/Buff.h"
#include "rm_msgs/TrackData.h"
#include "rm_msgs/SentryAttackingTarget.h"
#include <rm_msgs/PowerManagementSampleAndStatusData.h>
#include <rm_msgs/PowerManagementSystemExceptionData.h>
#include <rm_msgs/PowerManagementInitializationExceptionData.h>
Expand Down
56 changes: 48 additions & 8 deletions rm_referee/include/rm_referee/common/protocol.h
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,9 @@ typedef enum
CLIENT_CHARACTER_CMD = 0x0110,
SENTRY_CMD = 0x0120,
RADAR_CMD = 0x0121,
CURRENT_SENTRY_POSITION_CMD = 0x0200 // send radar->sentry
BULLET_NUM_SHARE_CMD = 0x0200, // send robot->aerial
SENTRY_TO_RADAR_CMD = 0x0201 // send sentry->radar
// send radar->sentry
} DataCmdId;

typedef enum
Expand Down Expand Up @@ -282,7 +284,19 @@ typedef struct

typedef struct
{
uint32_t event_type;
uint8_t forward_supply_station_state : 1;
uint8_t inside_supply_station_state : 1;
uint8_t supplier_zone_state : 1;
uint8_t power_rune_activation_point_state : 1;
uint8_t small_power_rune_state : 1;
uint8_t large_power_rune_state : 1;
uint8_t ring_elevated_ground_state : 2;
uint8_t r3_state : 2;
uint8_t r4_state : 2;
uint8_t base_shield_value : 7;
uint16_t be_hit_time : 9;
uint8_t be_hit_target : 2;
uint8_t central_point_state : 2;
} __packed EventData;

typedef struct
Expand Down Expand Up @@ -371,7 +385,27 @@ typedef struct

typedef struct
{
uint32_t rfid_status;
uint8_t base_buff_point_state : 1;
uint8_t own_ring_elevated_ground_state : 1;
uint8_t enemy_ring_elevated_ground_state : 1;
uint8_t own_r3_state : 1;
uint8_t enemy_r3_state : 1;
uint8_t own_r4_state : 1;
uint8_t enemy_r4_state : 1;
uint8_t power_rune_activation_point_state : 1;
uint8_t forward_own_launch_ramp_buff_point_state : 1;
uint8_t behind_own_launch_ramp_buff_point_state : 1;
uint8_t forward_enemy_launch_ramp_buff_point_state : 1;
uint8_t behind_enemy_launch_ramp_buff_point_state : 1;
uint8_t own_outpost_buff_point : 1;
uint8_t own_side_restoration_zone : 1;
uint8_t own_sentry_patrol_zones : 1;
uint8_t enemy_sentry_patrol_zones : 1;
uint8_t own_large_resource_island_point : 1;
uint8_t enemy_large_resource_island_point : 1;
uint8_t own_exchange_zone : 1;
uint8_t central_buff_point : 1;
uint32_t reverse : 12;
} __packed RfidStatus;

typedef struct
Expand Down Expand Up @@ -535,6 +569,14 @@ typedef struct
float target_position_y;
} __packed ClientMapReceiveData;

typedef struct
{
InteractiveDataHeader header_data;
uint8_t target_robot_ID;
float target_position_x;
float target_position_y;
} __packed SentryAttackingTargetData;

typedef struct
{
int16_t mouse_x;
Expand All @@ -559,11 +601,9 @@ typedef struct
typedef struct
{
InteractiveDataHeader header_data;
float position_x;
float position_y;
float position_z;
float position_yaw;
} __packed CurrentSentryPosData;
uint8_t bullet_42_mm_num;
uint8_t bullet_17_mm_num;
} __packed BulletNumData;

typedef struct
{
Expand Down
4 changes: 2 additions & 2 deletions rm_referee/include/rm_referee/referee.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,6 @@ class Referee
game_status_pub_ = nh.advertise<rm_msgs::GameStatus>("game_status", 1);
power_heat_data_pub_ = nh.advertise<rm_msgs::PowerHeatData>("power_heat_data", 1);
game_robot_hp_pub_ = nh.advertise<rm_msgs::GameRobotHp>("game_robot_hp", 1);
current_sentry_pos_pub_ = nh.advertise<rm_msgs::CurrentSentryPosData>("current_sentry_pos", 1);
buff_pub_ = nh.advertise<rm_msgs::Buff>("robot_buff", 1);
event_data_pub_ = nh.advertise<rm_msgs::EventData>("event_data", 1);
dart_status_pub_ = nh.advertise<rm_msgs::DartStatus>("dart_status_data", 1);
Expand All @@ -75,6 +74,7 @@ class Referee
game_robot_pos_pub_ = nh.advertise<rm_msgs::GameRobotPosData>("game_robot_pos", 1);
sentry_info_pub_ = nh.advertise<rm_msgs::SentryInfo>("sentry_info", 1);
radar_info_pub_ = nh.advertise<rm_msgs::RadarInfo>("radar_info", 1);
sentry_to_radar_pub_ = nh.advertise<rm_msgs::SentryAttackingTarget>("sentry_target_to_radar", 1);

ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management");
power_management_sample_and_status_data_pub_ =
Expand Down Expand Up @@ -102,7 +102,7 @@ class Referee
ros::Publisher game_status_pub_;
ros::Publisher power_heat_data_pub_;
ros::Publisher game_robot_hp_pub_;
ros::Publisher current_sentry_pos_pub_;
ros::Publisher sentry_to_radar_pub_;
ros::Publisher event_data_pub_;
ros::Publisher dart_status_pub_;
ros::Publisher buff_pub_;
Expand Down
19 changes: 14 additions & 5 deletions rm_referee/include/rm_referee/referee_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include "rm_referee/ui/trigger_change_ui.h"
#include "rm_referee/ui/time_change_ui.h"
#include "rm_referee/ui/flash_ui.h"
#include "rm_referee/ui/interactive_data.h"

namespace rm_referee
{
Expand All @@ -38,6 +39,7 @@ class RefereeBase
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);
virtual void updateBulletRemainData(const rm_referee::BulletNumData& data);

// sub call back
virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state);
Expand All @@ -56,12 +58,12 @@ class RefereeBase
virtual void balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data);
virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data);
virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data);
virtual void sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data);
virtual void sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data);
virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data);
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);
virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data);

// send ui
void sendSerialDataCallback();
Expand All @@ -85,18 +87,21 @@ class RefereeBase
ros::Subscriber balance_state_sub_;
ros::Subscriber radar_receive_sub_;
ros::Subscriber map_sentry_sub_;
ros::Subscriber sentry_to_radar_sub_;
ros::Subscriber radar_to_sentry_sub_;
ros::Subscriber sentry_cmd_sub_;
ros::Subscriber radar_cmd_sub_;
ros::Subscriber sentry_state_sub_;
ros::Subscriber drone_pose_sub_;
ros::Subscriber shoot_cmd_sub_;

ChassisTriggerChangeUi* chassis_trigger_change_ui_{};
ShooterTriggerChangeUi* shooter_trigger_change_ui_{};
GimbalTriggerChangeUi* gimbal_trigger_change_ui_{};
TargetTriggerChangeUi* target_trigger_change_ui_{};
TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{};
CameraTriggerChangeUi* camera_trigger_change_ui_{};
FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{};

BulletTimeChangeUi* bullet_time_change_ui_{};
CapacitorTimeChangeUi* capacitor_time_change_ui_{};
Expand All @@ -110,26 +115,30 @@ class RefereeBase
JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{},
*engineer_joint3_time_change_ui{};
TargetDistanceTimeChangeUi* target_distance_time_change_ui_{};
FriendBulletsTimeChangeGroupUi* friend_bullets_time_change_group_ui_{};

DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{};
StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{},
*joint_temperature_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{};


FixedUi* fixed_ui_{};

CoverFlashUi* cover_flash_ui_{};
SpinFlashUi* spin_flash_ui_{};
HeroHitFlashUi* hero_hit_flash_ui_{};
ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{};
EngineerActionFlashUi* engineer_action_flash_ui_{};

InteractiveSender* interactive_data_sender_{};
InteractiveSender* enemy_hero_state_sender_{};
InteractiveSender* sentry_state_sender_{};
CustomInfoSender* enemy_hero_state_sender_{};
CustomInfoSender* sentry_state_sender_{};
BulletNumShare* bullet_num_share_{};
SentryToRadar* sentry_to_radar_{};

GroupUiBase* graph_queue_sender_{};
std::deque<Graph> graph_queue_;
std::deque<Graph> character_queue_;
// std::deque<std::tuple<>> interactive_data_queue_;

ros::Time radar_interactive_data_last_send_;
ros::Time sentry_interactive_data_last_send_;
Expand Down
41 changes: 41 additions & 0 deletions rm_referee/include/rm_referee/ui/flash_ui.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,47 @@ class FlashUi : public UiBase
void updateFlashUiForQueue(const ros::Time& time, bool state, bool once);
};

class FlashGroupUi : public GroupUiBase
{
public:
explicit FlashGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name,
std::deque<Graph>* graph_queue, std::deque<Graph>* character_queue)
: GroupUiBase(rpc_value, base, graph_queue, character_queue)
{
graph_name_ = graph_name;
}
virtual void display(const ros::Time& time){};
virtual void updateConfig(){};
void updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph);

protected:
std::string graph_name_;
};

class EngineerActionFlashUi : public FlashGroupUi
{
public:
explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque<Graph>* graph_queue,
std::deque<Graph>* character_queue)
: FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue)
{
if (rpc_value.hasMember("data"))
{
XmlRpc::XmlRpcValue& data = rpc_value["data"];
for (int i = 0; i < static_cast<int>(rpc_value["data"].size()); i++)
{
graph_vector_.insert(std::pair<std::string, Graph*>(std::to_string(static_cast<int>(data[i]["flag"])),
new Graph(data[i]["config"], base_, id_++)));
}
}
}
void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time);

private:
void display(const ros::Time& time) override;
uint32_t symbol_;
};

class CoverFlashUi : public FlashUi
{
public:
Expand Down
19 changes: 19 additions & 0 deletions rm_referee/include/rm_referee/ui/graph.h
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,25 @@ class Graph
{
config_.radius = radius;
}
void setIntNum(int num)
{
int a = num & 1023;
int b = (num >> 10) & 2047;
int c = num >> 21;
config_.radius = a;
config_.end_x = b;
config_.end_y = c;
}
void setFloatNum(float data)
{
int num = static_cast<int>(data * 1000);
int a = num & 1023;
int b = (num >> 10) & 2047;
int c = num >> 21;
config_.radius = a;
config_.end_x = b;
config_.end_y = c;
}
void setStartX(int start_x)
{
config_.start_x = start_x;
Expand Down
Loading

0 comments on commit df15d63

Please sign in to comment.