Skip to content

Update base pitch control on manual for zipped standard26 #155

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Bluefairy-ljy merged 1 commit into from
Oct 14, 2026
Merged

Update base pitch control on manual for zipped standard26 #155

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ find_package(catkin REQUIRED
rm_ecat_msgs
rm_msgs
rm_common
control_toolbox
tf2_geometry_msgs
serial
std_msgs
Expand Down Expand Up @@ -48,6 +49,7 @@ catkin_package(
rm_ecat_msgs
rm_msgs
rm_common
control_toolbox
tf2_geometry_msgs
std_msgs
actionlib
Expand Down
36 changes: 27 additions & 9 deletions include/rm_manual/chassis_gimbal_shooter_cover_manual.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,11 @@

#include "rm_manual/chassis_gimbal_shooter_manual.h"

#include <std_msgs/Float64.h>
#include <algorithm>
#include <rm_common/math_utilities.h>
#include <control_toolbox/pid.h>

namespace rm_manual
{
class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
Expand All @@ -26,17 +31,20 @@ 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;
void bPress() override;
void cPress() override;
void qPress() override;
void ctrlRPressing();
void ctrlRRelease();
void ctrlRRelease() override;
void wPress() override;
void wPressing() override;
void aPressing() override;
Expand All @@ -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<control_toolbox::Pid> zipped_pitch_rate_pid_{};
};
} // namespace rm_manual
182 changes: 157 additions & 25 deletions src/chassis_gimbal_shooter_cover_manual.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,27 @@ 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<std_msgs::Float64>(base_pitch_topic, 1);
zipped_pitch_rate_pid_ = std::make_shared<control_toolbox::Pid>();
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");
switch_buff_srv_ = new rm_common::SwitchDetectionCaller(buff_switch_nh);
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);
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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();
}

Expand All @@ -123,41 +156,98 @@ 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<size_t>(pitch_index) < joint_state_.position.size() &&
static_cast<size_t>(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);
}
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();
Expand Down Expand Up @@ -206,7 +296,6 @@ void ChassisGimbalShooterCoverManual::eRelease()

void ChassisGimbalShooterCoverManual::bPress()
{
chassis_cmd_sender_->power_limit_->setStartBurstTime(ros::Time::now());
}

void ChassisGimbalShooterCoverManual::cPress()
Expand Down Expand Up @@ -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()
{
Expand All @@ -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);
Expand All @@ -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