Skip to content

Dev/sentry#98

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
creeper5820 merged 2 commits into from
Jul 28, 2026
Merged

Dev/sentry #98

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e1a8d23
wip(robots): Enable sentry navigation, add navigation supercap boost …
zlq04222 Jul 28, 2026
64bb6ea
wip(robots): Add leave phase after sentry climber landing
zlq04222 Jul 28, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions rmcs_ws/src/rmcs_bringup/config/sentry.yaml
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -22,7 +22,7 @@ rmcs_executor:
- rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller
- rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller

# - rmcs::navigation::Navigation -> rmcs_navigation
- rmcs::navigation::Navigation -> rmcs_navigation

- rmcs::AutoAimRecorderComponent -> auto_aim_recorder
- rmcs::AutoAimCapturerComponent -> auto_aim_capturer
Expand All@@ -40,7 +40,7 @@ rmcs_navigation:
auto_aim_capturer:
ros__parameters:
camera_name: ""
exposure_us: 2000.0
exposure_us: 1000.0
gain: 8.0
framerate: 120.0
invert_image: true
Expand All@@ -60,15 +60,15 @@ auto_aim_recorder:
auto_aim_component:
ros__parameters:
manual_shoot: true
enable_rune: true
enable_rune: false
camera_translation: [0.07128, 0.0, 0.0481]
fire_control:
bullet_speed: 22.5
shoot_delay: 0.02
offset_yaw: -0.3
offset_pitch: +0.9
attack_window: 80.0
degraded_angle_speed: 12.0
degraded_angle_speed: 10.0
window_hysteresis: 0.2
attack_preaim: false
require_stable_command: false
Expand DownExpand Up@@ -144,7 +144,7 @@ sentry_climber:
stick_group:
speed_drop: 30.0
speed_rise: 60.0
rise_torque_limit: 1.2
rise_torque_limit: 1.5
land_speed_begin: 100.0
land_speed_final: 10.0
land_duration: 0.5
Expand All@@ -158,7 +158,7 @@ sentry_climber:
hold_torque: 0.01
block_hold: 0.05
align:
err: 0.20
err: 0.18
w: 0.2
hold: 0.05
timeout: 15.0
Expand All@@ -168,7 +168,7 @@ sentry_climber:
approach_vx: 1.2
deploy_vx: 0.3
dash_vx: 3.0
retract_vx: 0.3
retract_vx: 0.0
dash_min: 0.1
dash_duration: 0.8
stick_timeout: 8.0
Expand All@@ -181,6 +181,8 @@ sentry_climber:
stick_timeout: 8.0
soft_timeout: 3.0
settle_timeout: 8.0
leave_vx: 0.3
leave_duration: 1.0

friction_wheel_controller:
ros__parameters:
Expand DownExpand Up@@ -247,11 +249,11 @@ steering_wheel_controller:
no_load_power: 11.37

chassis_translation_kp: 20.0
chassis_translation_ki: 0.001
chassis_translation_ki: 0.0001
chassis_translation_kd: 0.00

chassis_angular_velocity_kp: 8.0
chassis_angular_velocity_ki: 0.0
chassis_angular_velocity_ki: 0.005
chassis_angular_velocity_kd: 1.0

steering_velocity_kp: 0.15
Expand Down
7 changes: 6 additions & 1 deletion rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -30,6 +30,7 @@ class ChassisPowerController
register_input("/chassis/power", chassis_power_);
register_input("/chassis/supercap/voltage", supercap_voltage_);
register_input("/chassis/supercap/enabled", supercap_enabled_);
register_input("/rmcs_navigation/enable_supercap", navigation_supercap_, false);

register_input("/referee/chassis/power_limit", chassis_power_limit_referee_);
register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_);
Expand DownExpand Up@@ -110,7 +111,10 @@ class ChassisPowerController
void update_control_power_limit() {
double power_limit;

if (boost_mode_ && *supercap_enabled_)
const auto navigation_supercap_boost =
navigation_supercap_.ready() && *navigation_supercap_;

if ((boost_mode_ || navigation_supercap_boost) && *supercap_enabled_)
power_limit =
rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0;
else
Expand DownExpand Up@@ -159,6 +163,7 @@ class ChassisPowerController

InputInterface<double> supercap_voltage_;
InputInterface<bool> supercap_enabled_;
InputInterface<bool> navigation_supercap_;

InputInterface<double> chassis_power_limit_referee_;
InputInterface<double> chassis_buffer_energy_referee_;
Expand Down
49 changes: 44 additions & 5 deletions rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -60,6 +60,8 @@ class SentryClimber
double stick_timeout;
double soft_timeout;
double settle_timeout;
double leave_vx;
double leave_duration;
} land;

double block_hold;
Expand DownExpand Up@@ -126,6 +128,8 @@ class SentryClimber
.stick_timeout = param_or("land.stick_timeout", 8.0),
.soft_timeout = param_or("land.soft_timeout", 3.0),
.settle_timeout = param_or("land.settle_timeout", 8.0),
.leave_vx = param_or("land.leave_vx", 0.5),
.leave_duration = param_or("land.leave_duration", 1.0),
},
.block_hold = param_or("block_hold", 0.05),
};
Expand DownExpand Up@@ -239,7 +243,21 @@ class SentryClimber

OutputInterface<double> chassis_track_direction; // 以履带方向为正向
OutputInterface<double> chassis_climb_speed; // 正向为基准的速度值
OutputInterface<double> chassis_climb_status; // 事件进度
OutputInterface<double>
chassis_climb_status; // 事件进度: 0=空闲, 1=成功, -1=失败, (0,1)阶段小数

// /chassis/climber/status 阶段编码：(0, 0.55) 上台阶，[0.55, 1) 下台阶
static constexpr double kStatusClimbAlign = 0.1;
static constexpr double kStatusClimbApproach = 0.2;
static constexpr double kStatusClimbDeploy = 0.3;
static constexpr double kStatusClimbDash = 0.4;
static constexpr double kStatusClimbRetract = 0.5;
static constexpr double kStatusLandAlign = 0.6;
static constexpr double kStatusLandDash = 0.7;
static constexpr double kStatusLandSettle = 0.75;
static constexpr double kStatusLandSoft = 0.8;
static constexpr double kStatusLandFinal = 0.9;
static constexpr double kStatusLandLeave = 0.95;

struct SimpleComponent : public rmcs_executor::Component {
std::function<void()> fn;
Expand DownExpand Up@@ -390,7 +408,7 @@ class SentryClimber
auto climb(double direction) -> CoSchduler::Task {
using namespace std::chrono_literals;

*chassis_climb_status = 0.0;
*chassis_climb_status = kStatusClimbAlign;

node::info("Climb start, direction={:.3f}", direction);
*chassis_track_direction = direction;
Expand All@@ -400,6 +418,7 @@ class SentryClimber
stick_group->set_state(StickState::kHold);
*chassis_climb_speed = 0.0;
{
const auto t0 = std::chrono::steady_clock::now();
const auto timed_out = co_await context.wait_align(
direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold),
seconds_to_duration(config.align.timeout));
Expand All@@ -409,9 +428,15 @@ class SentryClimber
*chassis_climb_status = -1;
co_return;
}
const auto elapsed =
std::chrono::duration<double>(std::chrono::steady_clock::now() - t0);
node::info(
"climb ALIGN done: err={:.3f}, took={:.3f}s", context.align_error(direction),
elapsed.count());
}

// [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度
*chassis_climb_status = kStatusClimbApproach;
track_group->set_state(TrackState::kRush);
stick_group->set_state(StickState::kHold);
*chassis_climb_speed = config.climb.approach_vx;
Expand All@@ -425,6 +450,7 @@ class SentryClimber
}

// [] 伸出撑杆，同时慢速向台阶方向前进
*chassis_climb_status = kStatusClimbDeploy;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kDrop);
*chassis_climb_speed = config.climb.deploy_vx;
Expand All@@ -436,12 +462,14 @@ class SentryClimber
}

// [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔
*chassis_climb_status = kStatusClimbDash;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kHold);
*chassis_climb_speed = config.climb.dash_vx;
co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)};

// [] 上台阶完毕，收回撑杆
*chassis_climb_status = kStatusClimbRetract;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kRise);
*chassis_climb_speed = config.climb.retract_vx;
Expand All@@ -463,7 +491,7 @@ class SentryClimber
}

auto land(double direction) -> CoSchduler::Task {
*chassis_climb_status = 0.0;
*chassis_climb_status = kStatusLandAlign;

node::info("Land start, direction={:.3f}", direction);
*chassis_track_direction = direction + std::numbers::pi;
Expand All@@ -474,8 +502,8 @@ class SentryClimber
*chassis_climb_speed = 0.0;
{
const auto timed_out = co_await context.wait_align(
direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold),
seconds_to_duration(config.align.timeout));
direction + std::numbers::pi, config.align.err, config.align.w,
seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout));
if (timed_out) {
node::warn("land ALIGN failed");
release_climber();
Expand All@@ -485,6 +513,7 @@ class SentryClimber
}

// [] 伸出撑杆，以较快速度冲下台阶
*chassis_climb_status = kStatusLandDash;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kDrop);
*chassis_climb_speed = -config.land.dash_vx;
Expand All@@ -496,6 +525,7 @@ class SentryClimber
}

// [] 保持撑杆伸出，直到撑杆从台阶落下，底盘倾角低于某个阈值，趋近水平
*chassis_climb_status = kStatusLandSettle;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kDrop);
*chassis_climb_speed = -config.land.dash_vx;
Expand All@@ -512,6 +542,7 @@ class SentryClimber
co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)};

// [] 撑杆按照速度曲线收回，减少落地震动，并缓慢前进，让履带顺着台阶落下
*chassis_climb_status = kStatusLandSoft;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kLand);
*chassis_climb_speed = -config.land.soft_vx;
Expand All@@ -535,6 +566,7 @@ class SentryClimber
}

// [] 完全收回撑杆，结束下台阶
*chassis_climb_status = kStatusLandFinal;
track_group->set_state(TrackState::kFree);
stick_group->set_state(StickState::kRise);
*chassis_climb_speed = kNaN;
Expand All@@ -545,6 +577,13 @@ class SentryClimber
node::warn("land FINAL stick timeout, continue");
}

// [] 撑杆收回后，以一定速度向前（驶离台阶方向）运动一段时间
*chassis_climb_status = kStatusLandLeave;
track_group->set_state(TrackState::kHold);
stick_group->set_state(StickState::kHold);
*chassis_climb_speed = -config.land.leave_vx;
co_await CoSchduler::Sleep{seconds_to_duration(config.land.leave_duration)};

*chassis_climb_status = 1.0;
release_climber();
}
Expand Down