From 4b71f2f472fe60e1173ddceee3d65819890c9833 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 14 Jun 2024 17:55:41 +0800 Subject: [PATCH 1/4] Only shoot with tracking target when pressing mouseRight. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 1 + src/chassis_gimbal_shooter_manual.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index bd147d4b..7766afb8 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -64,6 +64,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual 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); + turn_flag_ = false; } void wPress() override; void aPress() override; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 2fcffb68..0b17f6a9 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -343,14 +343,16 @@ void ChassisGimbalShooterManual::mouseLeftPress() prepare_shoot_ = false; } if (prepare_shoot_) - { - shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); - shooter_cmd_sender_->checkError(ros::Time::now()); - } + if ((!turn_flag_) || (turn_flag_ && track_data_.id != 0)) + { + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + shooter_cmd_sender_->checkError(ros::Time::now()); + } } void ChassisGimbalShooterManual::mouseRightPress() { + turn_flag_ = true; if (track_data_.id == 0) gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else From 6438dd6367c367476b77e83472eb69a545cc294b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 15 Jun 2024 16:11:35 +0800 Subject: [PATCH 2/4] Modify something wrong. --- src/chassis_gimbal_shooter_manual.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 0b17f6a9..40bea015 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -343,11 +343,15 @@ void ChassisGimbalShooterManual::mouseLeftPress() prepare_shoot_ = false; } if (prepare_shoot_) - if ((!turn_flag_) || (turn_flag_ && track_data_.id != 0)) + { + if (!turn_flag_ || (turn_flag_ && track_data_.id != 0)) { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); shooter_cmd_sender_->checkError(ros::Time::now()); } + else + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + } } void ChassisGimbalShooterManual::mouseRightPress() From dd973ba9c5d51904948365b44e7520254940e4dc Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 8 Jul 2024 22:26:44 +0800 Subject: [PATCH 3/4] Add flag to judge mouseRightPess. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 4 ++-- src/chassis_gimbal_shooter_manual.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 7766afb8..2c468f8b 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -64,7 +64,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual 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); - turn_flag_ = false; + is_mouse_right_press_ = false; } void wPress() override; void aPress() override; @@ -122,7 +122,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, - adjust_image_transmission_ = false; + adjust_image_transmission_ = false, is_mouse_right_press_ = false; double yaw_current_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 40bea015..a9074095 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -189,6 +189,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() shooter_calibration_->stop(); gimbal_calibration_->stop(); turn_flag_ = false; + is_mouse_right_press_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -207,6 +208,7 @@ void ChassisGimbalShooterManual::robotDie() ManualBase::robotDie(); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); turn_flag_ = false; + is_mouse_right_press_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -344,7 +346,7 @@ void ChassisGimbalShooterManual::mouseLeftPress() } if (prepare_shoot_) { - if (!turn_flag_ || (turn_flag_ && track_data_.id != 0)) + if (!is_mouse_right_press_ || (is_mouse_right_press_ && track_data_.id != 0)) { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); shooter_cmd_sender_->checkError(ros::Time::now()); @@ -356,7 +358,7 @@ void ChassisGimbalShooterManual::mouseLeftPress() void ChassisGimbalShooterManual::mouseRightPress() { - turn_flag_ = true; + is_mouse_right_press_ = true; if (track_data_.id == 0) gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else From 7b782c3e80cfe81d03d36acb9b0bd9d222add79e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 9 Jul 2024 09:13:16 +0800 Subject: [PATCH 4/4] Modify to judge mouseRightPess. --- include/rm_manual/chassis_gimbal_shooter_manual.h | 3 +-- src/chassis_gimbal_shooter_manual.cpp | 5 +---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 2c468f8b..bd147d4b 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -64,7 +64,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual 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); - is_mouse_right_press_ = false; } void wPress() override; void aPress() override; @@ -122,7 +121,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, - adjust_image_transmission_ = false, is_mouse_right_press_ = false; + adjust_image_transmission_ = false; double yaw_current_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index a9074095..af95de4d 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -189,7 +189,6 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() shooter_calibration_->stop(); gimbal_calibration_->stop(); turn_flag_ = false; - is_mouse_right_press_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -208,7 +207,6 @@ void ChassisGimbalShooterManual::robotDie() ManualBase::robotDie(); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); turn_flag_ = false; - is_mouse_right_press_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -346,7 +344,7 @@ void ChassisGimbalShooterManual::mouseLeftPress() } if (prepare_shoot_) { - if (!is_mouse_right_press_ || (is_mouse_right_press_ && track_data_.id != 0)) + if (!mouse_right_event_.getState() || (mouse_right_event_.getState() && track_data_.id != 0)) { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); shooter_cmd_sender_->checkError(ros::Time::now()); @@ -358,7 +356,6 @@ void ChassisGimbalShooterManual::mouseLeftPress() void ChassisGimbalShooterManual::mouseRightPress() { - is_mouse_right_press_ = true; if (track_data_.id == 0) gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else