From aae52109f9fd419e66830c84fdd8ff9a6783205f Mon Sep 17 00:00:00 2001 From: Bluefairy-ljy <3099568863@qq.com> Date: Sat, 3 Oct 2026 22:30:40 +0800 Subject: [PATCH] Use full ChassisGimbalShooterManual for 2026 RMUC hero. --- include/rm_manual/chassis_gimbal_manual.h | 2 +- .../chassis_gimbal_shooter_cover_manual.h | 31 +- .../rm_manual/chassis_gimbal_shooter_manual.h | 56 +- include/rm_manual/manual_base.h | 17 +- src/balance_manual.cpp | 15 +- src/chassis_gimbal_manual.cpp | 5 - src/chassis_gimbal_shooter_cover_manual.cpp | 150 +----- src/chassis_gimbal_shooter_manual.cpp | 505 ++++++++++++------ src/manual_base.cpp | 10 +- 9 files changed, 445 insertions(+), 346 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_manual.h b/include/rm_manual/chassis_gimbal_manual.h index 3a17f78f..66f1d77e 100644 --- a/include/rm_manual/chassis_gimbal_manual.h +++ b/include/rm_manual/chassis_gimbal_manual.h @@ -62,7 +62,7 @@ class ChassisGimbalManual : public ManualBase rm_common::ChassisCommandSender* chassis_cmd_sender_{}; double x_scale_{}, y_scale_{}; - bool is_gyro_{ 0 }, wheels_offline_ = false; + bool is_gyro_{ false }, wheels_offline_{ false }; double speed_change_scale_{ 1. }; double gimbal_scale_{ 1. }; double traj_scale_{ 0.5 }; diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 5771ddc6..a72150a1 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -22,25 +22,21 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void changeSpeedMode(SpeedMode speed_mode); double getDynamicScale(const double base_scale, const double amplitude, const double period, const double phase); void changeGyroSpeedMode(SpeedMode speed_mode); - void checkWheelsOnline(); void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void checkReferee() override; void sendCommand(const ros::Time& time) override; - void updateWheelsState(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data, - const std::vector& chassis_motor); void rightSwitchDownRise() override; void rightSwitchMidRise() override; void rightSwitchUpRise() override; - void wheelsOnlineCallback(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data); - void gameRobotStatusCallback(const rm_msgs::GameRobotStatus::ConstPtr& data) override; + void mouseRightPress() override; void ePress() override; void eRelease() override; void bPress() override; void cPress() override; void qPress() override; void ctrlRPressing(); - void ctrlRRelease() override; + void ctrlRRelease(); void wPress() override; void wPressing() override; void aPressing() override; @@ -50,15 +46,17 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void aRelease() override; void sRelease() override; void dRelease() override; + void bRelease() override + { + } void zPress() override { } - - virtual void ctrlZPress(); - virtual void ctrlZRelease() + void ctrlCPress() override { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - }; + } + virtual void ctrlZPress(); + virtual void ctrlZRelease(); virtual void ctrlXPress(); ros::Subscriber wheel_online_sub_; @@ -70,17 +68,12 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual rm_common::SwitchDetectionCaller* switch_buff_srv_{}; rm_common::SwitchDetectionCaller* switch_buff_type_srv_{}; rm_common::SwitchDetectionCaller* switch_exposure_srv_{}; - rm_common::JointPositionBinaryCommandSender* cover_command_sender_{}; InputEvent ctrl_z_event_, z_event_; std::string supply_frame_; std::string wireless_frame_; - ros::Time last_switch_time_, last_check_wheels_time_; - std::vector chassis_motor_; - std::vector wheels_online_state_; - bool supply_ = false; - bool need_wireless_ = false; - bool cover_close_ = true; - bool last_power_chassis_output_ = false; + ros::Time last_switch_time_; + bool supply_ = { false }; + bool need_wireless_ = { false }; 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 efa30bad..9b0fee44 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -6,7 +6,9 @@ #include "rm_manual/chassis_gimbal_manual.h" #include +#include #include +#include namespace rm_manual { @@ -19,10 +21,14 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual protected: void ecatReconnected() override; void checkReferee() override; + void checkWheelsOnline(); void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void sendCommand(const ros::Time& time) override; + void updateWheelsState(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data, + const std::vector& chassis_motor); + void wheelsOnlineCallback(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data); void chassisOutputOn() override; void shooterOutputOn() override; void gimbalOutputOn() override; @@ -37,7 +43,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void remoteControlTurnOff() override; void remoteControlTurnOn() override; void robotDie() override; - void robotRevive() override; void rightSwitchDownRise() override; void rightSwitchMidRise() override; void rightSwitchUpRise() override; @@ -54,6 +59,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override; void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override; void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data) override; + void ballisticSolutionCallback(const std_msgs::Float32MultiArray::ConstPtr& data) override; + void protectStateCallback(const std_msgs::Bool::ConstPtr& data) override; void leftSwitchUpOn(ros::Duration duration); void leftSwitchUpFall(); void mouseLeftPress(); @@ -62,13 +69,14 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); prepare_shoot_ = true; } - void mouseRightPress(); + virtual void mouseRightPress(); void mouseRightRelease() { + if (deployed_) + return; 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); } + void mouseRightRising(); void wPress() override; void aPress() override; void sPress() override; @@ -94,12 +102,8 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual virtual void shiftPress(); virtual void shiftRelease(); virtual void rPress(); - virtual void qPress() - { - } - virtual void qRelease() - { - } + virtual void qPress(); + void ctrlFPress() { shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); @@ -108,31 +112,45 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual void ctrlRPress(); void ctrlZPress(); void ctrlXPress(); - virtual void ctrlRRelease(); + virtual void ctrlCPress(); virtual void ctrlQPress(); virtual void ctrlBPress(); InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_, r_event_, v_event_, z_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, - ctrl_z_event_, ctrl_x_event_, shift_event_, ctrl_shift_b_event_, mouse_left_event_, mouse_right_event_; + ctrl_z_event_, ctrl_c_event_, ctrl_x_event_, shift_event_, mouse_left_event_, mouse_right_event_; rm_common::ShooterCommandSender* shooter_cmd_sender_{}; rm_common::CameraSwitchCommandSender* camera_switch_cmd_sender_{}; rm_common::JointPositionBinaryCommandSender* scope_cmd_sender_{}; rm_common::JointPositionBinaryCommandSender* image_transmission_cmd_sender_{}; + rm_common::ChassisActiveSuspensionCommandSender* chassis_active_sus_cmd_sender_{}; + rm_common::BallisticSolverRequestCommandSender* ballistic_solver_request_cmd_sender_{}; + rm_common::SwitchDetectionCaller* switch_detection_srv_{}; rm_common::SwitchDetectionCaller* switch_detection_left_srv_{}; rm_common::SwitchDetectionCaller* switch_armor_target_srv_{}; + rm_common::ServiceCallerBase* relocate_srv_{}; + rm_common::ColorChangeServiceCaller* color_change_srv_{}; + rm_common::TrackerResetServiceCaller* tracker_reset_srv_{}; + rm_common::CalibrationQueue* chassis_calibration_; rm_common::CalibrationQueue* shooter_calibration_; rm_common::CalibrationQueue* gimbal_calibration_; - geometry_msgs::PointStamped point_out_; - uint8_t last_shoot_freq_{}; + ros::Subscriber wheel_online_sub_; + ros::Time last_wheels_power_time_; + std::vector chassis_motor_; + std::vector wheels_online_state_; - bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false, - up_change_position_ = false, low_change_position_ = false, need_change_position_ = false, deployed_ = false; - double yaw_current_{}; - double traj_yaw_, traj_pitch_; - double scale_; + std_msgs::Float32MultiArray ballistic_solution_; + ros::Time last_ballistic_solution_request_time_; + + bool prepare_shoot_{ false }, is_balance_{ false }, use_scope_{ false }, deployed_{ false }, + is_follow_yaw_reverse_{ false }, all_wheel_offline_{ false }, protect_state_{ false }; + double ballistic_yaw_{}, ballistic_pitch_{}; + double ballistic_yaw_step_{}, ballistic_pitch_step_{}; + double deploy_pitch_{}, deploy_yaw_; + uint8_t last_shoot_freq_{}; + double scale_{}; }; } // namespace rm_manual diff --git a/include/rm_manual/manual_base.h b/include/rm_manual/manual_base.h index f48df33d..89d34470 100644 --- a/include/rm_manual/manual_base.h +++ b/include/rm_manual/manual_base.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -98,6 +99,12 @@ class ManualBase virtual void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data) { } + virtual void ballisticSolutionCallback(const std_msgs::Float32MultiArray::ConstPtr& data) + { + } + virtual void protectStateCallback(const std_msgs::Bool::ConstPtr& data) + { + } // EtherCAT virtual void ecatReconnected() @@ -157,7 +164,8 @@ class ManualBase ros::Subscriber ecat_bus_state_sub_, 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_; + game_robot_status_sub_, suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_, ballistic_solution_sub_, + protect_state_sub_; sensor_msgs::JointState joint_state_; rm_msgs::TrackData track_data_; @@ -171,11 +179,10 @@ class ManualBase ros::NodeHandle nh_; ros::Time referee_last_get_stamp_; - bool remote_is_open_{ false }, referee_is_online_ = false; - bool ecat_bus_is_online_{ true }; + bool remote_is_open_{ false }, referee_is_online_{ false }, ecat_bus_is_online_{ true }; int state_ = PASSIVE; - int robot_id_, chassis_power_; - int chassis_output_on_ = 0, gimbal_output_on_ = 0, shooter_output_on_ = 0; + int robot_id_{}, chassis_power_{}; + int chassis_output_on_{}, gimbal_output_on_{}, shooter_output_on_{}; InputEvent robot_hp_event_, right_switch_down_event_, right_switch_mid_event_, right_switch_up_event_, left_switch_down_event_, left_switch_mid_event_, left_switch_up_event_, ecat_reconnected_event_; diff --git a/src/balance_manual.cpp b/src/balance_manual.cpp index 68f8e25d..9adce256 100644 --- a/src/balance_manual.cpp +++ b/src/balance_manual.cpp @@ -23,20 +23,7 @@ void BalanceManual::sendCommand(const ros::Time& time) chassis_cmd_sender_->getMsg()->follow_source_frame = reverse_frame_; else chassis_cmd_sender_->getMsg()->follow_source_frame = "yaw"; - - if (supply_) - { - cover_close_ = false; - cover_command_sender_->on(); - } - else - { - cover_close_ = true; - cover_command_sender_->off(); - } - - ChassisGimbalShooterManual::sendCommand(time); - cover_command_sender_->sendCommand(time); + rm_manual::ChassisGimbalShooterCoverManual::sendCommand(time); } void BalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index feee435e..061f25d5 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -249,11 +249,6 @@ void ChassisGimbalManual::setChassisMode(int mode) is_gyro_ = true; vel_cmd_sender_->setAngularZVel(0.0); break; - case rm_msgs::ChassisCmd::FALLEN: - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN); - is_gyro_ = false; - vel_cmd_sender_->setAngularZVel(0.0); - break; } } diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index cd5209dd..07544b7b 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -9,11 +9,7 @@ namespace rm_manual ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalShooterManual(nh, nh_referee) { - wheel_online_sub_ = nh.subscribe( - "/rm_ecat_hw/rm_readings", 10, &ChassisGimbalShooterCoverManual::wheelsOnlineCallback, this); - ros::NodeHandle cover_nh(nh, "cover"); nh.param("supply_frame", supply_frame_, std::string("supply_frame")); - cover_command_sender_ = new rm_common::JointPositionBinaryCommandSender(cover_nh); ros::NodeHandle wireless_nh(nh, "wireless"); nh.param("wireless_frame", wireless_frame_, std::string("wireless_frame")); ros::NodeHandle buff_switch_nh(nh, "buff_switch"); @@ -41,16 +37,6 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle boost::bind(&ChassisGimbalShooterCoverManual::eRelease, this)); q_event_.setRising(boost::bind(&ChassisGimbalShooterCoverManual::qPress, this)); z_event_.setRising(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this)); - - XmlRpc::XmlRpcValue xml; - if (!nh.getParam("chassis_motor", xml)) - ROS_ERROR("chassis_motor no defined (namespace: %s)", nh.getNamespace().c_str()); - else - { - for (int i = 0; i < xml.size(); i++) - chassis_motor_.push_back(xml[i]); - wheels_online_state_.resize(chassis_motor_.size(), true); - } } void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) @@ -101,28 +87,10 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) } } -void ChassisGimbalShooterCoverManual::checkWheelsOnline() -{ - bool all_wheels_online = true; - if (ros::Time::now() - last_check_wheels_time_ < ros::Duration(3.0)) - { - for (auto wheel_status : wheels_online_state_) - { - if (!wheel_status) - all_wheels_online = false; - } - } - if (!all_wheels_online) - wheels_offline_ = true; - else if (wheels_offline_) - wheels_offline_ = false; -} - void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalShooterManual::updatePc(dbus_data); - gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, - cover_command_sender_->getState() ? 0.0 : -dbus_data->m_y * gimbal_scale_); + gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, -dbus_data->m_y * gimbal_scale_); if (is_gyro_) { if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) @@ -142,13 +110,11 @@ void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr void ChassisGimbalShooterCoverManual::checkReferee() { - manual_to_referee_pub_data_.cover_state = cover_command_sender_->getState(); if (switch_detection_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) manual_to_referee_pub_data_.det_target = switch_buff_type_srv_->getTarget(); else manual_to_referee_pub_data_.det_target = switch_detection_srv_->getTarget(); ChassisGimbalShooterManual::checkReferee(); - checkWheelsOnline(); } void ChassisGimbalShooterCoverManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) @@ -157,95 +123,22 @@ void ChassisGimbalShooterCoverManual::checkKeyboard(const rm_msgs::DbusData::Con ctrl_z_event_.update(dbus_data->key_ctrl & dbus_data->key_z); } -void ChassisGimbalShooterCoverManual::wheelsOnlineCallback( - const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data) -{ - updateWheelsState(data, chassis_motor_); -} - -void ChassisGimbalShooterCoverManual::gameRobotStatusCallback(const rm_msgs::GameRobotStatus::ConstPtr& data) -{ - ChassisGimbalShooterManual::gameRobotStatusCallback(data); - if (data->mains_power_chassis_output - last_power_chassis_output_ == 1) - last_check_wheels_time_ = ros::Time::now(); - last_power_chassis_output_ = data->mains_power_chassis_output; -} - void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) { if (supply_) { chassis_cmd_sender_->getMsg()->follow_source_frame = supply_frame_; chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); - cover_close_ = false; - try - { - double roll, pitch, yaw; - quatToRPY(tf_buffer_.lookupTransform("base_link", supply_frame_, ros::Time(0)).transform.rotation, roll, pitch, - yaw); - if (std::abs(yaw) < 0.05) - cover_command_sender_->on(); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } } - else + else if (need_wireless_) { - cover_command_sender_->off(); - if (!cover_close_) - { - try - { - double roll, pitch, yaw; - quatToRPY(tf_buffer_.lookupTransform("base_link", "cover", ros::Time(0)).transform.rotation, roll, pitch, yaw); - if (pitch - cover_command_sender_->getMsg()->data > 0.05) - { - chassis_cmd_sender_->getMsg()->follow_source_frame = supply_frame_; - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); - } - else - { - cover_close_ = true; - chassis_cmd_sender_->getMsg()->follow_source_frame = "yaw"; - } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } - } - if (need_wireless_) - { - chassis_cmd_sender_->getMsg()->follow_source_frame = wireless_frame_; - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); - } - else - chassis_cmd_sender_->getMsg()->follow_source_frame = "yaw"; + chassis_cmd_sender_->getMsg()->follow_source_frame = wireless_frame_; + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); } - ChassisGimbalShooterManual::sendCommand(time); - cover_command_sender_->sendCommand(time); -} - -void ChassisGimbalShooterCoverManual::updateWheelsState(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data, - const std::vector& chassis_motor) -{ - std::unordered_map wheel_index_map; - for (size_t i = 0; i < chassis_motor.size(); ++i) - wheel_index_map[chassis_motor[i]] = i; + else + chassis_cmd_sender_->getMsg()->follow_source_frame = "yaw"; - for (const auto& reading : data->readings) - { - for (size_t i = 0; i < reading.names.size(); ++i) - { - const auto& name = reading.names[i]; - const auto it = wheel_index_map.find(name); - if (it == wheel_index_map.end()) - continue; - wheels_online_state_[it->second] = reading.isOnline[i]; - } - } + ChassisGimbalShooterManual::sendCommand(time); } void ChassisGimbalShooterCoverManual::rightSwitchDownRise() @@ -265,6 +158,19 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise() ChassisGimbalShooterManual::rightSwitchUpRise(); supply_ = false; } +void ChassisGimbalShooterCoverManual::mouseRightPress() +{ + ChassisGimbalShooterManual::mouseRightPress(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && + gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::TRACK) + { + if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP) + { + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + prepare_shoot_ = true; + } + } +} void ChassisGimbalShooterCoverManual::ePress() { @@ -384,21 +290,22 @@ void ChassisGimbalShooterCoverManual::dRelease() void ChassisGimbalShooterCoverManual::ctrlZPress() { - if (!cover_command_sender_->getState()) + if (!supply_) chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); else chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - supply_ = !cover_command_sender_->getState(); + supply_ = !supply_; if (supply_) - { changeSpeedMode(LOW); - } else - { changeSpeedMode(NORMAL); - } } +void ChassisGimbalShooterCoverManual::ctrlZRelease() +{ + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); +}; + void ChassisGimbalShooterCoverManual::ctrlXPress() { if (!need_wireless_) @@ -424,7 +331,7 @@ void ChassisGimbalShooterCoverManual::ctrlRPressing() { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); double traj_yaw = M_PI * count_ / 1000; - double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 1100) / 1100) + 0.15; + double traj_pitch = 0.15; count_++; gimbal_cmd_sender_->setGimbalTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); @@ -432,6 +339,7 @@ void ChassisGimbalShooterCoverManual::ctrlRPressing() else { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); + gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); shooter_cmd_sender_->checkError(ros::Time::now()); } diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 716d2385..4d3ac7f5 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -9,6 +9,12 @@ namespace rm_manual ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalManual(nh, nh_referee) { + ballistic_pitch_step_ = getParam(nh, "ballistic_pitch_step", 0.01); + ballistic_yaw_step_ = getParam(nh, "ballistic_yaw_step", 0.01); + deploy_pitch_ = getParam(nh, "deploy_pitch", 0.0); + deploy_yaw_ = getParam(nh, "deploy_yaw", 0.0); + wheel_online_sub_ = nh.subscribe( + "/rm_ecat_hw/rm_readings", 10, &ChassisGimbalShooterManual::wheelsOnlineCallback, this); ros::NodeHandle shooter_nh(nh, "shooter"); shooter_cmd_sender_ = new rm_common::ShooterCommandSender(shooter_nh); if (nh.hasParam("camera")) @@ -27,20 +33,48 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: image_transmission_cmd_sender_ = new rm_common::JointPositionBinaryCommandSender(image_transmission_nh); scale_ = getParam(image_transmission_nh, "position_scale", 1); } + if (nh.hasParam("ballistic_solver_request")) + { + ros::NodeHandle ballistic_solver_request_nh(nh, "ballistic_solver_request"); + ballistic_solver_request_cmd_sender_ = + new rm_common::BallisticSolverRequestCommandSender(ballistic_solver_request_nh); + } + if (nh.hasParam("ActiveSuspension")) + { + ros::NodeHandle active_sus_nh(nh, "ActiveSuspension"); + chassis_active_sus_cmd_sender_ = new rm_common::ChassisActiveSuspensionCommandSender(active_sus_nh); + } ros::NodeHandle detection_switch_nh(nh, "detection_switch"); - ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); switch_detection_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_nh); + ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); switch_detection_left_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_left_nh); ros::NodeHandle armor_target_switch_nh(nh, "armor_target_switch"); switch_armor_target_srv_ = new rm_common::SwitchDetectionCaller(armor_target_switch_nh); - XmlRpc::XmlRpcValue rpc_value; - nh.getParam("shooter_calibration", rpc_value); - shooter_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); - nh.getParam("gimbal_calibration", rpc_value); - gimbal_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); - nh.getParam("chassis_calibration", rpc_value); - chassis_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); + ros::NodeHandle relocate_nh(nh, "relocate"); + relocate_srv_ = new rm_common::ServiceCallerBase(relocate_nh); + ros::NodeHandle tracker_reset_nh(nh, "reset_tracker"); + tracker_reset_srv_ = new rm_common::TrackerResetServiceCaller(tracker_reset_nh); + ros::NodeHandle color_change_nh(nh, "change_color"); + color_change_srv_ = new rm_common::ColorChangeServiceCaller(color_change_nh); + + XmlRpc::XmlRpcValue shooter_calibration_rpc, gimbal_calibration_rpc, chassis_calibration_rpc, rpc_value; + nh.getParam("shooter_calibration", shooter_calibration_rpc); + shooter_calibration_ = new rm_common::CalibrationQueue(shooter_calibration_rpc, nh, controller_manager_); + nh.getParam("gimbal_calibration", gimbal_calibration_rpc); + gimbal_calibration_ = new rm_common::CalibrationQueue(gimbal_calibration_rpc, nh, controller_manager_); + nh.getParam("chassis_calibration", chassis_calibration_rpc); + chassis_calibration_ = new rm_common::CalibrationQueue(chassis_calibration_rpc, nh, controller_manager_); + + if (!nh.getParam("chassis_motor", rpc_value)) + ROS_WARN("chassis_motor no defined (namespace: %s)", nh.getNamespace().c_str()); + else + { + for (int i = 0; i < rpc_value.size(); i++) + chassis_motor_.push_back(rpc_value[i]); + wheels_online_state_.resize(chassis_motor_.size(), true); + } + shooter_power_on_event_.setRising(boost::bind(&ChassisGimbalShooterManual::shooterOutputOn, this)); self_inspection_event_.setRising(boost::bind(&ChassisGimbalShooterManual::selfInspectionStart, this)); game_start_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gameStart, this)); @@ -58,20 +92,22 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: g_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gPress, this)); v_event_.setRising(boost::bind(&ChassisGimbalShooterManual::vPress, this)); z_event_.setRising(boost::bind(&ChassisGimbalShooterManual::zPress, this)); + q_event_.setRising(boost::bind(&ChassisGimbalShooterManual::qPress, this)); ctrl_f_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlFPress, this)); ctrl_v_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlVPress, this)); ctrl_b_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlBPress, this)); ctrl_q_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlQPress, this)); ctrl_z_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlZPress, this)); + ctrl_c_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlCPress, this)); ctrl_x_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlXPress, this)); - ctrl_r_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::ctrlRPress, this), - boost::bind(&ChassisGimbalShooterManual::ctrlRRelease, this)); + ctrl_r_event_.setRising(boost::bind(&ChassisGimbalShooterManual::ctrlRPress, this)); shift_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::shiftPress, this), boost::bind(&ChassisGimbalShooterManual::shiftRelease, this)); mouse_left_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::mouseLeftPress, this)); mouse_left_event_.setFalling(boost::bind(&ChassisGimbalShooterManual::mouseLeftRelease, this)); mouse_right_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::mouseRightPress, this)); mouse_right_event_.setFalling(boost::bind(&ChassisGimbalShooterManual::mouseRightRelease, this)); + mouse_right_event_.setRising(boost::bind(&ChassisGimbalShooterManual::mouseRightRising, this)); } void ChassisGimbalShooterManual::run() @@ -87,9 +123,6 @@ void ChassisGimbalShooterManual::ecatReconnected() ChassisGimbalManual::ecatReconnected(); shooter_calibration_->reset(); gimbal_calibration_->reset(); - up_change_position_ = false; - low_change_position_ = false; - need_change_position_ = false; } void ChassisGimbalShooterManual::checkReferee() @@ -102,9 +135,39 @@ void ChassisGimbalShooterManual::checkReferee() manual_to_referee_pub_data_.det_color = switch_detection_srv_->getColor(); manual_to_referee_pub_data_.det_exposure = switch_detection_srv_->getExposureLevel(); manual_to_referee_pub_data_.stamp = ros::Time::now(); + checkWheelsOnline(); ChassisGimbalManual::checkReferee(); } +void ChassisGimbalShooterManual::checkWheelsOnline() +{ + bool all_wheels_online = true, exist_wheel_online = false; + for (auto wheel_status : wheels_online_state_) + { + if (wheel_status) + exist_wheel_online = true; + } + if (!exist_wheel_online) + all_wheel_offline_ = true; + if (all_wheel_offline_ && exist_wheel_online) + { + last_wheels_power_time_ = ros::Time::now(); + all_wheel_offline_ = false; + } + if (ros::Time::now() - last_wheels_power_time_ < ros::Duration(3.0)) + { + for (auto wheel_status : wheels_online_state_) + { + if (!wheel_status) + all_wheels_online = false; + } + } + if (!all_wheels_online) + wheels_offline_ = true; + else if (wheels_offline_) + wheels_offline_ = false; +} + void ChassisGimbalShooterManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalManual::checkKeyboard(dbus_data); @@ -123,9 +186,9 @@ void ChassisGimbalShooterManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr ctrl_q_event_.update(dbus_data->key_ctrl & dbus_data->key_q); ctrl_r_event_.update(dbus_data->key_ctrl & dbus_data->key_r); ctrl_z_event_.update(dbus_data->key_ctrl & dbus_data->key_z); + ctrl_c_event_.update(dbus_data->key_ctrl & dbus_data->key_c); ctrl_x_event_.update(dbus_data->key_ctrl & dbus_data->key_x); shift_event_.update(dbus_data->key_shift & !dbus_data->key_ctrl); - ctrl_shift_b_event_.update(dbus_data->key_ctrl & dbus_data->key_shift & dbus_data->key_b); mouse_left_event_.update(dbus_data->p_l & !dbus_data->key_ctrl); mouse_right_event_.update(dbus_data->p_r & !dbus_data->key_ctrl); } @@ -188,12 +251,26 @@ void ChassisGimbalShooterManual::shootDataCallback(const rm_msgs::ShootData::Con shooter_cmd_sender_->updateShootData(*data); } +void ChassisGimbalShooterManual::ballisticSolutionCallback(const std_msgs::Float32MultiArray::ConstPtr& data) +{ + ballistic_solution_ = *data; +} + +void ChassisGimbalShooterManual::protectStateCallback(const std_msgs::Bool::ConstPtr& data) +{ + protect_state_ = data->data; +} + void ChassisGimbalShooterManual::sendCommand(const ros::Time& time) { ChassisGimbalManual::sendCommand(time); shooter_cmd_sender_->sendCommand(time); + if (chassis_active_sus_cmd_sender_) + chassis_active_sus_cmd_sender_->sendCommand(time); if (camera_switch_cmd_sender_) camera_switch_cmd_sender_->sendCommand(time); + if (ballistic_solver_request_cmd_sender_) + ballistic_solver_request_cmd_sender_->sendCommand(time); if (scope_cmd_sender_) { if (!use_scope_) @@ -204,24 +281,44 @@ void ChassisGimbalShooterManual::sendCommand(const ros::Time& time) } if (image_transmission_cmd_sender_) { - if (!need_change_position_) - { - if (!adjust_image_transmission_) - image_transmission_cmd_sender_->off(); - else - image_transmission_cmd_sender_->on(); - } - if (up_change_position_) - { - image_transmission_cmd_sender_->changePosition(scale_); - up_change_position_ = false; - } - else if (low_change_position_) + image_transmission_cmd_sender_->sendCommand(time); + } + // Avoid deploy state in non-deploy mode + if (deployed_ && (state_ != PC || chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::RAW || + gimbal_cmd_sender_->getMsg()->mode != rm_msgs::GimbalCmd::TRAJ)) + { + shooter_cmd_sender_->setDeployState(false); + shooter_cmd_sender_->setDeployAllowFireFlag(false); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd ::RATE); + chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); + if (image_transmission_cmd_sender_) + image_transmission_cmd_sender_->off(); + deployed_ = false; + } +} + +void ChassisGimbalShooterManual::wheelsOnlineCallback(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data) +{ + updateWheelsState(data, chassis_motor_); +} + +void ChassisGimbalShooterManual::updateWheelsState(const rm_ecat_msgs::RmEcatStandardSlaveReadings::ConstPtr& data, + const std::vector& chassis_motor) +{ + std::unordered_map wheel_index_map; + for (size_t i = 0; i < chassis_motor.size(); ++i) + wheel_index_map[chassis_motor[i]] = i; + + for (const auto& reading : data->readings) + { + for (size_t i = 0; i < reading.names.size(); ++i) { - image_transmission_cmd_sender_->changePosition(-scale_); - low_change_position_ = false; + const auto& name = reading.names[i]; + const auto it = wheel_index_map.find(name); + if (it == wheel_index_map.end()) + continue; + wheels_online_state_[it->second] = reading.isOnline[i]; } - image_transmission_cmd_sender_->sendCommand(time); } } @@ -233,11 +330,6 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() gimbal_calibration_->stop(); chassis_calibration_->stop(); use_scope_ = false; - gimbal_cmd_sender_->setEject(false); - adjust_image_transmission_ = false; - up_change_position_ = false; - low_change_position_ = false; - need_change_position_ = false; deployed_ = false; } @@ -260,11 +352,6 @@ void ChassisGimbalShooterManual::robotDie() ManualBase::robotDie(); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); use_scope_ = false; - adjust_image_transmission_ = false; - up_change_position_ = false; - low_change_position_ = false; - need_change_position_ = false; - deployed_ = false; } void ChassisGimbalShooterManual::chassisOutputOn() @@ -324,17 +411,27 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu } if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::TRAJ && deployed_) { - traj_yaw_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_yaw * ros::Duration(0.01).toSec(); - traj_pitch_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_pitch * ros::Duration(0.01).toSec(); - gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_); + // Avoid mistaken fire. + if (!dbus_data->key_f) + { + if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); + shooter_cmd_sender_->setDeployAllowFireFlag(false); + } + else + { + shooter_cmd_sender_->setDeployAllowFireFlag(true); + } + if (dbus_data->key_ctrl) + { + ballistic_yaw_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_yaw * ros::Duration(0.01).toSec(); + ballistic_pitch_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_pitch * ros::Duration(0.01).toSec(); + } + gimbal_cmd_sender_->setGimbalTraj(ballistic_yaw_, ballistic_pitch_); } - if (deployed_ && std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + - std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) > 0.0) + if (protect_state_ && chassis_active_sus_cmd_sender_) { - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - deployed_ = false; - shooter_cmd_sender_->setDeployState(false); + chassis_active_sus_cmd_sender_->setMode(rm_msgs::ChassisActiveSusCmd::UP); } } @@ -374,7 +471,7 @@ void ChassisGimbalShooterManual::leftSwitchMidRise() void ChassisGimbalShooterManual::leftSwitchMidOn(ros::Duration duration) { - if (track_data_.id == 0) + if (track_data_.tracking == 0) gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); @@ -394,7 +491,7 @@ void ChassisGimbalShooterManual::leftSwitchUpFall() void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration) { - if (track_data_.id == 0) + if (track_data_.tracking == 0) gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); else gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK); @@ -421,37 +518,42 @@ void ChassisGimbalShooterManual::mouseLeftPress() shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); prepare_shoot_ = false; } - if (prepare_shoot_) + if (mouse_right_event_.getState()) { - if (!mouse_right_event_.getState() || (mouse_right_event_.getState() && track_data_.id != 0)) + if (track_data_.tracking == 1) { - 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()); + } } - else + else if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP) shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } + else if (prepare_shoot_) + { + shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH); + } } void ChassisGimbalShooterManual::mouseRightPress() { - if (track_data_.id == 0) + if (deployed_) + return; + if (track_data_.tracking == 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) - { - 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()); - } - } } +void ChassisGimbalShooterManual::mouseRightRising() +{ + tracker_reset_srv_->reset(); +} void ChassisGimbalShooterManual::ePress() { switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE); @@ -468,6 +570,8 @@ void ChassisGimbalShooterManual::eRelease() void ChassisGimbalShooterManual::cPress() { + if (deployed_) + return; if (is_gyro_) { setChassisMode(rm_msgs::ChassisCmd::FOLLOW); @@ -482,12 +586,15 @@ void ChassisGimbalShooterManual::cPress() void ChassisGimbalShooterManual::bPress() { - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); + last_ballistic_solution_request_time_ = ros::Time::now(); + if (ballistic_solver_request_cmd_sender_) + ballistic_solver_request_cmd_sender_->setBallisticSolverRequest(true); } void ChassisGimbalShooterManual::bRelease() { - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + if (ballistic_solver_request_cmd_sender_) + ballistic_solver_request_cmd_sender_->setBallisticSolverRequest(false); } void ChassisGimbalShooterManual::rPress() @@ -500,7 +607,6 @@ void ChassisGimbalShooterManual::rPress() else { gimbal_cmd_sender_->setEject(false); - adjust_image_transmission_ = false; } } } @@ -512,102 +618,170 @@ void ChassisGimbalShooterManual::gPress() void ChassisGimbalShooterManual::wPress() { - ChassisGimbalManual::wPress(); - if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && - (gimbal_cmd_sender_->getEject() && !use_scope_)) + if (deployed_) + ballistic_pitch_ -= ballistic_pitch_step_; + else { - gimbal_cmd_sender_->setEject(false); - manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + ChassisGimbalManual::wPress(); + if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && + (gimbal_cmd_sender_->getEject() && !use_scope_)) + { + gimbal_cmd_sender_->setEject(false); + manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + } } } void ChassisGimbalShooterManual::aPress() { - ChassisGimbalManual::aPress(); - if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && - (gimbal_cmd_sender_->getEject() && !use_scope_)) + if (deployed_) + ballistic_yaw_ += ballistic_yaw_step_; + else { - gimbal_cmd_sender_->setEject(false); - manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + ChassisGimbalManual::aPress(); + if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && + (gimbal_cmd_sender_->getEject() && !use_scope_)) + { + gimbal_cmd_sender_->setEject(false); + manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + } } } void ChassisGimbalShooterManual::sPress() { - ChassisGimbalManual::sPress(); - if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && - (gimbal_cmd_sender_->getEject() && !use_scope_)) + if (deployed_) + ballistic_pitch_ += ballistic_pitch_step_; + else { - gimbal_cmd_sender_->setEject(false); - manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + ChassisGimbalManual::sPress(); + if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && + (gimbal_cmd_sender_->getEject() && !use_scope_)) + { + gimbal_cmd_sender_->setEject(false); + manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + } } } void ChassisGimbalShooterManual::dPress() { - ChassisGimbalManual::dPress(); - if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && - (gimbal_cmd_sender_->getEject() && !use_scope_)) + if (deployed_) + ballistic_yaw_ -= ballistic_yaw_step_; + else { - gimbal_cmd_sender_->setEject(false); - manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + ChassisGimbalManual::dPress(); + if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) && + (gimbal_cmd_sender_->getEject() && !use_scope_)) + { + gimbal_cmd_sender_->setEject(false); + manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject(); + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); + } } } void ChassisGimbalShooterManual::wRelease() { - ChassisGimbalManual::wRelease(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::wRelease(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + } + } } void ChassisGimbalShooterManual::aRelease() { - ChassisGimbalManual::aRelease(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::aRelease(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + } + } } void ChassisGimbalShooterManual::sRelease() { - ChassisGimbalManual::sRelease(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::sRelease(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + } + } } void ChassisGimbalShooterManual::dRelease() { - ChassisGimbalManual::dRelease(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::dRelease(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0); + } + } } void ChassisGimbalShooterManual::wPressing() { - ChassisGimbalManual::wPressing(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::wPressing(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + } + } } void ChassisGimbalShooterManual::aPressing() { - ChassisGimbalManual::aPressing(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::aPressing(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + } + } } void ChassisGimbalShooterManual::sPressing() { - ChassisGimbalManual::sPressing(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::sPressing(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + } + } } void ChassisGimbalShooterManual::dPressing() { - ChassisGimbalManual::dPressing(); - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + if (!deployed_) + { + if (!use_scope_) + { + ChassisGimbalManual::dPressing(); + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0); + } + } } void ChassisGimbalShooterManual::xPress() @@ -628,6 +802,8 @@ void ChassisGimbalShooterManual::xPress() void ChassisGimbalShooterManual::xRelease() { + if (deployed_) + return; gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); } @@ -636,25 +812,49 @@ void ChassisGimbalShooterManual::vPress() shooter_cmd_sender_->raiseSpeed(); } +void ChassisGimbalShooterManual::qPress() +{ + if (!chassis_active_sus_cmd_sender_) + return; + if (chassis_active_sus_cmd_sender_->getMsg()->mode == rm_msgs::ChassisActiveSusCmd::DOWN && !deployed_) + { + chassis_active_sus_cmd_sender_->setMode(rm_msgs::ChassisActiveSusCmd::MID); + } + else if (chassis_active_sus_cmd_sender_->getMsg()->mode == rm_msgs::ChassisActiveSusCmd::MID && !deployed_) + { + chassis_active_sus_cmd_sender_->setMode(rm_msgs::ChassisActiveSusCmd::UP); + } + else if (chassis_active_sus_cmd_sender_->getMsg()->mode == rm_msgs::ChassisActiveSusCmd::UP && !deployed_) + { + chassis_active_sus_cmd_sender_->setMode(rm_msgs::ChassisActiveSusCmd::DOWN); + } + chassis_active_sus_cmd_sender_->setLegSwitchTime(ros::Time::now()); + chassis_cmd_sender_->power_limit_->setUpstairsPower( + (chassis_active_sus_cmd_sender_->getLegMode() == rm_msgs::ChassisActiveSusCmd::MID || + chassis_active_sus_cmd_sender_->getLegMode() == rm_msgs::ChassisActiveSusCmd::UP)); +} + void ChassisGimbalShooterManual::zPress() { if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::RAW && !deployed_) { - double roll{}, pitch{}, yaw{}; - try + if ((ros::Time::now() - last_ballistic_solution_request_time_).toSec() < 3.0) { - quatToRPY(tf_buffer_.lookupTransform("base_link", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw); + ballistic_yaw_ = ballistic_solution_.data[0]; + ballistic_pitch_ = ballistic_solution_.data[1]; } - catch (tf2::TransformException& ex) + else { - ROS_WARN("%s", ex.what()); + ballistic_yaw_ = deploy_yaw_; + ballistic_pitch_ = deploy_pitch_; } gimbal_cmd_sender_->setGimbalTrajFrameId("base_link"); gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - traj_yaw_ = yaw, traj_pitch_ = -0.585; - gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_); + gimbal_cmd_sender_->setGimbalTraj(ballistic_yaw_, ballistic_pitch_); setChassisMode(rm_msgs::ChassisCmd::DEPLOY); shooter_cmd_sender_->setDeployState(true); + if (image_transmission_cmd_sender_) + image_transmission_cmd_sender_->on(); deployed_ = true; } else @@ -662,13 +862,15 @@ void ChassisGimbalShooterManual::zPress() setChassisMode(rm_msgs::ChassisCmd::FOLLOW); gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); shooter_cmd_sender_->setDeployState(false); + if (image_transmission_cmd_sender_) + image_transmission_cmd_sender_->off(); deployed_ = false; } } void ChassisGimbalShooterManual::shiftPress() { - if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_) + if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_ && !deployed_) { setChassisMode(rm_msgs::ChassisCmd::FOLLOW); gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); @@ -696,31 +898,12 @@ void ChassisGimbalShooterManual::ctrlVPress() void ChassisGimbalShooterManual::ctrlRPress() { - if (image_transmission_cmd_sender_) - adjust_image_transmission_ = !image_transmission_cmd_sender_->getState(); - if (adjust_image_transmission_) - { - double roll{}, pitch{}, yaw{}; - try - { - quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - gimbal_cmd_sender_->setGimbalTraj(yaw, -0.45); - } -} - -void ChassisGimbalShooterManual::ctrlRRelease() -{ - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + relocate_srv_->callService(); } void ChassisGimbalShooterManual::ctrlBPress() { + color_change_srv_->changeColor(); switch_detection_srv_->switchEnemyColor(); switch_detection_srv_->callService(); } @@ -729,28 +912,34 @@ void ChassisGimbalShooterManual::ctrlQPress() { shooter_calibration_->reset(); gimbal_calibration_->reset(); - adjust_image_transmission_ = false; - up_change_position_ = false; - low_change_position_ = false; - need_change_position_ = false; } -void ChassisGimbalShooterManual::ctrlZPress() +void ChassisGimbalShooterManual::ctrlCPress() { - up_change_position_ = true; - need_change_position_ = true; + std::string target_frame; + if (!is_follow_yaw_reverse_) + { + target_frame = "yaw_reverse"; + is_follow_yaw_reverse_ = true; + } + else + { + target_frame = "yaw"; + is_follow_yaw_reverse_ = false; + } + chassis_cmd_sender_->setFollowSourceFrame(target_frame); } -void ChassisGimbalShooterManual::ctrlXPress() +void ChassisGimbalShooterManual::ctrlZPress() { - low_change_position_ = true; - need_change_position_ = true; + if (image_transmission_cmd_sender_) + image_transmission_cmd_sender_->changePosition(scale_); } -void ChassisGimbalShooterManual::robotRevive() +void ChassisGimbalShooterManual::ctrlXPress() { - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - ManualBase::robotRevive(); + if (image_transmission_cmd_sender_) + image_transmission_cmd_sender_->changePosition(-scale_); } } // namespace rm_manual diff --git a/src/manual_base.cpp b/src/manual_base.cpp index 263b1842..751af798 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -17,13 +17,12 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) actuator_state_sub_ = nh.subscribe("/actuator_states", 10, &ManualBase::actuatorStateCallback, this); dbus_sub_ = nh.subscribe(dbus_topic_, 10, &ManualBase::dbusDataCallback, this); - track_sub_ = nh.subscribe("/track", 10, &ManualBase::trackCallback, this); + track_sub_ = nh.subscribe("/sp_vision/track", 10, &ManualBase::trackCallback, this); gimbal_des_error_sub_ = nh.subscribe("/controllers/gimbal_controller/error", 10, &ManualBase::gimbalDesErrorCallback, 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, - &ManualBase::shootBeforehandCmdCallback, this); + shoot_beforehand_cmd_sub_ = nh.subscribe( + "/controllers/gimbal_controller/shoot_beforehand_cmd", 10, &ManualBase::shootBeforehandCmdCallback, this); suggest_fire_sub_ = nh.subscribe("/forecast/suggest_fire", 10, &ManualBase::suggestFireCallback, this); @@ -38,6 +37,9 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) power_heat_data_sub_ = nh_referee.subscribe("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this); shoot_data_sub_ = nh_referee.subscribe("shoot_data", 10, &ManualBase::shootDataCallback, this); + ballistic_solution_sub_ = nh.subscribe( + "/controllers/gimbal_controller/ballistic_solution", 10, &ManualBase::ballisticSolutionCallback, this); + protect_state_sub_ = nh.subscribe("/protect_state", 10, &ManualBase::protectStateCallback, this); // pub manual_to_referee_pub_ = nh.advertise("/manual_to_referee", 1);