Skip to content

Manual code for RM26 dart2. #152

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

Open
Aoalas wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Manual code for RM26 dart2. #152

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
131 changes: 102 additions & 29 deletions include/rm_manual/dart2_manual.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Created by chen_gr on 2025/3/9.
// Created by Aoalas on 2525/10/5.
//

#pragma once
Expand All @@ -11,6 +11,7 @@
#include <rm_msgs/GameStatus.h>
#include <unordered_map>
#include <rm_msgs/Dart.h>
#include <rm_msgs/DartInfo.h>

namespace rm_manual
{
Expand All @@ -37,7 +38,8 @@ class Dart2Manual : public ManualBase
ENGAGE = 2,
PULLUP = 3,
READY = 4,
PUSH = 5
PUSH = 5,
RECYCLE = 6
};
enum AutoAimState
{
Expand Down Expand Up @@ -76,9 +78,10 @@ class Dart2Manual : public ManualBase
bool triggerIsWorked() const;
bool triggerIsHome() const;
void gameRobotStatusCallback(const rm_msgs::GameRobotStatus::ConstPtr& data) override;
void gameRobotHpCallback(const rm_msgs::GameRobotHp::ConstPtr& data) override;
void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
void dartInfoCallback(const rm_msgs::DartInfo::ConstPtr& data);
void dartClientCmdCallback(const rm_msgs::DartClientCmd::ConstPtr& data);
void gameRobotHpCallback(const rm_msgs::GameRobotHp::ConstPtr& data) override;
void gameStatusCallback(const rm_msgs::GameStatus::ConstPtr& data) override;
void updateAllowDartDoorOpenTimes();
void wheelClockwise();
Expand All @@ -95,13 +98,24 @@ class Dart2Manual : public ManualBase
void engage();
void ready();
void push();
void recycle();

// void longCameraDataCallback(const rm_msgs::Dart::ConstPtr& data);
void shortCameraDataCallback(const rm_msgs::Dart::ConstPtr& data);
void cameraDataCallback(const rm_msgs::Dart::ConstPtr& data);
void updateAutoAimState();
void aim();
void adjust();
void autoAim();
bool shouldAutoAimCurrentDart() const;
bool isAutoFireRestrictionEnabled() const;
bool enemyOutpostAlive() const;

int enemy_outpost_hp_{ 0 };
int dart_current_target_;
int dart_remaining_time_;
bool enemy_outpost_dangerous_{ false };

bool enemy_outpost_ignore_{ false };
bool enemy_outpost_danger_detect_{ false };

rm_common::JointPointCommandSender* yaw_sender_;
rm_common::JointPointCommandSender* trigger_sender_;
Expand All @@ -118,95 +132,154 @@ class Dart2Manual : public ManualBase
std::unordered_map<std::string, std::vector<double>> target_position_{};

double scale_{ 0.05 }, scale_micro_{ 0.0005 };
bool if_stop_{ true }, has_stopped{ false }, is_reach_{ false }, is_calibrate_{ false }, trigger_has_work_{ false };
int has_fired_num_{};
bool confirm_place_{ false }, confirm_back_{ false };
ros::Time last_time_{};
uint8_t launch_mode_{ 0 }, last_launch_mode_{ 6 };
bool if_stop_{ true };

uint8_t launch_mode_{ 0 }, last_launch_mode_{ 5 };
uint8_t auto_aim_state_{ 0 }, last_auto_aim_state_{};

rm_msgs::DbusData dbus_data_;
uint8_t robot_id_, game_progress_{}, dart_launch_opening_status_{ 3 };
uint16_t target_change_time_{ 0 };
ros::Time gate_opened_time_{};
ros::Time gate_opening_start_time_{};
bool auto_shoot_active_{ false };
int auto_shoot_dart_count_{ 0 };

bool aborted_for_current_gate_{ false };
bool has_entered_battle_{ false };
uint8_t pc_last_game_progress_{};
bool target_found_once_{ false };
bool is_gate_actually_opening_{ false };
ros::Time camera_lost_start_time_{};
ros::Time pc_last_pulldown_time_{};

uint16_t remain_time_{ 420 };

int dart_fired_num_ = 0;
double current_x_offset_ = 0.0;
double target_x_offset = 0.0;
double current_y_offset_ = 0.0;
double target_y_offset_ = 0.0;
double random_fixed_set_point_ = 0.0;
double trigger_home_command_{}, trigger_work_command_{};
double trigger_confirm_home_{}, trigger_confirm_work_{};
double belt_left_position_{}, belt_right_position_{}, trigger_position_{};
double belt_left_max_{}, belt_right_max_{}, belt_left_min_{}, belt_right_min_{}, belt_left_slow_{},
belt_right_slow_{};
double range_velocity_ = 0., yaw_velocity_ = 0., load_velocity_ = 0.;

double short_camera_x_set_point_, long_camera_x_set_point_{}, long_camera_y_set_point_{};
double range_velocity_ = 0., yaw_velocity_ = 0., load_velocity_ = 0., load_position_ = 0.;

double camera_x_set_point_, camera_y_set_point_;
double camera_x_, camera_y_;
double camera_fast_p_x_, camera_normal_p_x_, camera_slow_p_x_, camera_retarget_slow_p_x_;
double camera_fast_x_threshold_, camera_normal_x_threshold_, camera_slow_x_threshold_;
double camera_x_before_push_{}, camera_x_after_push_{};

double short_camera_x_threshold_, long_camera_x_threshold_, retarget_threshold_;
double camera_x_offset_, camera_y_offset_;
double long_camera_p_x_, short_camera_p_x_, long_camera_p_y_;
double retarget_threshold_;

bool is_camera_found_{ false };
bool aim_failed_{ false };
bool camera_x_init_{ false };
bool is_long_camera_found_{ false }, is_short_camera_found_{ false }, is_long_camera_aim_{};
double long_camera_x_{}, long_camera_y_{}, short_camera_x_{}, short_camera_y_{}, last_camera_x{};
bool camera_central_{}, is_adjust_{};

bool camera_is_online_{};
bool use_auto_aim_{}, start_aim_{}, auto_aim_start_{};
bool random_fixed_target_{};
bool random_fixed_compensation_{ false };
bool get_reference_yaw_position_in_15s_{ false };
bool custom_auto_aim_{ false };
bool keep_auto_aim_effect_on_timeout_{ false };
bool had_adjust_{};

bool launch_result_{ false };

bool retarget{ false };
bool use_load_{ false }, load_start_{ false }, load_finished_{ false }, clamp_finished_{ false };
bool use_load_{ false }, load_start_{ false }, load_started_this_launch_{ false }, is_loading_{ false },
load_finished_{ false }, clamp_finished_{ false };
bool clamp_manual_{ true };
bool belt_left_ready_{ false }, belt_right_ready_{ false }, dart_ready_{ false }, vision_ready_{ false };
double clamp_left_release_position_, clamp_mid_release_position_, clamp_right_release_position_,
clamp_default_position_;

double load_init_position_, load_left_position_, load_mid_position_, load_right_position_;

double current_load_target_{ 0.0 }, cur_pos{ 0.0 };

double long_camera_x_before_push_{}, long_camera_x_after_push_{};

InputEvent wheel_clockwise_event_, wheel_anticlockwise_event_;

ros::Time last_engage_time_{};
ros::Time last_loading_time_{};
ros::Time last_loaded_time_{};
ros::Time last_push_time_{};
ros::Time last_ready_time_{};
ros::Time last_init_time_{};
ros::Time last_get_camera_data_time_{};
ros::Time pulldown_start_time_{};
ros::Time pullup_start_time_{};
ros::Time start_aim_time_{};
ros::Time last_adjust_time_{};
ros::Time last_clamp_finished_time_{};
ros::Time load_start_time_{};
ros::Time load_finish_time_{};
ros::Time vision_end_time_{};
ros::Time recycle_pulldown_start_time_{};
ros::Time recycle_pullup_start_time_{};
ros::Time recycle_start_time_{};
ros::Time reference_yaw_15s_stage_start_time_{};
ros::Time reference_yaw_15s_aim_start_time_{};

ros::Subscriber dart_client_cmd_sub_;
ros::Subscriber long_camera_data_sub_;
ros::Subscriber short_camera_data_sub_;
ros::Subscriber camera_data_sub_;
InputEvent dart_client_cmd_event_;
int outpost_hp_;

int allow_dart_door_open_times_ = 0, last_dart_door_status_ = 1;
bool triggered_30s_{ false }, triggered_4min_{ false };

int auto_state_ = BASE, manual_state_ = BASE, move_state_ = NORMAL;
int clamp_num_{ 1 };
int pc_once_fire_num_{ 4 }, all_pc_fired_num_{ 0 };
int auto_fire_num_{ 4 }, auto_fire_count_{ 0 };

bool temp_clamp_{ true }, temp_load_{ false };
bool temp_load_{ false };
double temp_load_position_{ 0.01 };

ros::Time push_failed_time = ros::Time::now();
bool push_failed = true, push_succeeded = false;
bool push_failed_ = true, push_succeeded_ = false, auto_push_ = false, auto_dart_ = false, load_skip_ = false;
bool auto_fire_restriction_{ false };
bool current_launch_auto_fire_{ false }, current_launch_auto_fire_counted_{ false };
bool pulldown_timeout_detect_{ false }, pullup_timeout_detect_{ false };
bool recycle_pulldown_timeout_detect_{ false };
bool recycle_timeout_detect_{ false };
double pulldown_timeout_s_{ 6.0 }, pullup_timeout_s_{ 6.0 };

bool left_recycle_done = false, right_recycle_done = false;
bool dart_recycling_ = false, recycle_mode_ = false, pc_target_detection_ = true, recycle_should_skip_load_ = false;
uint8_t recycle_step_{ 0 };
ros::Time recycle_timer_{};

double false_engage_time_{ 1.0 };
double random_fixed_reference_yaw_position_{ 0.0 };
double random_fixed_compensation_coefficient_{ 0.0 };

bool referee_data_okay_{ true };

ros::Time camera_target_visible_start_{};
ros::Time camera_target_lost_start_{};
ros::Time fake_gate_opened_time_{};
bool fake_gate_is_open_{ false };

bool reference_yaw_15s_started_{ false };
bool reference_yaw_15s_aiming_{ false };
bool reference_yaw_15s_finished_{ false };
bool reference_yaw_15s_success_{ false };
double reference_yaw_15s_set_point_{ 0.0 };

bool test_auto_aim_active_{ false };
bool test_camera_online_active_{ false };
bool load_init_toggle_active_{ false };
double test_yaw_start_pos_;
bool s_r_was_up_{ false }, s_l_was_down_{ true };

bool pc_locked_{ false }, reset_belt_{ false };

std::vector<bool> trigger_status_;
std::vector<bool> auto_aim_status_{ true, true, true, true };
};

} // namespace rm_manual
Loading
Loading