Skip to content

Add RMU 2026 support. #291

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

Closed
Whathelp233 wants to merge 1 commit into from
+1,298 −1,495
Closed

Add RMU 2026 support. #291

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
26 changes: 19 additions & 7 deletions .github/workflows/format.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,24 @@ on:
jobs:
pre-commit:
name: pre-commit
runs-on: ubuntu-22.04
runs-on: ubuntu-20.04
steps:
- uses: actions/checkout@v3
- uses: actions/setup-python@v3
- name: Install clang-format
- uses: actions/checkout@v2
- uses: actions/setup-python@v2
- name: Install clang-format-10
run: |
sudo apt update
sudo apt install clang-format
- uses: pre-commit/action@v3.0.0
wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add -
sudo apt-get install clang-format-10
- name: Install catkin-lint
run: |
lsb_release -sc
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get -q update
sudo apt-get -q install python3-rosdep
sudo rm -rf /etc/ros/rosdep/sources.list.d/*
sudo rosdep init
rosdep update
sudo apt-get -q install catkin-lint
export ROS_DISTRO=noetic
- uses: pre-commit/action@v2.0.0
9 changes: 8 additions & 1 deletion .pre-commit-config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,14 @@ repos:
- id: clang-format
name: clang-format
description: Format files with ClangFormat.
entry: clang-format
entry: clang-format-10
language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: [ "-fallback-style=none", "-i" ]
- id: catkin_lint
name: catkin_lint
description: Check package.xml and cmake files
entry: catkin_lint .
language: system
always_run: true
pass_filenames: false
143 changes: 26 additions & 117 deletions rm_common/include/rm_common/decision/command_sender.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,21 @@

namespace rm_common
{
namespace detail
{
template <typename TMsg>
auto setTrajFrameIdIfSupported(TMsg& msg, const std::string& traj_frame_id, int)
-> decltype((void)(msg.traj_frame_id = traj_frame_id), void())
{
msg.traj_frame_id = traj_frame_id;
}

template <typename TMsg>
void setTrajFrameIdIfSupported(TMsg&, const std::string&, long)
{
}
} // namespace detail

template <class MsgType>
class CommandSenderBase
{
Expand Down Expand Up @@ -269,10 +284,6 @@ class ChassisCommandSender : public TimeStampCommandSenderBase<rm_msgs::ChassisC
{
msg_.follow_vel_des = follow_vel_des;
}
void setWirelessState(bool state)
{
msg_.wireless_state = state;
}
void sendChassisCommand(const ros::Time& time, bool is_gyro)
{
power_limit_->setLimitPower(msg_, is_gyro);
Expand All @@ -297,10 +308,6 @@ class GimbalCommandSender : public TimeStampCommandSenderBase<rm_msgs::GimbalCmd
ROS_ERROR("Max yaw velocity no defined (namespace: %s)", nh.getNamespace().c_str());
if (!nh.getParam("max_pitch_vel", max_pitch_vel_))
ROS_ERROR("Max pitch velocity no defined (namespace: %s)", nh.getNamespace().c_str());
if (!nh.getParam("time_constant_rc", time_constant_rc_))
ROS_ERROR("Time constant rc no defined (namespace: %s)", nh.getNamespace().c_str());
if (!nh.getParam("time_constant_pc", time_constant_pc_))
ROS_ERROR("Time constant pc no defined (namespace: %s)", nh.getNamespace().c_str());
if (!nh.getParam("track_timeout", track_timeout_))
ROS_ERROR("Track timeout no defined (namespace: %s)", nh.getNamespace().c_str());
if (!nh.getParam("eject_sensitivity", eject_sensitivity_))
Expand All @@ -313,14 +320,8 @@ class GimbalCommandSender : public TimeStampCommandSenderBase<rm_msgs::GimbalCmd
scale_yaw = scale_yaw > 0 ? 1 : -1;
if (std::abs(scale_pitch) > 1)
scale_pitch = scale_pitch > 0 ? 1 : -1;
double time_constant{};
if (use_rc_)
time_constant = time_constant_rc_;
else
time_constant = time_constant_pc_;
msg_.rate_yaw = msg_.rate_yaw + (scale_yaw * max_yaw_vel_ - msg_.rate_yaw) * (0.001 / (time_constant + 0.001));
msg_.rate_pitch =
msg_.rate_pitch + (scale_pitch * max_pitch_vel_ - msg_.rate_pitch) * (0.001 / (time_constant + 0.001));
msg_.rate_yaw = scale_yaw * max_yaw_vel_;
msg_.rate_pitch = scale_pitch * max_pitch_vel_;
if (eject_flag_)
{
msg_.rate_yaw *= eject_sensitivity_;
Expand All @@ -332,9 +333,9 @@ class GimbalCommandSender : public TimeStampCommandSenderBase<rm_msgs::GimbalCmd
msg_.traj_yaw = traj_yaw;
msg_.traj_pitch = traj_pitch;
}
void setGimbalTrajFrameId(const std::string& traj_frame_id)
void setTrajFrameId(const std::string& traj_frame_id)
{
msg_.traj_frame_id = traj_frame_id;
detail::setTrajFrameIdIfSupported(msg_, traj_frame_id, 0);
}
void setZero() override
{
Expand All @@ -349,10 +350,6 @@ class GimbalCommandSender : public TimeStampCommandSenderBase<rm_msgs::GimbalCmd
{
eject_flag_ = flag;
}
void setUseRc(bool flag)
{
use_rc_ = flag;
}
bool getEject() const
{
return eject_flag_;
Expand All @@ -364,8 +361,7 @@ class GimbalCommandSender : public TimeStampCommandSenderBase<rm_msgs::GimbalCmd

private:
double max_yaw_vel_{}, max_pitch_vel_{}, track_timeout_{}, eject_sensitivity_ = 1.;
double time_constant_rc_{}, time_constant_pc_{};
bool eject_flag_{}, use_rc_{};
bool eject_flag_{};
};

class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd>
Expand All @@ -385,11 +381,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
nh.getParam("wheel_speed_16", wheel_speed_16_);
nh.getParam("wheel_speed_18", wheel_speed_18_);
nh.getParam("wheel_speed_30", wheel_speed_30_);
nh.param("wheel_speed_offset_front", wheel_speed_offset_front_, 0.0);
nh.param("wheel_speed_offset_back", wheel_speed_offset_back_, 0.0);
nh.param("speed_oscillation", speed_oscillation_, 1.0);
nh.param("extra_wheel_speed_once", extra_wheel_speed_once_, 0.);
nh.param("deploy_wheel_speed", deploy_wheel_speed_, 410.0);
if (!nh.getParam("auto_wheel_speed", auto_wheel_speed_))
ROS_INFO("auto_wheel_speed no defined (namespace: %s), set to false.", nh.getNamespace().c_str());
if (!nh.getParam("target_acceleration_tolerance", target_acceleration_tolerance_))
Expand All @@ -399,13 +392,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
}
if (!nh.getParam("track_armor_error_tolerance", track_armor_error_tolerance_))
ROS_ERROR("track armor error tolerance no defined (namespace: %s)", nh.getNamespace().c_str());
nh.param("untrack_armor_error_tolerance", untrack_armor_error_tolerance_, track_armor_error_tolerance_);
nh.param("track_buff_error_tolerance", track_buff_error_tolerance_, track_armor_error_tolerance_);
if (!nh.getParam("max_track_target_vel", max_track_target_vel_))
{
max_track_target_vel_ = 9.0;
ROS_ERROR("max track target vel no defined (namespace: %s)", nh.getNamespace().c_str());
}
}
~ShooterCommandSender()
{
Expand Down Expand Up @@ -474,26 +461,16 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
return;
}
}
double gimbal_error_tolerance;
if (track_data_.id == 12)
gimbal_error_tolerance = track_buff_error_tolerance_;
else if (std::abs(track_data_.v_yaw) < max_track_target_vel_)
gimbal_error_tolerance = track_armor_error_tolerance_;
else
gimbal_error_tolerance = untrack_armor_error_tolerance_;
double gimbal_error_tolerance = track_data_.id == 12 ? track_buff_error_tolerance_ : track_armor_error_tolerance_;
if (((gimbal_des_error_.error > gimbal_error_tolerance && time - gimbal_des_error_.stamp < ros::Duration(0.1)) ||
(track_data_.accel > target_acceleration_tolerance_)) ||
(!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE))
if (msg_.mode == rm_msgs::ShootCmd::PUSH)
{
setMode(rm_msgs::ShootCmd::READY);
}
}
void sendCommand(const ros::Time& time) override
{
msg_.wheel_speed = getWheelSpeedDes();
msg_.wheels_speed_offset_front = getFrontWheelSpeedOffset();
msg_.wheels_speed_offset_back = getBackWheelSpeedOffset();
msg_.hz = heat_limit_->getShootFrequency();
TimeStampCommandSenderBase<rm_msgs::ShootCmd>::sendCommand(time);
}
Expand All @@ -505,26 +482,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
double getWheelSpeedDes()
{
setSpeedDesAndWheelSpeedDes();
if (hero_flag_)
{
if (deploy_flag_)
return deploy_wheel_speed_;
return wheel_speed_des_;
}
return wheel_speed_des_ + total_extra_wheel_speed_;
}
void setDeployState(bool flag)
{
deploy_flag_ = flag;
}
void setHeroState(bool flag)
{
hero_flag_ = flag;
}
bool getDeployState()
{
return deploy_flag_;
}
void setSpeedDesAndWheelSpeedDes()
{
switch (heat_limit_->getSpeedLimit())
Expand Down Expand Up @@ -566,29 +525,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
}
}
}
double getFrontWheelSpeedOffset()
{
wheels_speed_offset_front_ = wheel_speed_offset_front_;
return wheels_speed_offset_front_;
}
double getBackWheelSpeedOffset()
{
wheels_speed_offset_back_ = wheel_speed_offset_back_;
return wheels_speed_offset_back_;
}
void dropSpeed()
{
if (hero_flag_)
wheel_speed_offset_front_ -= extra_wheel_speed_once_;
else
total_extra_wheel_speed_ -= extra_wheel_speed_once_;
total_extra_wheel_speed_ -= extra_wheel_speed_once_;
}
void raiseSpeed()
{
if (hero_flag_)
wheel_speed_offset_front_ += extra_wheel_speed_once_;
else
total_extra_wheel_speed_ += extra_wheel_speed_once_;
total_extra_wheel_speed_ += extra_wheel_speed_once_;
}
void setArmorType(uint8_t armor_type)
{
Expand All @@ -605,28 +548,16 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
void setZero() override{};
HeatLimit* heat_limit_{};

int getShootMode()
{
return msg_.mode;
}

private:
double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}, speed_limit_{};
double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{},
wheel_speed_des_{}, last_bullet_speed_{}, speed_oscillation_{};
double wheel_speed_offset_front_{}, wheel_speed_offset_back_{};
double wheels_speed_offset_front_{}, wheels_speed_offset_back_{};
double track_armor_error_tolerance_{};
double track_buff_error_tolerance_{};
double untrack_armor_error_tolerance_{};
double max_track_target_vel_{};
double target_acceleration_tolerance_{};
double extra_wheel_speed_once_{};
double total_extra_wheel_speed_{};
double deploy_wheel_speed_{};
bool auto_wheel_speed_ = false;
bool hero_flag_{};
bool deploy_flag_ = false;
rm_msgs::TrackData track_data_;
rm_msgs::GimbalDesError gimbal_des_error_;
rm_msgs::ShootBeforehandCmd shoot_beforehand_cmd_;
Expand All @@ -635,24 +566,6 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
uint8_t armor_type_{};
};

class UseLioCommandSender : public CommandSenderBase<std_msgs::Bool>
{
public:
explicit UseLioCommandSender(ros::NodeHandle& nh) : CommandSenderBase<std_msgs::Bool>(nh)
{
}

void setUseLio(bool flag)
{
msg_.data = flag;
}
bool getUseLio() const
{
return msg_.data;
}
void setZero() override{};
};

class BalanceCommandSender : public CommandSenderBase<std_msgs::UInt8>
{
public:
Expand Down Expand Up @@ -900,16 +813,12 @@ class CameraSwitchCommandSender : public CommandSenderBase<std_msgs::String>
public:
explicit CameraSwitchCommandSender(ros::NodeHandle& nh) : CommandSenderBase<std_msgs::String>(nh)
{
ROS_ASSERT(nh.getParam("camera_left_name", camera1_name_) && nh.getParam("camera_right_name", camera2_name_));
msg_.data = camera2_name_;
}
void switchCameraLeft()
{
ROS_ASSERT(nh.getParam("camera1_name", camera1_name_) && nh.getParam("camera2_name", camera2_name_));
msg_.data = camera1_name_;
}
void switchCameraRight()
void switchCamera()
{
msg_.data = camera2_name_;
msg_.data = msg_.data == camera1_name_ ? camera2_name_ : camera1_name_;
}
void sendCommand(const ros::Time& time) override
{
Expand Down
Loading
Loading