From 60a648af721f6aa1299c3de10cd7b091570df006 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 10:35:51 +0800 Subject: [PATCH 01/39] Add fuctions in standard. --- .../rm_manual/chassis_gimbal_shooter_cover_manual.h | 2 ++ include/rm_manual/chassis_gimbal_shooter_manual.h | 4 ++-- src/chassis_gimbal_shooter_cover_manual.cpp | 12 ++++++++++++ 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index c7474c5a..be56fc75 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -33,6 +33,8 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void zRelease(); void wPress() override; void wPressing() override; + void mouseRightPress() override; + void mouseRightRelease() override; virtual void ctrlZPress(); virtual void ctrlZRelease() diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index bd147d4b..eff3d94b 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -58,8 +58,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); prepare_shoot_ = true; } - void mouseRightPress(); - void mouseRightRelease() + virtual void mouseRightPress(); + virtual void mouseRightRelease() { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 0006949c..e717ecc2 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -132,6 +132,18 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise() supply_ = false; } +void ChassisGimbalShooterCoverManual::mouseRightPress() +{ + ChassisGimbalShooterManual::mouseRightPress(); + ROS_INFO("OK"); +} + +void ChassisGimbalShooterCoverManual::mouseRightRelease() +{ + ChassisGimbalShooterManual::mouseRightRelease(); + ROS_INFO("OK_release"); +} + void ChassisGimbalShooterCoverManual::rPress() { if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) From 8e91447ec3c91d0b640cd361e2b13d9baad50a17 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 15:30:43 +0800 Subject: [PATCH 02/39] Achieve auto_shoot by vision. --- .../chassis_gimbal_shooter_cover_manual.h | 2 ++ src/chassis_gimbal_shooter_cover_manual.cpp | 34 +++++++++++++++++-- 2 files changed, 34 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index be56fc75..ea4e9bc1 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -52,5 +52,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual ros::Time last_switch_time_; bool supply_ = false; bool cover_close_ = true; + bool is_auto_ = true; + int count_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index e717ecc2..632c9b1f 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -135,13 +135,43 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise() void ChassisGimbalShooterCoverManual::mouseRightPress() { ChassisGimbalShooterManual::mouseRightPress(); - ROS_INFO("OK"); + // 这里还欠一个判断is_auto的 + if (is_auto_) + { + if (!is_gyro_) + { + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + is_gyro_ = true; + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + else + vel_cmd_sender_->setAngularZVel(1.0); + } + if (track_data_.id == 0) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); + gimbal_cmd_sender_->setYawTraj(1, 800, count_); + gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.15); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + count_++; + } + else + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); + gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); + if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP) + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + } + } } void ChassisGimbalShooterCoverManual::mouseRightRelease() { ChassisGimbalShooterManual::mouseRightRelease(); - ROS_INFO("OK_release"); + is_auto_ = false; + count_ = 0; } void ChassisGimbalShooterCoverManual::rPress() From cc36deb80f7b6177d74b19ac2a31250f56ba888d Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 16:05:39 +0800 Subject: [PATCH 03/39] Add sub to subscribe event_data. --- include/rm_manual/manual_base.h | 6 +++++- src/manual_base.cpp | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index bcbda8b5..8b34069b 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +76,9 @@ class ManualBase virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { } + virtual void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) + { + } virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data) { } @@ -141,7 +145,7 @@ class ManualBase ros::Subscriber odom_sub_, dbus_sub_, track_sub_, referee_sub_, capacity_sub_, game_status_sub_, joint_state_sub_, game_robot_hp_sub_, actuator_state_sub_, power_heat_data_sub_, gimbal_des_error_sub_, game_robot_status_sub_, - suggest_fire_sub_, shoot_beforehand_cmd_sub_; + suggest_fire_sub_, shoot_beforehand_cmd_sub_, event_dart_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/manual_base.cpp b/src/manual_base.cpp index f3db6d50..b04c3354 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -30,10 +30,11 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) nh_referee.subscribe("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this); suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); - shoot_beforehand_cmd_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, &ManualBase::shootBeforehandCmdCallback, this); + event_dart_sub_ = + nh.subscribe("/rm_referee/event_data", 10, &ManualBase::eventDartCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From d489c5f9cb466a034f0c21a2240778e522d05e53 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 16:35:12 +0800 Subject: [PATCH 04/39] Modify wrong topic_name. --- src/manual_base.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/manual_base.cpp b/src/manual_base.cpp index b04c3354..08f0ae9a 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -33,8 +33,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) shoot_beforehand_cmd_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, &ManualBase::shootBeforehandCmdCallback, this); - event_dart_sub_ = - nh.subscribe("/rm_referee/event_data", 10, &ManualBase::eventDartCallback, this); + event_dart_sub_ = nh.subscribe("event_data", 10, &ManualBase::eventDartCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From 5d68d551bbdaea9c1ef3fc00f09c979e724bc722 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 17:02:50 +0800 Subject: [PATCH 05/39] Modify wrong nh. --- src/manual_base.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 08f0ae9a..933d7100 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -17,6 +17,11 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) gimbal_des_error_sub_ = nh.subscribe("/controllers/gimbal_controller/error", 10, &ManualBase::gimbalDesErrorCallback, this); odom_sub_ = nh.subscribe("/odom", 10, &ManualBase::odomCallback, this); + suggest_fire_sub_ = + nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); + shoot_beforehand_cmd_sub_ = + nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, + &ManualBase::shootBeforehandCmdCallback, this); game_robot_status_sub_ = nh_referee.subscribe("game_robot_status", 10, &ManualBase::gameRobotStatusCallback, this); @@ -28,12 +33,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) "power_management/sample_and_status", 10, &ManualBase::capacityDataCallback, this); power_heat_data_sub_ = nh_referee.subscribe("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this); - suggest_fire_sub_ = - nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); - shoot_beforehand_cmd_sub_ = - nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10, - &ManualBase::shootBeforehandCmdCallback, this); - event_dart_sub_ = nh.subscribe("event_data", 10, &ManualBase::eventDartCallback, this); + event_dart_sub_ = nh_referee.subscribe("event_data", 10, &ManualBase::eventDartCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From 03002f21e3ec1b70fe6fa047a7ef66cdf07cd756 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 17:20:27 +0800 Subject: [PATCH 06/39] Finish callback. --- include/rm_manual/chassis_gimbal_shooter_cover_manual.h | 1 + src/chassis_gimbal_shooter_cover_manual.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index ea4e9bc1..c6e1c8ff 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -35,6 +35,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void wPressing() override; void mouseRightPress() override; void mouseRightRelease() override; + void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; virtual void ctrlZPress(); virtual void ctrlZRelease() diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 632c9b1f..e4725d4c 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -252,4 +252,9 @@ void ChassisGimbalShooterCoverManual::ctrlZPress() } } +void ChassisGimbalShooterCoverManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) +{ + ChassisGimbalManual::eventDartCallback(data); +} + } // namespace rm_manual From 2358156dd08d173481e6a5b0a64dda73d1490970 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 09:18:34 +0800 Subject: [PATCH 07/39] Achieve auto_shoot in ChassisGimbalShooterManual. --- .../chassis_gimbal_shooter_cover_manual.h | 5 -- .../rm_manual/chassis_gimbal_shooter_manual.h | 14 ++- src/chassis_gimbal_shooter_cover_manual.cpp | 47 ---------- src/chassis_gimbal_shooter_manual.cpp | 86 ++++++++++++++++--- 4 files changed, 88 insertions(+), 64 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index c6e1c8ff..c7474c5a 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -33,9 +33,6 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void zRelease(); void wPress() override; void wPressing() override; - void mouseRightPress() override; - void mouseRightRelease() override; - void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; virtual void ctrlZPress(); virtual void ctrlZRelease() @@ -53,7 +50,5 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual ros::Time last_switch_time_; bool supply_ = false; bool cover_close_ = true; - bool is_auto_ = true; - int count_{}; }; } // namespace rm_manual diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index eff3d94b..f8d4f185 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -58,9 +58,14 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); prepare_shoot_ = true; } - virtual void mouseRightPress(); - virtual void mouseRightRelease() + void mouseRightPress(); + void mouseRightRelease() { + if (is_auto_) + { + is_auto_ = false; + count_ = 0; + } gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); @@ -104,7 +109,10 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlVPress(); void ctrlBPress(); void ctrlRPress(); + void judgeIsAuto(); + void sentryMode(); virtual void ctrlQPress(); + void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_, r_event_, v_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, shift_event_, @@ -123,5 +131,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false; double yaw_current_{}; + bool is_auto_ = true; + int count_{}, target_hit_by_dart{}, time_hit_by_dart{}, last_time_hit_by_dart{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index e4725d4c..0006949c 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -132,48 +132,6 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise() supply_ = false; } -void ChassisGimbalShooterCoverManual::mouseRightPress() -{ - ChassisGimbalShooterManual::mouseRightPress(); - // 这里还欠一个判断is_auto的 - if (is_auto_) - { - if (!is_gyro_) - { - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - is_gyro_ = true; - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); - else - vel_cmd_sender_->setAngularZVel(1.0); - } - if (track_data_.id == 0) - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - gimbal_cmd_sender_->setYawTraj(1, 800, count_); - gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.15); - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - count_++; - } - else - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); - gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); - if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP) - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); - } - } -} - -void ChassisGimbalShooterCoverManual::mouseRightRelease() -{ - ChassisGimbalShooterManual::mouseRightRelease(); - is_auto_ = false; - count_ = 0; -} - void ChassisGimbalShooterCoverManual::rPress() { if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) @@ -252,9 +210,4 @@ void ChassisGimbalShooterCoverManual::ctrlZPress() } } -void ChassisGimbalShooterCoverManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) -{ - ChassisGimbalManual::eventDartCallback(data); -} - } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 2fcffb68..58e8e93a 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -351,19 +351,25 @@ void ChassisGimbalShooterManual::mouseLeftPress() void ChassisGimbalShooterManual::mouseRightPress() { - if (track_data_.id == 0) - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + // judgeIsAuto(); + if (is_auto_ && robot_id_ != rm_msgs::GameRobotStatus::BLUE_HERO && robot_id_ != rm_msgs::GameRobotStatus::RED_HERO) + sentryMode(); else { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); - gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); - } - if (switch_armor_target_srv_->getArmorTarget() == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE) - { - if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP) + if (track_data_.id == 0) + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + else + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); + gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); + } + if (switch_armor_target_srv_->getArmorTarget() == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE) { - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); - shooter_cmd_sender_->checkError(ros::Time::now()); + if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP) + { + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + shooter_cmd_sender_->checkError(ros::Time::now()); + } } } } @@ -618,4 +624,64 @@ void ChassisGimbalShooterManual::ctrlQPress() shooter_calibration_->reset(); gimbal_calibration_->reset(); } + +void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) +{ + ChassisGimbalManual::eventDartCallback(data); + time_hit_by_dart = data->be_hit_time; + target_hit_by_dart = data->be_hit_target; +} + +void ChassisGimbalShooterManual::judgeIsAuto() +{ + ros::Time hit_time; + if (time_hit_by_dart != last_time_hit_by_dart) + { + last_time_hit_by_dart = time_hit_by_dart; + hit_time = ros::Time::now(); + is_auto_ = true; + } + // std::cout << " out hit_time is " << hit_time.toSec() << std::endl; + // std::cout << " out ROS TIME is " << ros::Time::now().toSec() << std::endl; + // while (is_auto_) + // { + double is = (ros::Time::now() - hit_time).toSec(); + std::cout << "cha is " << is << std::endl; + // } + if ((target_hit_by_dart == 1 && (ros::Time::now() - hit_time).toSec() >= 5) || + (target_hit_by_dart == 2 && (ros::Time::now() - hit_time).toSec() >= 10) || + (target_hit_by_dart == 3 && (ros::Time::now() - hit_time).toSec() >= 15)) + is_auto_ = false; +} + +void ChassisGimbalShooterManual::sentryMode() +{ + if (!is_gyro_) + { + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + is_gyro_ = true; + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + else + vel_cmd_sender_->setAngularZVel(1.0); + } + if (track_data_.id == 0) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); + gimbal_cmd_sender_->setYawTraj(1, 800, count_); + gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.2); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + count_++; + } + else + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); + gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); + if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP) + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + } +} } // namespace rm_manual From 7de88b844ff1930396671d66fa7e03df2cae884c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 09:22:42 +0800 Subject: [PATCH 08/39] Modify something wrong. --- src/chassis_gimbal_shooter_manual.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 58e8e93a..c090879f 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -658,7 +658,6 @@ void ChassisGimbalShooterManual::sentryMode() { if (!is_gyro_) { - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); is_gyro_ = true; chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); From a4ba9299fd039f25821da90c45958ba2f42ba642 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 10:49:40 +0800 Subject: [PATCH 09/39] Achieve different mode to auto. --- .../rm_manual/chassis_gimbal_shooter_manual.h | 3 ++- src/chassis_gimbal_shooter_manual.cpp | 22 +++++++------------ 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index f8d4f185..c5083969 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -131,7 +131,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false; double yaw_current_{}; - bool is_auto_ = true; + bool is_auto_ = false; + ros::Time hit_time_; int count_{}, target_hit_by_dart{}, time_hit_by_dart{}, last_time_hit_by_dart{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index c090879f..f056189e 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -351,7 +351,6 @@ void ChassisGimbalShooterManual::mouseLeftPress() void ChassisGimbalShooterManual::mouseRightPress() { - // judgeIsAuto(); if (is_auto_ && robot_id_ != rm_msgs::GameRobotStatus::BLUE_HERO && robot_id_ != rm_msgs::GameRobotStatus::RED_HERO) sentryMode(); else @@ -630,27 +629,22 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con ChassisGimbalManual::eventDartCallback(data); time_hit_by_dart = data->be_hit_time; target_hit_by_dart = data->be_hit_target; + judgeIsAuto(); } void ChassisGimbalShooterManual::judgeIsAuto() { - ros::Time hit_time; if (time_hit_by_dart != last_time_hit_by_dart) { last_time_hit_by_dart = time_hit_by_dart; - hit_time = ros::Time::now(); + hit_time_ = ros::Time::now(); is_auto_ = true; } - // std::cout << " out hit_time is " << hit_time.toSec() << std::endl; - // std::cout << " out ROS TIME is " << ros::Time::now().toSec() << std::endl; - // while (is_auto_) - // { - double is = (ros::Time::now() - hit_time).toSec(); - std::cout << "cha is " << is << std::endl; - // } - if ((target_hit_by_dart == 1 && (ros::Time::now() - hit_time).toSec() >= 5) || - (target_hit_by_dart == 2 && (ros::Time::now() - hit_time).toSec() >= 10) || - (target_hit_by_dart == 3 && (ros::Time::now() - hit_time).toSec() >= 15)) + if ((target_hit_by_dart == 1 && (ros::Time::now() - hit_time_).toSec() >= 5) || + (target_hit_by_dart == 2 && (ros::Time::now() - hit_time_).toSec() >= 10) || + (target_hit_by_dart == 3 && (ros::Time::now() - hit_time_).toSec() >= 15)) + is_auto_ = false; + if (target_hit_by_dart == 0) is_auto_ = false; } @@ -669,7 +663,7 @@ void ChassisGimbalShooterManual::sentryMode() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - gimbal_cmd_sender_->setYawTraj(1, 800, count_); + gimbal_cmd_sender_->setYawTraj(0, 800, count_); gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.2); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); count_++; From c9a66c6aee4b02815790c251b35e1fa56bfb5123 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 10:58:39 +0800 Subject: [PATCH 10/39] Make a reset. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 3 +-- src/chassis_gimbal_shooter_manual.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index c5083969..a03017d1 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -128,10 +128,9 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; - bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, + bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; double yaw_current_{}; - bool is_auto_ = false; ros::Time hit_time_; int count_{}, target_hit_by_dart{}, time_hit_by_dart{}, last_time_hit_by_dart{}; }; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index f056189e..9bea91c0 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -190,6 +190,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() gimbal_calibration_->stop(); turn_flag_ = false; use_scope_ = false; + is_auto_ = false; adjust_image_transmission_ = false; } @@ -208,6 +209,7 @@ void ChassisGimbalShooterManual::robotDie() shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); turn_flag_ = false; use_scope_ = false; + is_auto_ = false; adjust_image_transmission_ = false; } From a128ffd526b6ab75dd583cccb60beecd0d70f714 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 11:03:41 +0800 Subject: [PATCH 11/39] Modify something wrong. --- .../rm_manual/chassis_gimbal_shooter_manual.h | 2 +- src/chassis_gimbal_shooter_manual.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index a03017d1..6fb3d8e9 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -132,6 +132,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual adjust_image_transmission_ = false; double yaw_current_{}; ros::Time hit_time_; - int count_{}, target_hit_by_dart{}, time_hit_by_dart{}, last_time_hit_by_dart{}; + int count_{}, target_hit_by_dart_{}, time_hit_by_dart_{}, last_time_hit_by_dart_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 9bea91c0..f79dfbe0 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -629,24 +629,24 @@ void ChassisGimbalShooterManual::ctrlQPress() void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) { ChassisGimbalManual::eventDartCallback(data); - time_hit_by_dart = data->be_hit_time; - target_hit_by_dart = data->be_hit_target; + time_hit_by_dart_ = data->be_hit_time; + target_hit_by_dart_ = data->be_hit_target; judgeIsAuto(); } void ChassisGimbalShooterManual::judgeIsAuto() { - if (time_hit_by_dart != last_time_hit_by_dart) + if (time_hit_by_dart_ != last_time_hit_by_dart_) { - last_time_hit_by_dart = time_hit_by_dart; + last_time_hit_by_dart_ = time_hit_by_dart_; hit_time_ = ros::Time::now(); is_auto_ = true; } - if ((target_hit_by_dart == 1 && (ros::Time::now() - hit_time_).toSec() >= 5) || - (target_hit_by_dart == 2 && (ros::Time::now() - hit_time_).toSec() >= 10) || - (target_hit_by_dart == 3 && (ros::Time::now() - hit_time_).toSec() >= 15)) + if ((target_hit_by_dart_ == 1 && (ros::Time::now() - hit_time_).toSec() >= 5) || + (target_hit_by_dart_ == 2 && (ros::Time::now() - hit_time_).toSec() >= 10) || + (target_hit_by_dart_ == 3 && (ros::Time::now() - hit_time_).toSec() >= 15)) is_auto_ = false; - if (target_hit_by_dart == 0) + if (target_hit_by_dart_ == 0) is_auto_ = false; } From daea52a90ac956c1ced30715708c280f71f9dbb6 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 11:16:50 +0800 Subject: [PATCH 12/39] Modify something for try. --- src/chassis_gimbal_shooter_manual.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index f79dfbe0..4bd88dd4 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -665,7 +665,7 @@ void ChassisGimbalShooterManual::sentryMode() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - gimbal_cmd_sender_->setYawTraj(0, 800, count_); + gimbal_cmd_sender_->setYawTraj(1, 800, count_); gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.2); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); count_++; From 542fb1f9e4a34beb0050690cda9fc4ccd834cfaf Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 10 Jul 2024 17:40:45 +0800 Subject: [PATCH 13/39] Modify wrong of unable to be auto when press mouseRight twice. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 5 ----- src/chassis_gimbal_shooter_manual.cpp | 13 +++++++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 6fb3d8e9..01204cf1 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -61,11 +61,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void mouseRightPress(); void mouseRightRelease() { - if (is_auto_) - { - is_auto_ = false; - count_ = 0; - } gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 4bd88dd4..52fc8eb8 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -640,12 +640,17 @@ void ChassisGimbalShooterManual::judgeIsAuto() { last_time_hit_by_dart_ = time_hit_by_dart_; hit_time_ = ros::Time::now(); - is_auto_ = true; } - if ((target_hit_by_dart_ == 1 && (ros::Time::now() - hit_time_).toSec() >= 5) || - (target_hit_by_dart_ == 2 && (ros::Time::now() - hit_time_).toSec() >= 10) || - (target_hit_by_dart_ == 3 && (ros::Time::now() - hit_time_).toSec() >= 15)) + if (((target_hit_by_dart_ == 1 && (ros::Time::now() - hit_time_).toSec() <= 5) || + (target_hit_by_dart_ == 2 && (ros::Time::now() - hit_time_).toSec() <= 10) || + (target_hit_by_dart_ == 3 && (ros::Time::now() - hit_time_).toSec() <= 15)) && + mouse_right_event_.getState()) + is_auto_ = true; + else + { is_auto_ = false; + count_ = 0; + } if (target_hit_by_dart_ == 0) is_auto_ = false; } From 83976fc35751234eac2a666f8331654fd1b02d00 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Jul 2024 15:18:35 +0800 Subject: [PATCH 14/39] Modify the method of achievement. --- src/chassis_gimbal_shooter_manual.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 52fc8eb8..666f6518 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -670,8 +670,12 @@ void ChassisGimbalShooterManual::sentryMode() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - gimbal_cmd_sender_->setYawTraj(1, 800, count_); - gimbal_cmd_sender_->setPitchTraj(0.15, 1000, count_, 0.2); + double yaw_des, pitch_des; + yaw_des = M_PI * count_ / 900; + pitch_des = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; + count_ = (count_ + 1) % 900; + gimbal_cmd_sender_->yawTraj(yaw_des); + gimbal_cmd_sender_->pitchTraj(pitch_des); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); count_++; } From def92d68702b0bde5ec18d50820bc8e72f327656 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Jul 2024 15:47:16 +0800 Subject: [PATCH 15/39] Modify another function. --- src/chassis_gimbal_shooter_manual.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 666f6518..6da2ea62 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -674,8 +674,7 @@ void ChassisGimbalShooterManual::sentryMode() yaw_des = M_PI * count_ / 900; pitch_des = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; count_ = (count_ + 1) % 900; - gimbal_cmd_sender_->yawTraj(yaw_des); - gimbal_cmd_sender_->pitchTraj(pitch_des); + gimbal_cmd_sender_->setYawAndPitchTraj(yaw_des, pitch_des); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); count_++; } From 506e1e132b48ab62a7e7d966e54af675163ebd59 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Jul 2024 16:46:03 +0800 Subject: [PATCH 16/39] Modify the wrong of shoot when tracking and exiting auto_mode. --- src/chassis_gimbal_shooter_manual.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 6da2ea62..685c8c9d 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -355,6 +355,8 @@ void ChassisGimbalShooterManual::mouseRightPress() { if (is_auto_ && robot_id_ != rm_msgs::GameRobotStatus::BLUE_HERO && robot_id_ != rm_msgs::GameRobotStatus::RED_HERO) sentryMode(); + else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); else { if (track_data_.id == 0) From 617b8c3a3b9f53dad45b508299520fef0b9022e3 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 19 Jul 2024 17:34:53 +0800 Subject: [PATCH 17/39] Modify param name in msg. --- src/chassis_gimbal_shooter_manual.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 7ac11dc1..5629d0b5 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -686,11 +686,11 @@ void ChassisGimbalShooterManual::sentryMode() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - double yaw_des, pitch_des; - yaw_des = M_PI * count_ / 900; - pitch_des = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; + double traj_yaw, traj_pitch; + traj_yaw = M_PI * count_ / 900; + traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; count_ = (count_ + 1) % 900; - gimbal_cmd_sender_->setYawAndPitchTraj(yaw_des, pitch_des); + gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); count_++; } From 21de4a4fcb49f9b8ddf2775110c39d072d3ccec0 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 19 Jul 2024 23:55:08 +0800 Subject: [PATCH 18/39] Delete judgeIsAuto() for clean. --- .../rm_manual/chassis_gimbal_shooter_manual.h | 3 +-- src/chassis_gimbal_shooter_manual.cpp | 21 +++++++------------ 2 files changed, 9 insertions(+), 15 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 863df1a0..9346d0f5 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -105,7 +105,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlVPress(); void ctrlBPress(); void ctrlRPress(); - void judgeIsAuto(); void sentryMode(); virtual void ctrlQPress(); void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; @@ -129,6 +128,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual adjust_image_transmission_ = false; double yaw_current_{}; ros::Time hit_time_; - int count_{}, target_hit_by_dart_{}, time_hit_by_dart_{}, last_time_hit_by_dart_{}; + int count_{}, last_time_hit_by_dart_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 5629d0b5..f24c424f 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -645,21 +645,16 @@ void ChassisGimbalShooterManual::ctrlQPress() void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) { ChassisGimbalManual::eventDartCallback(data); - time_hit_by_dart_ = data->be_hit_time; - target_hit_by_dart_ = data->be_hit_target; - judgeIsAuto(); -} - -void ChassisGimbalShooterManual::judgeIsAuto() -{ - if (time_hit_by_dart_ != last_time_hit_by_dart_) + double time_hit_by_dart = data->be_hit_time; + double target_hit_by_dart = data->be_hit_target; + if (time_hit_by_dart != last_time_hit_by_dart_) { - last_time_hit_by_dart_ = time_hit_by_dart_; + last_time_hit_by_dart_ = time_hit_by_dart; hit_time_ = ros::Time::now(); } - if (((target_hit_by_dart_ == 1 && (ros::Time::now() - hit_time_).toSec() <= 5) || - (target_hit_by_dart_ == 2 && (ros::Time::now() - hit_time_).toSec() <= 10) || - (target_hit_by_dart_ == 3 && (ros::Time::now() - hit_time_).toSec() <= 15)) && + if (((target_hit_by_dart == 1 && (ros::Time::now() - hit_time_).toSec() <= 5) || + (target_hit_by_dart == 2 && (ros::Time::now() - hit_time_).toSec() <= 10) || + (target_hit_by_dart == 3 && (ros::Time::now() - hit_time_).toSec() <= 15)) && mouse_right_event_.getState()) is_auto_ = true; else @@ -667,7 +662,7 @@ void ChassisGimbalShooterManual::judgeIsAuto() is_auto_ = false; count_ = 0; } - if (target_hit_by_dart_ == 0) + if (target_hit_by_dart == 0) is_auto_ = false; } From fbf8399283fa9f63abca4ecc2ff1d4b983af8450 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 00:00:30 +0800 Subject: [PATCH 19/39] Move place where setting count_. --- src/chassis_gimbal_shooter_manual.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index f24c424f..2323e199 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -651,6 +651,7 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con { last_time_hit_by_dart_ = time_hit_by_dart; hit_time_ = ros::Time::now(); + count_ = 0; } if (((target_hit_by_dart == 1 && (ros::Time::now() - hit_time_).toSec() <= 5) || (target_hit_by_dart == 2 && (ros::Time::now() - hit_time_).toSec() <= 10) || @@ -658,10 +659,7 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con mouse_right_event_.getState()) is_auto_ = true; else - { is_auto_ = false; - count_ = 0; - } if (target_hit_by_dart == 0) is_auto_ = false; } From caff303b055100d33e80d5c6cbd890df5acf1180 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 00:07:06 +0800 Subject: [PATCH 20/39] Add init. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 9346d0f5..726db036 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -127,7 +127,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; double yaw_current_{}; - ros::Time hit_time_; + ros::Time hit_time_{}; int count_{}, last_time_hit_by_dart_{}; }; } // namespace rm_manual From ebee8b8a23d97abb3211d3bc5fbfa52a268df63f Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 00:07:35 +0800 Subject: [PATCH 21/39] Add init. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 726db036..4991758e 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -124,10 +124,11 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; uint8_t last_shoot_freq_{}; + ros::Time hit_time_{}; + bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; double yaw_current_{}; - ros::Time hit_time_{}; int count_{}, last_time_hit_by_dart_{}; }; } // namespace rm_manual From ac62f700e8e2b9bb33d80a1c25bff2c3ece83847 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 00:15:48 +0800 Subject: [PATCH 22/39] Use enum to replace number. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 9 ++++++++- src/chassis_gimbal_shooter_manual.cpp | 6 +++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 4991758e..c97b5daf 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -124,8 +124,15 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; uint8_t last_shoot_freq_{}; + enum TargetHitByDart + { + OUTPOST = 1, + BASE = 2, + MOVE_BASE = 3 + }; + ros::Time hit_time_{}; - + bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; double yaw_current_{}; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 2323e199..1dc8e446 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -653,9 +653,9 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con hit_time_ = ros::Time::now(); count_ = 0; } - if (((target_hit_by_dart == 1 && (ros::Time::now() - hit_time_).toSec() <= 5) || - (target_hit_by_dart == 2 && (ros::Time::now() - hit_time_).toSec() <= 10) || - (target_hit_by_dart == 3 && (ros::Time::now() - hit_time_).toSec() <= 15)) && + if (((target_hit_by_dart == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || + (target_hit_by_dart == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || + (target_hit_by_dart == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) && mouse_right_event_.getState()) is_auto_ = true; else From fca04e4812410661b959251e47a35edd7751463e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 00:23:04 +0800 Subject: [PATCH 23/39] Delete something unused. --- src/chassis_gimbal_shooter_manual.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 1dc8e446..c08c8522 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -688,12 +688,6 @@ void ChassisGimbalShooterManual::sentryMode() count_++; } else - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); - gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); - if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP) - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); - } } } // namespace rm_manual From a265dc66ba4a174485e273176857141dde388d43 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 15:56:48 +0800 Subject: [PATCH 24/39] Delete something unused. --- src/chassis_gimbal_shooter_manual.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index c08c8522..b160aefc 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -685,7 +685,6 @@ void ChassisGimbalShooterManual::sentryMode() count_ = (count_ + 1) % 900; gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - count_++; } else shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); From a7f8a197d961c8f70d89d6cd54ee51477ea53979 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 16:26:01 +0800 Subject: [PATCH 25/39] Override mouseRightPress in ChassisGimbalShooterCoverManual. --- include/rm_manual/chassis_gimbal_shooter_cover_manual.h | 1 + include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- src/chassis_gimbal_shooter_cover_manual.cpp | 4 ++++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 6773fde3..fc95aa2f 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -33,6 +33,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void zRelease(); void wPress() override; void wPressing() override; + void mouseRightPress() override; virtual void ctrlZPress(); virtual void ctrlZRelease() diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index c97b5daf..85893b51 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -59,7 +59,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); prepare_shoot_ = true; } - void mouseRightPress(); + virtual void mouseRightPress(); void mouseRightRelease() { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 0c685f5f..2884353e 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -211,4 +211,8 @@ void ChassisGimbalShooterCoverManual::ctrlZPress() } } +void ChassisGimbalShooterCoverManual::mouseRightPress() +{ +} + } // namespace rm_manual From 3650acdd4993fe66ddef32ac8d8732fe41ee7765 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 16:33:38 +0800 Subject: [PATCH 26/39] Achieve sentry mode in ChassisGimbalShooterCoverManual. --- .../chassis_gimbal_shooter_cover_manual.h | 1 + .../rm_manual/chassis_gimbal_shooter_manual.h | 1 - src/chassis_gimbal_shooter_cover_manual.cpp | 31 +++++++++++++++ src/chassis_gimbal_shooter_manual.cpp | 39 ++----------------- 4 files changed, 35 insertions(+), 37 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index fc95aa2f..21779e5d 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -40,6 +40,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); }; + void sentryMode(); double low_speed_scale_{}, normal_speed_scale_{}; double exit_buff_mode_duration_{}; rm_common::SwitchDetectionCaller* switch_buff_srv_{}; diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 85893b51..633fe2d6 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -105,7 +105,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlVPress(); void ctrlBPress(); void ctrlRPress(); - void sentryMode(); virtual void ctrlQPress(); void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 2884353e..b3c58c99 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -213,6 +213,37 @@ void ChassisGimbalShooterCoverManual::ctrlZPress() void ChassisGimbalShooterCoverManual::mouseRightPress() { + if (is_auto_) + sentryMode(); + else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + else + ChassisGimbalShooterManual::mouseRightPress(); } +void ChassisGimbalShooterCoverManual::sentryMode() +{ + if (!is_gyro_) + { + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); + is_gyro_ = true; + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + else + vel_cmd_sender_->setAngularZVel(1.0); + } + if (track_data_.id == 0) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); + double traj_yaw, traj_pitch; + traj_yaw = M_PI * count_ / 900; + traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; + count_ = (count_ + 1) % 900; + gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + } + else + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); +} } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index b160aefc..26b0c25f 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -367,17 +367,11 @@ void ChassisGimbalShooterManual::mouseLeftPress() void ChassisGimbalShooterManual::mouseRightPress() { - if (is_auto_ && robot_id_ != rm_msgs::GameRobotStatus::BLUE_HERO && robot_id_ != rm_msgs::GameRobotStatus::RED_HERO) - sentryMode(); - else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + if (track_data_.id == 0) + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else { - if (track_data_.id == 0) - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - else - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); } if (switch_armor_target_srv_->getArmorTarget() == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE) @@ -388,7 +382,6 @@ void ChassisGimbalShooterManual::mouseRightPress() shooter_cmd_sender_->checkError(ros::Time::now()); } } - } } void ChassisGimbalShooterManual::ePress() @@ -663,30 +656,4 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con if (target_hit_by_dart == 0) is_auto_ = false; } - -void ChassisGimbalShooterManual::sentryMode() -{ - if (!is_gyro_) - { - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - is_gyro_ = true; - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); - else - vel_cmd_sender_->setAngularZVel(1.0); - } - if (track_data_.id == 0) - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - double traj_yaw, traj_pitch; - traj_yaw = M_PI * count_ / 900; - traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; - count_ = (count_ + 1) % 900; - gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - } - else - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); -} } // namespace rm_manual From d55c02814269194c1ca076d120df9c5fa89e7ebe Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 20 Jul 2024 16:47:25 +0800 Subject: [PATCH 27/39] Modify name. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- include/rm_manual/manual_base.h | 4 ++-- src/chassis_gimbal_shooter_manual.cpp | 6 +++--- src/manual_base.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 633fe2d6..f8b3c1f9 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -106,7 +106,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlBPress(); void ctrlRPress(); virtual void ctrlQPress(); - void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override; + void eventDataCallback(const rm_msgs::EventData ::ConstPtr& data) override; InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_, r_event_, v_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, shift_event_, diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 8b34069b..8be3fc4d 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -76,7 +76,7 @@ class ManualBase virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { } - virtual void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) + virtual void eventDataCallback(const rm_msgs::EventData ::ConstPtr& data) { } virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data) @@ -145,7 +145,7 @@ class ManualBase ros::Subscriber odom_sub_, dbus_sub_, track_sub_, referee_sub_, capacity_sub_, game_status_sub_, joint_state_sub_, game_robot_hp_sub_, actuator_state_sub_, power_heat_data_sub_, gimbal_des_error_sub_, game_robot_status_sub_, - suggest_fire_sub_, shoot_beforehand_cmd_sub_, event_dart_sub_; + suggest_fire_sub_, shoot_beforehand_cmd_sub_, event_data_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 26b0c25f..81abe5b1 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -635,9 +635,9 @@ void ChassisGimbalShooterManual::ctrlQPress() gimbal_calibration_->reset(); } -void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::ConstPtr& data) +void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::ConstPtr& data) { - ChassisGimbalManual::eventDartCallback(data); + ChassisGimbalManual::eventDataCallback(data); double time_hit_by_dart = data->be_hit_time; double target_hit_by_dart = data->be_hit_target; if (time_hit_by_dart != last_time_hit_by_dart_) @@ -653,7 +653,7 @@ void ChassisGimbalShooterManual::eventDartCallback(const rm_msgs::EventData::Con is_auto_ = true; else is_auto_ = false; - if (target_hit_by_dart == 0) + if (time_hit_by_dart == 0) is_auto_ = false; } } // namespace rm_manual diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 933d7100..2bc8504b 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -33,7 +33,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) "power_management/sample_and_status", 10, &ManualBase::capacityDataCallback, this); power_heat_data_sub_ = nh_referee.subscribe("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this); - event_dart_sub_ = nh_referee.subscribe("event_data", 10, &ManualBase::eventDartCallback, this); + event_data_sub_ = nh_referee.subscribe("event_data", 10, &ManualBase::eventDataCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From 957af8e2e35c90835e8828612cb0d2c148e22cbe Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 23 Jul 2024 20:44:38 +0800 Subject: [PATCH 28/39] Update for clean. --- src/chassis_gimbal_shooter_cover_manual.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index b3c58c99..6062c5d8 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -213,12 +213,11 @@ void ChassisGimbalShooterCoverManual::ctrlZPress() void ChassisGimbalShooterCoverManual::mouseRightPress() { + ChassisGimbalShooterManual::mouseRightPress(); if (is_auto_) sentryMode(); else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - else - ChassisGimbalShooterManual::mouseRightPress(); } void ChassisGimbalShooterCoverManual::sentryMode() @@ -236,9 +235,8 @@ void ChassisGimbalShooterCoverManual::sentryMode() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - double traj_yaw, traj_pitch; - traj_yaw = M_PI * count_ / 900; - traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; + double traj_yaw = M_PI * count_ / 900; + double traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; count_ = (count_ + 1) % 900; gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); From 112dff340907b67fbbba8a992d978c5d0b48219d Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 23 Jul 2024 20:55:23 +0800 Subject: [PATCH 29/39] Update for clean. --- .../chassis_gimbal_shooter_cover_manual.h | 1 - src/chassis_gimbal_shooter_cover_manual.cpp | 40 ++++++++----------- 2 files changed, 16 insertions(+), 25 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 21779e5d..fc95aa2f 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -40,7 +40,6 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); }; - void sentryMode(); double low_speed_scale_{}, normal_speed_scale_{}; double exit_buff_mode_duration_{}; rm_common::SwitchDetectionCaller* switch_buff_srv_{}; diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 6062c5d8..195f85f3 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -215,33 +215,25 @@ void ChassisGimbalShooterCoverManual::mouseRightPress() { ChassisGimbalShooterManual::mouseRightPress(); if (is_auto_) - sentryMode(); - else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); -} - -void ChassisGimbalShooterCoverManual::sentryMode() -{ - if (!is_gyro_) { - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); - is_gyro_ = true; - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + if (!is_gyro_) + { + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + setChassisMode(rm_msgs::ChassisCmd::RAW); + } + if (track_data_.id == 0) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); + double traj_yaw = M_PI * count_ / 900; + double traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; + count_ = (count_ + 1) % 900; + gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + } else - vel_cmd_sender_->setAngularZVel(1.0); + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); } - if (track_data_.id == 0) - { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - double traj_yaw = M_PI * count_ / 900; - double traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; - count_ = (count_ + 1) % 900; - gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); + else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); - } - else - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); } } // namespace rm_manual From 9dfaf3f86dcdcde5332237ac6c8b9bddfbe23e5d Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 15:43:20 +0800 Subject: [PATCH 30/39] Fix wrong. --- include/rm_manual/manual_base.h | 2 +- src/manual_base.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index fa317b48..422ea149 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -149,7 +149,7 @@ class ManualBase ros::Subscriber odom_sub_, dbus_sub_, track_sub_, referee_sub_, capacity_sub_, game_status_sub_, joint_state_sub_, game_robot_hp_sub_, actuator_state_sub_, power_heat_data_sub_, gimbal_des_error_sub_, game_robot_status_sub_, - suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_; + suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_, event_data_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 2bc8504b..7194b597 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -34,6 +34,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) power_heat_data_sub_ = nh_referee.subscribe("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this); event_data_sub_ = nh_referee.subscribe("event_data", 10, &ManualBase::eventDataCallback, this); + shoot_data_sub_ = nh_referee.subscribe("shoot_data", 10, &ManualBase::shootDataCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1); From 3fa7c25264ff2a08feffba215fff97339ce7df16 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 21:48:19 +0800 Subject: [PATCH 31/39] Modify the method of count cycle. --- src/chassis_gimbal_shooter_cover_manual.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 195f85f3..019c0e54 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -225,13 +225,15 @@ void ChassisGimbalShooterCoverManual::mouseRightPress() { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); double traj_yaw = M_PI * count_ / 900; - double traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2; - count_ = (count_ + 1) % 900; + double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 900) / 900) + 0.15; + count_++; gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } else + { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + } } else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); From 069fc0f15ca2f5e8283d5f871cf2593709952c11 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:01:27 +0800 Subject: [PATCH 32/39] Modify function name. --- src/chassis_gimbal_shooter_cover_manual.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 019c0e54..01eb1135 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -227,7 +227,7 @@ void ChassisGimbalShooterCoverManual::mouseRightPress() double traj_yaw = M_PI * count_ / 900; double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 900) / 900) + 0.15; count_++; - gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch); + gimbal_cmd_sender_->setGimbalTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } else From 91c2bd0eee08298fc9603da285f955516349c3fe Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:07:44 +0800 Subject: [PATCH 33/39] Fix wrong. --- src/chassis_gimbal_shooter_cover_manual.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 01eb1135..34a5e67b 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -233,6 +233,7 @@ void ChassisGimbalShooterCoverManual::mouseRightPress() else { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + shooter_cmd_sender_->checkError(ros::Time::now()); } } else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) From 60b3b949aae9b6d5a64f050d10456a03aeb3f4a1 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:15:24 +0800 Subject: [PATCH 34/39] Update for clean. --- src/chassis_gimbal_shooter_manual.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index f68f81e9..553f632f 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -632,22 +632,20 @@ void ChassisGimbalShooterManual::ctrlQPress() void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::ConstPtr& data) { ChassisGimbalManual::eventDataCallback(data); - double time_hit_by_dart = data->be_hit_time; - double target_hit_by_dart = data->be_hit_target; - if (time_hit_by_dart != last_time_hit_by_dart_) + if (data->be_hit_time != last_time_hit_by_dart_) { - last_time_hit_by_dart_ = time_hit_by_dart; + last_time_hit_by_dart_ = data->be_hit_time; hit_time_ = ros::Time::now(); count_ = 0; } - if (((target_hit_by_dart == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || - (target_hit_by_dart == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || - (target_hit_by_dart == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) && + if (((data->be_hit_target == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || + (data->be_hit_target == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || + (data->be_hit_target == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) && mouse_right_event_.getState()) is_auto_ = true; else is_auto_ = false; - if (time_hit_by_dart == 0) + if (data->be_hit_time == 0) is_auto_ = false; } } // namespace rm_manual From b4016e9a499d6500b0c7bef5e1af725ef0fa7d83 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:17:40 +0800 Subject: [PATCH 35/39] Delete something unused. --- src/chassis_gimbal_shooter_manual.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 553f632f..a064f7fc 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -638,10 +638,9 @@ void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::Con hit_time_ = ros::Time::now(); count_ = 0; } - if (((data->be_hit_target == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || - (data->be_hit_target == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || - (data->be_hit_target == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) && - mouse_right_event_.getState()) + if ((data->be_hit_target == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || + (data->be_hit_target == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || + (data->be_hit_target == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) is_auto_ = true; else is_auto_ = false; From 1b90fb40d5df539c9284b0c75b34155c50271646 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:30:51 +0800 Subject: [PATCH 36/39] Delete something unused. --- src/chassis_gimbal_shooter_manual.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index a064f7fc..600794e3 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -644,7 +644,5 @@ void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::Con is_auto_ = true; else is_auto_ = false; - if (data->be_hit_time == 0) - is_auto_ = false; } } // namespace rm_manual From 8b9157308d2f295c3cad6ed14bdb0b46ce8e8be2 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 26 Jul 2024 22:34:25 +0800 Subject: [PATCH 37/39] Modify a param name. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- src/chassis_gimbal_shooter_manual.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 265964e0..abdb84b7 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -131,7 +131,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual MOVE_BASE = 3 }; - ros::Time hit_time_{}; + ros::Time start_timer_time_{}; bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 600794e3..c8e92d66 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -635,12 +635,12 @@ void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::Con if (data->be_hit_time != last_time_hit_by_dart_) { last_time_hit_by_dart_ = data->be_hit_time; - hit_time_ = ros::Time::now(); + start_timer_time_ = ros::Time::now(); count_ = 0; } - if ((data->be_hit_target == OUTPOST && (ros::Time::now() - hit_time_).toSec() <= 5) || - (data->be_hit_target == BASE && (ros::Time::now() - hit_time_).toSec() <= 10) || - (data->be_hit_target == MOVE_BASE && (ros::Time::now() - hit_time_).toSec() <= 15)) + if ((data->be_hit_target == OUTPOST && (ros::Time::now() - start_timer_time_).toSec() <= 5) || + (data->be_hit_target == BASE && (ros::Time::now() - start_timer_time_).toSec() <= 10) || + (data->be_hit_target == MOVE_BASE && (ros::Time::now() - start_timer_time_).toSec() <= 15)) is_auto_ = true; else is_auto_ = false; From e91fe4f2555fb443363eeb5eafee068efe03495b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 30 Jul 2024 11:31:52 +0800 Subject: [PATCH 38/39] Fix wrong from merge. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 0ff65169..81761821 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -134,7 +134,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual ros::Time start_timer_time_{}; - bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, + bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false, adjust_image_transmission_ = false; double yaw_current_{}; int count_{}, last_time_hit_by_dart_{}; From bf8f6ff2056caff352798d065c4e31ae24dc572e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 30 Jul 2024 12:25:41 +0800 Subject: [PATCH 39/39] Slow down rotate speed. --- src/chassis_gimbal_shooter_cover_manual.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 34a5e67b..39ea111e 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -224,8 +224,8 @@ void ChassisGimbalShooterCoverManual::mouseRightPress() if (track_data_.id == 0) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - double traj_yaw = M_PI * count_ / 900; - double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 900) / 900) + 0.15; + double traj_yaw = M_PI * count_ / 1000; + double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 1100) / 1100) + 0.15; count_++; gimbal_cmd_sender_->setGimbalTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);