diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index f3d153c6..9ecf4c06 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -47,6 +47,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void powerHeatDataCallback(const rm_msgs::PowerHeatData::ConstPtr& data) override; void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override; void gameStatusCallback(const rm_msgs::GameStatus::ConstPtr& data) override; + void gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data) override; void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data) override; void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override; void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override; @@ -106,6 +107,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlVPress(); void ctrlBPress(); void ctrlRPress(); + void ctrlRReleasing(); virtual void ctrlQPress(); InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_, @@ -123,8 +125,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual geometry_msgs::PointStamped point_out_; uint8_t last_shoot_freq_{}; - bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, + bool prepare_shoot_ = false, turn_x_flag_ = false, turn_ctrl_r_flag_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false; - double yaw_current_{}; + double yaw_current_{}, pitch_pos_error_{}, pitch_pos_des_{}; }; } // namespace rm_manual diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index 2ade4707..32b7be2a 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -73,6 +74,9 @@ class ManualBase virtual void gimbalDesErrorCallback(const rm_msgs::GimbalDesError::ConstPtr& data) { } + virtual void gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data) + { + } virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { } @@ -145,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_, gimbal_pos_state_sub_, shoot_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 e7b2aba4..be0d4c6b 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -59,6 +59,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: ctrl_b_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlBPress, this)); ctrl_q_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlQPress, this)); ctrl_r_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlRPress, this)); + ctrl_r_event_.setActiveLow(boost::bind(&ChassisGimbalShooterManual::ctrlRReleasing, this)); shift_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::shiftPress, this), boost::bind(&ChassisGimbalShooterManual::shiftRelease, this)); mouse_left_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::mouseLeftPress, this)); @@ -140,6 +141,13 @@ void ChassisGimbalShooterManual::gimbalDesErrorCallback(const rm_msgs::GimbalDes shooter_cmd_sender_->updateGimbalDesError(*data); } +void ChassisGimbalShooterManual::gimbalPosStateCallback(const rm_msgs::GimbalPosState::ConstPtr& data) +{ + ManualBase::gimbalPosStateCallback(data); + pitch_pos_error_ = data->error; + pitch_pos_des_ = data->set_point; +} + void ChassisGimbalShooterManual::shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) { ChassisGimbalManual::shootBeforehandCmdCallback(data); @@ -196,7 +204,8 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() shooter_cmd_sender_->setZero(); shooter_calibration_->stop(); gimbal_calibration_->stop(); - turn_flag_ = false; + turn_x_flag_ = false; + turn_ctrl_r_flag_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -214,7 +223,8 @@ void ChassisGimbalShooterManual::robotDie() { ManualBase::robotDie(); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); - turn_flag_ = false; + turn_x_flag_ = false; + turn_ctrl_r_flag_ = false; use_scope_ = false; adjust_image_transmission_ = false; } @@ -544,7 +554,7 @@ void ChassisGimbalShooterManual::dPressing() void ChassisGimbalShooterManual::xPress() { - turn_flag_ = true; + turn_x_flag_ = true; geometry_msgs::PointStamped point_in; try { @@ -565,7 +575,7 @@ void ChassisGimbalShooterManual::xPress() void ChassisGimbalShooterManual::xReleasing() { - if (turn_flag_) + if (turn_x_flag_) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT); gimbal_cmd_sender_->setPoint(point_out_); @@ -574,7 +584,7 @@ void ChassisGimbalShooterManual::xReleasing() if (std::abs(angles::shortest_angular_distance(yaw, yaw_current_)) > finish_turning_threshold_) { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - turn_flag_ = false; + turn_x_flag_ = false; } } } @@ -613,6 +623,37 @@ void ChassisGimbalShooterManual::ctrlRPress() { if (image_transmission_cmd_sender_) adjust_image_transmission_ = !image_transmission_cmd_sender_->getState(); + if (adjust_image_transmission_) + { + turn_ctrl_r_flag_ = true; + geometry_msgs::PointStamped point_in; + try + { + point_in.header.frame_id = "yaw"; + point_in.point.x = 1.; + point_in.point.y = 0.; + point_in.point.z = tf_buffer_.lookupTransform("yaw", "pitch", ros::Time(0)).transform.translation.z + 0.8; + tf2::doTransform(point_in, point_out_, tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0))); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + } +} + +void ChassisGimbalShooterManual::ctrlRReleasing() +{ + if (turn_ctrl_r_flag_) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT); + gimbal_cmd_sender_->setPoint(point_out_); + if (pitch_pos_error_ > -0.005 && pitch_pos_des_ < -0.6) + { + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + turn_ctrl_r_flag_ = false; + } + } } void ChassisGimbalShooterManual::ctrlBPress() diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 87f73004..60439bdd 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -16,6 +16,8 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) track_sub_ = nh.subscribe("/track", 10, &ManualBase::trackCallback, this); gimbal_des_error_sub_ = nh.subscribe("/controllers/gimbal_controller/error", 10, &ManualBase::gimbalDesErrorCallback, this); + gimbal_pos_state_sub_ = nh.subscribe("/controllers/gimbal_controller/pitch/pos_state", 10, + &ManualBase::gimbalPosStateCallback, this); odom_sub_ = nh.subscribe("/odom", 10, &ManualBase::odomCallback, this); shoot_beforehand_cmd_sub_ = nh.subscribe("/controllers/gimbal_controller/bullet_solver/shoot_beforehand_cmd", 10,