Dev/sentry

rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml

@@ -2,14 +2,14 @@ rmcs_executor:
  ros__parameters:
  update_rate: 1000.0
  components:
-# - rmcs::AutoAimPlayerComponent -> auto_aim_player
- - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player
+ - rmcs::AutoAimPlayerComponent -> auto_aim_player
+# - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player
  # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder
  - rmcs::AutoAimComponent -> auto_aim_component
 
 auto_aim_player:
  ros__parameters:
- input_path: "/workspaces/data/autoaim/自家大符/"
+ input_path: "/workspaces/data/autoaim/robot/blue_fast_track/"
  loop_play: true
 
 auto_aim_video_player:
@@ -25,11 +25,13 @@ auto_aim_recorder:
  flush_every_n_frames: 64
  max_duration_seconds: 0
  max_videos_size_gb: 0.0
+ auto_record: false
 
 auto_aim_component:
  ros__parameters:
  dangerous_fallback: "red"
  manual_shoot: false
+ enable_rune: true
  camera_translation: [0., 0., 0.]
 
  fire_control:

rmcs_ws/src/rmcs_bringup/config/navigation_test.yaml

@@ -7,5 +7,5 @@ rmcs_executor:
 rmcs_navigation:
  ros__parameters:
  command_vel_name: "/cmd_vel"
- endpoint: "train"
+ endpoint: "mock"
  enable_goal_topic_forward: true

rmcs_ws/src/rmcs_bringup/config/sentry.yaml

@@ -7,7 +7,7 @@ rmcs_executor:
  - rmcs_core::referee::Status -> referee_status
  - rmcs_core::referee::Command -> referee_command
  - rmcs_core::referee::command::Interaction -> referee_interaction
-# - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision
+ - rmcs_core::referee::command::interaction::SentryDecision -> referee_sentry_decision
  - rmcs_core::referee::command::interaction::Ui -> referee_ui
 
  - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller
@@ -24,17 +24,17 @@ rmcs_executor:
 
  # - rmcs::navigation::Navigation -> rmcs_navigation
 
-# - rmcs::AutoAimRecorderComponent -> auto_aim_recorder
-# - rmcs::AutoAimCapturerComponent -> auto_aim_capturer
-# - rmcs::AutoAimComponent -> auto_aim_component
+ - rmcs::AutoAimRecorderComponent -> auto_aim_recorder
+ - rmcs::AutoAimCapturerComponent -> auto_aim_capturer
+ - rmcs::AutoAimComponent -> auto_aim_component
 
  # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
  # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
 
 rmcs_navigation:
  ros__parameters:
  command_vel_name: "/cmd_vel"
- endpoint: "otaku"
+ endpoint: "train"
  enable_goal_topic_forward: true
 
 auto_aim_capturer:
@@ -53,8 +53,9 @@ auto_aim_recorder:
  output_path: "/tmp/autoaim/records"
  queue_depth: 16
  flush_every_n_frames: 64
- max_duration_seconds: 0
- max_videos_size_gb: 0.0
+ max_duration_seconds: 300
+ max_videos_size_gb: 100.0
+ auto_record: false
 
 auto_aim_component:
  ros__parameters:
@@ -63,10 +64,10 @@ auto_aim_component:
  camera_translation: [0.07128, 0.0, 0.0481]
  fire_control:
  bullet_speed: 22.5
- shoot_delay: 0.1
- offset_yaw: -0.5
- offset_pitch: +1.0
- attack_window: 120.0
+ shoot_delay: 0.02
+ offset_yaw: -0.3
+ offset_pitch: +0.9
+ attack_window: 80.0
  degraded_angle_speed: 12.0
  window_hysteresis: 0.2
  attack_preaim: false

rmcs_ws/src/rmcs_core/plugins.xml

@@ -54,6 +54,7 @@
  <class type="rmcs_core::referee::Command" base_class_type="rmcs_executor::Component" />
  <class type="rmcs_core::referee::command::Interaction" base_class_type="rmcs_executor::Component" />
  <class type="rmcs_core::referee::command::interaction::Ui" base_class_type="rmcs_executor::Component" />
+ <class type="rmcs_core::referee::command::interaction::SentryDecision" base_class_type="rmcs_executor::Component" />
 
  <class type="rmcs_core::referee::app::ui::Infantry" base_class_type="rmcs_executor::Component" />
  <class type="rmcs_core::referee::app::ui::DeformableInfantry" base_class_type="rmcs_executor::Component" />

rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp

@@ -35,13 +35,28 @@ class SteeringWheelController
  , no_load_power_(get_parameter("no_load_power").as_double())
  , control_acceleration_filter_(5.0, 1000.0)
  , chassis_velocity_expected_(Eigen::Vector3d::Zero())
- , chassis_translational_velocity_pid_(5.0, 0.0, 1.0)
- , chassis_angular_velocity_pid_(5.0, 0.0, 1.0)
+ , chassis_translational_velocity_pid_(
+ get_parameter("chassis_translation_kp").as_double(),
+ get_parameter("chassis_translation_ki").as_double(),
+ get_parameter("chassis_translation_kd").as_double())
+ , chassis_angular_velocity_pid_(
+ get_parameter("chassis_angular_velocity_kp").as_double(),
+ get_parameter("chassis_angular_velocity_ki").as_double(),
+ get_parameter("chassis_angular_velocity_kd").as_double())
  , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2
  , sin_varphi_(0, 1, 0, -1)
- , steering_velocity_pid_(0.15, 0.0, 0.0)
- , steering_angle_pid_(30.0, 0.0, 0.0)
- , wheel_velocity_pid_(0.6, 0.0, 0.0) {
+ , steering_velocity_pid_(
+ get_parameter("steering_velocity_kp").as_double(),
+ get_parameter("steering_velocity_ki").as_double(),
+ get_parameter("steering_velocity_kd").as_double())
+ , steering_angle_pid_(
+ get_parameter("steering_angle_kp").as_double(),
+ get_parameter("steering_angle_ki").as_double(),
+ get_parameter("steering_angle_kd").as_double())
+ , wheel_velocity_pid_(
+ get_parameter("wheel_velocity_kp").as_double(),
+ get_parameter("wheel_velocity_ki").as_double(),
+ get_parameter("wheel_velocity_kd").as_double()) {
 
  register_input("/remote/joystick/right", joystick_right_);
  register_input("/remote/joystick/left", joystick_left_);

rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp

@@ -0,0 +1,242 @@
+#include "referee/command/field.hpp"
+#include "referee/command/interaction/header.hpp"
+#include "referee/status/field.hpp"
+
+#include <array>
+#include <cstdint>
+#include <rclcpp/node.hpp>
+#include <rmcs_executor/component.hpp>
+#include <rmcs_msgs/full_robot_id.hpp>
+#include <rmcs_msgs/robot_id.hpp>
+#include <rmcs_msgs/sentry_event.hpp>
+#include <unordered_map>
+#include <unordered_set>
+
+namespace rmcs_core::referee::command::interaction {
+
+class SentryDecision
+ : public rmcs_executor::Component
+ , public rclcpp::Node {
+public:
+ using Command = status::SentryCommand;
+ using Posture = Command::Posture;
+ using SentryEvent = rmcs_msgs::SentryEvent;
+ using EventCounts = std::unordered_map<SentryEvent, std::uint16_t>;
+ using Clock = std::chrono::steady_clock;
+
+ InputInterface<EventCounts> sentry_events_;
+ InputInterface<rmcs_msgs::RobotId> robot_id_;
+ InputInterface<std::uint8_t> sentry_posture_fb_;
+ InputInterface<std::uint16_t> robot_hp_fb_;
+ InputInterface<std::uint8_t> energy_core_status_;
+ InputInterface<bool> can_rebirth_free_;
+
+ OutputInterface<Field> sentry_decision_field_;
+
+ Header header_{};
+ Command command_{};
+
+ EventCounts cached_events_;
+ std::unordered_set<SentryEvent> requests_;
+ std::unordered_map<SentryEvent, Posture> pose_targets_;
+ std::unordered_set<SentryEvent> logged_events_;
+ std::uint8_t last_fb_posture_ = 3;
+ bool last_can_rebirth_free_ = false;
+
+ static inline const auto kPoseEvents = std::unordered_set{
+ SentryEvent::SWITCH_POSE_ATTACK,
+ SentryEvent::SWITCH_POSE_DEFENSE,
+ SentryEvent::SWITCH_POSE_MOVE,
+ SentryEvent::SWITCH_POSE_POWERED_ATTACK,
+ SentryEvent::SWITCH_POSE_POWERED_DEFENSE,
+ SentryEvent::SWITCH_POSE_POWERED_MOVE,
+ };
+
+ static auto to_posture(SentryEvent event) -> Posture {
+ switch (event) {
+ case SentryEvent::SWITCH_POSE_ATTACK: return Posture::ATTACK;
+ case SentryEvent::SWITCH_POSE_DEFENSE: return Posture::DEFENSE;
+ case SentryEvent::SWITCH_POSE_MOVE: return Posture::MOVE;
+ case SentryEvent::SWITCH_POSE_POWERED_ATTACK: return Posture::POWERED_ATTACK;
+ case SentryEvent::SWITCH_POSE_POWERED_DEFENSE: return Posture::POWERED_DEFENSE;
+ case SentryEvent::SWITCH_POSE_POWERED_MOVE: return Posture::POWERED_MOVE;
+ default: return Posture::UNKNOWN;
+ }
+ }
+
+ static constexpr auto kEventPriority = std::array{
+ SentryEvent::CONFIRM_REBIRTH,
+ SentryEvent::CONFIRM_INSTANT_REBIRTH,
+ SentryEvent::SWITCH_POSE_ATTACK,
+ SentryEvent::SWITCH_POSE_DEFENSE,
+ SentryEvent::SWITCH_POSE_MOVE,
+ SentryEvent::SWITCH_POSE_POWERED_ATTACK,
+ SentryEvent::SWITCH_POSE_POWERED_DEFENSE,
+ SentryEvent::SWITCH_POSE_POWERED_MOVE,
+ SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT,
+ SentryEvent::EXCHANGE_AMMO_REMOTE,
+ SentryEvent::EXCHANGE_HP_REMOTE,
+ SentryEvent::ACTIVATE_ENERGY_CORE,
+ };
+
+ SentryDecision()
+ : Node{
+ get_component_name(),
+ rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {
+
+ register_input("/referee/id", robot_id_);
+ register_input("/rmcs_navigation/sentry_events", sentry_events_, false);
+ register_input("/referee/sentry/posture", sentry_posture_fb_, false);
+ register_input("/referee/current_hp", robot_hp_fb_, false);
+ register_input(
+ "/referee/event/ally_big_energy_activation_status", energy_core_status_, false);
+ register_input("/referee/sentry/can_rebirth_free", can_rebirth_free_, false);
+
+ register_output("/referee/command/interaction/sentry_decision", sentry_decision_field_);
+ }
+
+ auto before_updating() -> void override {
+ if (!sentry_events_.ready())
+ sentry_events_.make_and_bind_directly();
+ if (!sentry_posture_fb_.ready())
+ sentry_posture_fb_.make_and_bind_directly(uint8_t{3});
+ if (!robot_hp_fb_.ready())
+ robot_hp_fb_.make_and_bind_directly(uint16_t{0});
+ if (!energy_core_status_.ready())
+ energy_core_status_.make_and_bind_directly(uint8_t{0});
+ if (!can_rebirth_free_.ready())
+ can_rebirth_free_.make_and_bind_directly(false);
+ }
+
+ auto update() -> void override {
+ if (*robot_id_ == rmcs_msgs::RobotId::UNKNOWN) {
+ *sentry_decision_field_ = Field{};
+ return;
+ }
+
+ detect_new_events();
+
+ const auto can_rebirth_free = *can_rebirth_free_;
+ if (can_rebirth_free && !last_can_rebirth_free_) {
+ requests_.insert(SentryEvent::CONFIRM_REBIRTH);
+ }
+ last_can_rebirth_free_ = can_rebirth_free;
+
+ consume_one_event();
+ verify_feedback();
+ }
+
+private:
+ auto detect_new_events() -> void {
+ const auto& input = *sentry_events_;
+
+ for (const auto event : kEventPriority) {
+ auto input_it = input.find(event);
+ auto input_count = (input_it != input.end()) ? input_it->second : uint16_t{0};
+ auto cache_count = cached_events_[event];
+
+ if (cache_count != input_count) {
+ if (kPoseEvents.contains(event)) {
+ for (const auto rm : kPoseEvents)
+ requests_.erase(rm);
+ }
+ requests_.insert(event);
+ cached_events_[event] = input_count;
+ }
+ }
+ }
+
+ auto consume_one_event() -> void {
+ if (requests_.empty()) {
+ *sentry_decision_field_ = Field{};
+ return;
+ }
+
+ const auto id = rmcs_msgs::FullRobotId{*robot_id_};
+ header_.command_id = 0x0120;
+ header_.sender_id = id;
+ header_.receiver_id = rmcs_msgs::FullRobotId::REFEREE_SERVER;
+
+ for (const auto event : kEventPriority) {
+ if (!requests_.contains(event))
+ continue;
+
+ command_ = Command{};
+
+ if (kPoseEvents.contains(event)) {
+ command_.posture = to_posture(event);
+ pose_targets_[event] = to_posture(event);
+ } else if (event == SentryEvent::CONFIRM_REBIRTH) {
+ command_.rebirth_confirm = 1;
+ } else if (event == SentryEvent::CONFIRM_INSTANT_REBIRTH) {
+ command_.instant_rebirth_confirm = 1;
+ } else if (event == SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT) {
+ command_.ammo_exchange = 1;
+ } else if (event == SentryEvent::EXCHANGE_AMMO_REMOTE) {
+ command_.remote_ammo_request = 1;
+ } else if (event == SentryEvent::EXCHANGE_HP_REMOTE) {
+ command_.remote_hp_request = 1;
+ } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) {
+ command_.energy_core_confirm = 1;
+ }
+
+ *sentry_decision_field_ = MAKE_FIELD(header_, command_);
+
+ if (kPoseEvents.contains(event)) {
+ if (!logged_events_.contains(event)) {
+ RCLCPP_INFO(
+ get_logger(), "Sentry pose command: %d",
+ std::to_underlying(command_.posture));
+ logged_events_.insert(event);
+ }
+ }
+ break;
+ }
+ }
+
+ auto verify_feedback() -> void {
+ const auto fb_posture_id = *sentry_posture_fb_;
+ const auto fb_hp = *robot_hp_fb_;
+ const auto energy_core_status = *energy_core_status_;
+
+ if (fb_posture_id != last_fb_posture_) {
+ RCLCPP_INFO(
+ get_logger(), "Sentry posture feedback: %d → %d", last_fb_posture_, fb_posture_id);
+ last_fb_posture_ = fb_posture_id;
+ }
+
+ auto to_erase = std::vector<SentryEvent>{};
+ for (const auto event : requests_) {
+ if (kPoseEvents.contains(event)) {
+ auto it = pose_targets_.find(event);
+ if (it != pose_targets_.end()
+ && static_cast<uint8_t>(it->second) == fb_posture_id) {
+ to_erase.push_back(event);
+ pose_targets_.erase(it);
+ logged_events_.erase(event);
+ }
+ } else if (
+ event == SentryEvent::CONFIRM_REBIRTH
+ || event == SentryEvent::CONFIRM_INSTANT_REBIRTH) {
+ if (fb_hp > 0) {
+ to_erase.push_back(event);
+ }
+ } else if (event == SentryEvent::ACTIVATE_ENERGY_CORE) {
+ if (energy_core_status != 0) {
+ to_erase.push_back(event);
+ }
+ } else {
+ to_erase.push_back(event);
+ }
+ }
+
+ for (const auto event : to_erase)
+ requests_.erase(event);
+ }
+};
+
+} // namespace rmcs_core::referee::command::interaction
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+ rmcs_core::referee::command::interaction::SentryDecision, rmcs_executor::Component)