From 1668de734644d04e62346ac111a51a9ede8dcdc7 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Fri, 9 Oct 2026 13:58:06 +0000 Subject: [PATCH] Update zipped pitch control. --- CMakeLists.txt | 2 + .../chassis_gimbal_shooter_cover_manual.h | 36 +++- src/chassis_gimbal_shooter_cover_manual.cpp | 182 +++++++++++++++--- 3 files changed, 186 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a423ba2a..5ca17366 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED rm_ecat_msgs rm_msgs rm_common + control_toolbox tf2_geometry_msgs serial std_msgs @@ -48,6 +49,7 @@ catkin_package( rm_ecat_msgs rm_msgs rm_common + control_toolbox tf2_geometry_msgs std_msgs actionlib diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index a72150a1..d2844d4b 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -6,6 +6,11 @@ #include "rm_manual/chassis_gimbal_shooter_manual.h" +#include +#include +#include +#include + namespace rm_manual { class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual @@ -26,9 +31,12 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void checkReferee() override; void sendCommand(const ros::Time& time) override; + void remoteControlTurnOn() override; void rightSwitchDownRise() override; void rightSwitchMidRise() override; void rightSwitchUpRise() override; + void leftSwitchMidRise() override; + void leftSwitchDownRise() override; void mouseRightPress() override; void ePress() override; void eRelease() override; @@ -36,7 +44,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void cPress() override; void qPress() override; void ctrlRPressing(); - void ctrlRRelease(); + void ctrlRRelease() override; void wPress() override; void wPressing() override; void aPressing() override; @@ -46,34 +54,44 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void aRelease() override; void sRelease() override; void dRelease() override; + void rPress() override; void bRelease() override { } - void zPress() override - { - } + void zPress() override; + void zRelease(); void ctrlCPress() override { } + void shiftPress() override; + virtual void rRelease(); virtual void ctrlZPress(); virtual void ctrlZRelease(); virtual void ctrlXPress(); - ros::Subscriber wheel_online_sub_; + void getPitchErr(double& position); + void gimbalOutputOn() override; double low_speed_scale_{}, normal_speed_scale_{}; double exit_buff_mode_duration_{}; double gyro_speed_limit_{}; double sin_gyro_base_scale_{ 1. }, sin_gyro_amplitude_{ 0. }, sin_gyro_period_{ 1. }, sin_gyro_phase_{ 0. }; + + ros::Publisher base_pitch_pub_; + rm_common::SwitchDetectionCaller* switch_buff_srv_{}; rm_common::SwitchDetectionCaller* switch_buff_type_srv_{}; rm_common::SwitchDetectionCaller* switch_exposure_srv_{}; - InputEvent ctrl_z_event_, z_event_; - std::string supply_frame_; + + InputEvent ctrl_z_event_; + std::string wireless_frame_; ros::Time last_switch_time_; - bool supply_ = { false }; - bool need_wireless_ = { false }; + ros::Time gimbal_output_on_time_; + bool need_wireless_{ false }; int count_{}; + + bool zipped_{ false }; + std::shared_ptr zipped_pitch_rate_pid_{}; }; } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 07544b7b..ec51f229 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -9,7 +9,19 @@ namespace rm_manual ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalShooterManual(nh, nh_referee) { - nh.param("supply_frame", supply_frame_, std::string("supply_frame")); + if (nh.hasParam("base_pitch")) + { + ros::NodeHandle base_pitch_nh(nh, "base_pitch"); + std::string base_pitch_topic{}; + base_pitch_nh.param("topic", base_pitch_topic, std::string("/controllers/base_pitch_controller/command")); + base_pitch_pub_ = base_pitch_nh.advertise(base_pitch_topic, 1); + zipped_pitch_rate_pid_ = std::make_shared(); + if (base_pitch_nh.hasParam("zipped_pitch_rate_pid")) + { + zipped_pitch_rate_pid_->init(ros::NodeHandle(base_pitch_nh, "zipped_pitch_rate_pid")); + } + } + ros::NodeHandle wireless_nh(nh, "wireless"); nh.param("wireless_frame", wireless_frame_, std::string("wireless_frame")); ros::NodeHandle buff_switch_nh(nh, "buff_switch"); @@ -17,7 +29,7 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle ros::NodeHandle buff_type_switch_nh(nh, "buff_type_switch"); switch_buff_type_srv_ = new rm_common::SwitchDetectionCaller(buff_type_switch_nh); ros::NodeHandle exposure_switch_nh(nh, "exposure_switch"); - switch_exposure_srv_ = new rm_common::SwitchDetectionCaller(exposure_switch_nh); + switch_exposure_srv_ = new rm_common::SwitchDetectionCaller(exposure_switch_nh, "/hk_camera/exposure_status_switch"); ros::NodeHandle chassis_nh(nh, "chassis"); normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1); low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30); @@ -33,10 +45,19 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this)); ctrl_x_event_.setRising(boost::bind(&ChassisGimbalShooterCoverManual::ctrlXPress, this)); ctrl_r_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterCoverManual::ctrlRPressing, this)); + r_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::rPress, this), + boost::bind(&ChassisGimbalShooterCoverManual::rRelease, this)); e_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ePress, this), boost::bind(&ChassisGimbalShooterCoverManual::eRelease, this)); q_event_.setRising(boost::bind(&ChassisGimbalShooterCoverManual::qPress, this)); - z_event_.setRising(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this)); + z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this), + boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this)); +} + +void ChassisGimbalShooterCoverManual::remoteControlTurnOn() +{ + ChassisGimbalShooterManual::remoteControlTurnOn(); + zipped_ = true; } void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) @@ -90,7 +111,18 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalShooterManual::updatePc(dbus_data); - gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, -dbus_data->m_y * gimbal_scale_); + if (ros::Time::now().toSec() - 1.0 > gimbal_output_on_time_.toSec()) + { + gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, -dbus_data->m_y * gimbal_scale_); + } + else + { + double pitch_err = 0.0; + getPitchErr(pitch_err); + double cmd_rate = zipped_pitch_rate_pid_->computeCommand(pitch_err, ros::Duration(0.01)); + gimbal_cmd_sender_->getMsg()->rate_pitch = cmd_rate; + gimbal_cmd_sender_->getMsg()->rate_yaw = 0.0; + } if (is_gyro_) { if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) @@ -114,6 +146,7 @@ void ChassisGimbalShooterCoverManual::checkReferee() manual_to_referee_pub_data_.det_target = switch_buff_type_srv_->getTarget(); else manual_to_referee_pub_data_.det_target = switch_detection_srv_->getTarget(); + manual_to_referee_pub_data_.zip_state = zipped_; ChassisGimbalShooterManual::checkReferee(); } @@ -123,14 +156,40 @@ void ChassisGimbalShooterCoverManual::checkKeyboard(const rm_msgs::DbusData::Con ctrl_z_event_.update(dbus_data->key_ctrl & dbus_data->key_z); } -void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) +void ChassisGimbalShooterCoverManual::getPitchErr(double& err) { - if (supply_) + static int pitch_index = -1; + static int base_pitch_index = -1; + if (pitch_index < 0) { - chassis_cmd_sender_->getMsg()->follow_source_frame = supply_frame_; - chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); + auto it = std::find(joint_state_.name.begin(), joint_state_.name.end(), "pitch_joint"); + if (it == joint_state_.name.end()) + { + ROS_WARN_THROTTLE(1.0, "Pitch joint %s not found in joint_states.", "pitch_joint"); + return; + } + pitch_index = std::distance(joint_state_.name.begin(), it); } - else if (need_wireless_) + if (base_pitch_index < 0) + { + auto it = std::find(joint_state_.name.begin(), joint_state_.name.end(), "base_pitch_joint"); + if (it == joint_state_.name.end()) + { + ROS_WARN_THROTTLE(1.0, "Base pitch joint %s not found in joint_states.", "base_pitch_joint"); + return; + } + base_pitch_index = std::distance(joint_state_.name.begin(), it); + } + if (pitch_index >= 0 && base_pitch_index >= 0 && static_cast(pitch_index) < joint_state_.position.size() && + static_cast(base_pitch_index) < joint_state_.position.size()) + { + err = -joint_state_.position[base_pitch_index] - joint_state_.position[pitch_index]; + } +} + +void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) +{ + if (need_wireless_) { chassis_cmd_sender_->getMsg()->follow_source_frame = wireless_frame_; chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); @@ -138,26 +197,57 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time) else chassis_cmd_sender_->getMsg()->follow_source_frame = "yaw"; + if (base_pitch_pub_) + { + std_msgs::Float64 cmd; + if (zipped_) + { + cmd.data = 0.0; + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + double pitch_err = 0.0; + getPitchErr(pitch_err); + double cmd_rate = zipped_pitch_rate_pid_->computeCommand(pitch_err, ros::Duration(0.01)); + gimbal_cmd_sender_->getMsg()->rate_pitch = cmd_rate; + } + else + { + cmd.data = 0.5; + } + base_pitch_pub_.publish(cmd); + } ChassisGimbalShooterManual::sendCommand(time); } void ChassisGimbalShooterCoverManual::rightSwitchDownRise() { ChassisGimbalShooterManual::rightSwitchDownRise(); - supply_ = true; + zipped_ = true; } void ChassisGimbalShooterCoverManual::rightSwitchMidRise() { ChassisGimbalShooterManual::rightSwitchMidRise(); - supply_ = false; + zipped_ = true; } void ChassisGimbalShooterCoverManual::rightSwitchUpRise() { ChassisGimbalShooterManual::rightSwitchUpRise(); - supply_ = false; + zipped_ = false; +} + +void ChassisGimbalShooterCoverManual::leftSwitchMidRise() +{ + ChassisGimbalShooterManual::leftSwitchMidRise(); + zipped_ = false; +} + +void ChassisGimbalShooterCoverManual::leftSwitchDownRise() +{ + ChassisGimbalShooterManual::leftSwitchDownRise(); + zipped_ = true; } + void ChassisGimbalShooterCoverManual::mouseRightPress() { ChassisGimbalShooterManual::mouseRightPress(); @@ -206,7 +296,6 @@ void ChassisGimbalShooterCoverManual::eRelease() void ChassisGimbalShooterCoverManual::bPress() { - chassis_cmd_sender_->power_limit_->setStartBurstTime(ros::Time::now()); } void ChassisGimbalShooterCoverManual::cPress() @@ -288,23 +377,55 @@ void ChassisGimbalShooterCoverManual::dRelease() vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_); } +void ChassisGimbalShooterCoverManual::zPress() +{ + switch_buff_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF); + switch_detection_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF); + switch_buff_type_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF); + switch_exposure_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF); + switch_buff_srv_->callService(); + switch_detection_srv_->callService(); + switch_buff_type_srv_->callService(); + switch_exposure_srv_->callService(); + if (is_gyro_) + changeGyroSpeedMode(LOW); + last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency(); + shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL); +} + +void ChassisGimbalShooterCoverManual::zRelease() +{ + ChassisGimbalShooterManual::eRelease(); + switch_buff_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR); + switch_detection_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR); + switch_buff_type_srv_->setTargetType(switch_buff_srv_->getTarget()); + switch_exposure_srv_->setTargetType(rm_msgs::StatusChangeRequest::ARMOR); + switch_buff_srv_->callService(); + switch_detection_srv_->callService(); + switch_buff_type_srv_->callService(); + switch_exposure_srv_->callService(); + shooter_cmd_sender_->setShootFrequency(last_shoot_freq_); + if (is_gyro_) + changeGyroSpeedMode(NORMAL); +} + +void ChassisGimbalShooterCoverManual::rPress() +{ + zipped_ = true; +} + +void ChassisGimbalShooterCoverManual::rRelease() +{ + zipped_ = false; +} + void ChassisGimbalShooterCoverManual::ctrlZPress() { - if (!supply_) - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); - else - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - supply_ = !supply_; - if (supply_) - changeSpeedMode(LOW); - else - changeSpeedMode(NORMAL); } void ChassisGimbalShooterCoverManual::ctrlZRelease() { - gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); -}; +} void ChassisGimbalShooterCoverManual::ctrlXPress() { @@ -331,7 +452,7 @@ void ChassisGimbalShooterCoverManual::ctrlRPressing() { gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); double traj_yaw = M_PI * count_ / 1000; - double traj_pitch = 0.15; + double traj_pitch = 0.0; count_++; gimbal_cmd_sender_->setGimbalTraj(traj_yaw, traj_pitch); shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); @@ -352,4 +473,15 @@ void ChassisGimbalShooterCoverManual::ctrlRRelease() shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } +void ChassisGimbalShooterCoverManual::shiftPress() +{ + chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); +} + +void ChassisGimbalShooterCoverManual::gimbalOutputOn() +{ + gimbal_output_on_time_ = ros::Time::now(); + ChassisGimbalShooterManual::gimbalOutputOn(); +} + } // namespace rm_manual