Skip to content

Commit

Permalink
Merge branch 'master' into dbus-follow-feedforward
Browse files Browse the repository at this point in the history
  • Loading branch information
liyixin135 committed Jul 16, 2024
2 parents 52bee6e + cb20569 commit 99c6bd8
Show file tree
Hide file tree
Showing 7 changed files with 323 additions and 84 deletions.
1 change: 1 addition & 0 deletions rm_gimbal_controllers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ find_package(catkin REQUIRED

generate_dynamic_reconfigure_options(
cfg/BulletSolver.cfg
cfg/GimbalBase.cfg
)

###################################
Expand Down
8 changes: 8 additions & 0 deletions rm_gimbal_controllers/cfg/BulletSolver.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -14,5 +14,13 @@ gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)
gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0)
gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0)
gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0)
gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0)
gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0)

exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))
14 changes: 14 additions & 0 deletions rm_gimbal_controllers/cfg/GimbalBase.cfg
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
#!/usr/bin/env python
PACKAGE = "rm_gimbal_controllers"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0)
gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0)
gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0)
gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0)
gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0)

exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))
Original file line number Diff line number Diff line change
Expand Up @@ -46,13 +46,19 @@
#include <rm_common/hardware_interface/robot_state_interface.h>
#include <rm_common/eigen_types.h>
#include <rm_common/ros_utilities.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <rm_msgs/TrackData.h>
#include <rm_msgs/ShootBeforehandCmd.h>

namespace rm_gimbal_controllers
{
struct Config
{
double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18,
resistance_coff_qd_30, g, delay, dt, timeout;
resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle,
min_switch_angle, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
int min_fit_switch_count;
};

class BulletSolver
Expand All @@ -61,7 +67,7 @@ class BulletSolver
explicit BulletSolver(ros::NodeHandle& controller_nh);

bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw,
double r1, double r2, double dz, int armors_num);
double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z);
double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1,
double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed);
double getResistanceCoefficient(double bullet_speed) const;
Expand All @@ -76,25 +82,35 @@ class BulletSolver
void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw,
double r1, double r2, double dz, int armors_num);
void judgeShootBeforehand(const ros::Time& time);
void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time);
void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg);
void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t);
~BulletSolver() = default;

private:
std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_desire_pub_;
std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_real_pub_;
std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>> shoot_beforehand_cmd_pub_;
std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> fly_time_pub_;
ros::Subscriber identified_target_change_sub_;
ros::Time switch_armor_time_{};
realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
Config config_{};
double max_track_target_vel_;
bool dynamic_reconfig_initialized_{};
double output_yaw_{}, output_pitch_{};
double bullet_speed_{}, resistance_coff_{};
double fly_time_;
int shoot_beforehand_cmd_{};
int selected_armor_;
int count_;
bool track_target_;
bool identified_target_change_ = true;
bool is_in_delay_before_switch_{};
bool dynamic_reconfig_initialized_{};

geometry_msgs::Point target_pos_{};
double fly_time_;
visualization_msgs::Marker marker_desire_;
visualization_msgs::Marker marker_real_;
};
Expand Down
44 changes: 29 additions & 15 deletions rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -37,23 +37,33 @@

#pragma once

#include <effort_controllers/joint_position_controller.h>
#include <effort_controllers/joint_velocity_controller.h>
#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <rm_common/hardware_interface/robot_state_interface.h>
#include <hardware_interface/imu_sensor_interface.h>
#include <realtime_tools/realtime_publisher.h>
#include <rm_common/hardware_interface/robot_state_interface.h>
#include <rm_common/filters/filters.h>
#include <rm_msgs/GimbalCmd.h>
#include <rm_msgs/TrackData.h>
#include <rm_msgs/GimbalDesError.h>
#include <dynamic_reconfigure/server.h>
#include <rm_msgs/GimbalPosState.h>
#include <rm_gimbal_controllers/GimbalBaseConfig.h>
#include <rm_gimbal_controllers/bullet_solver.h>
#include <tf2_eigen/tf2_eigen.h>
#include <Eigen/Eigen>
#include <rm_common/filters/filters.h>
#include <control_toolbox/pid.h>
#include <urdf/model.h>
#include <dynamic_reconfigure/server.h>
#include <realtime_tools/realtime_publisher.h>

namespace rm_gimbal_controllers
{
struct GimbalConfig
{
double yaw_k_v_, pitch_k_v_, k_chassis_vel_;
double accel_pitch_{}, accel_yaw_{};
};

class ChassisVel
{
public:
Expand Down Expand Up @@ -139,27 +149,33 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
void updateChassisVel();
void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
void trackCB(const rm_msgs::TrackDataConstPtr& msg);
void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t);

rm_control::RobotStateHandle robot_state_handle_;
hardware_interface::ImuSensorHandle imu_sensor_handle_;
bool has_imu_ = true;
effort_controllers::JointPositionController ctrl_yaw_, ctrl_pitch_;
effort_controllers::JointVelocityController ctrl_yaw_, ctrl_pitch_;
control_toolbox::Pid pid_yaw_pos_, pid_pitch_pos_;

std::shared_ptr<BulletSolver> bullet_solver_;

// ROS Interface
ros::Time last_publish_time_{};
std::unique_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState>> yaw_pos_state_pub_, pitch_pos_state_pub_;
std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError>> error_pub_;
ros::Subscriber cmd_gimbal_sub_;
ros::Subscriber data_track_sub_;
realtime_tools::RealtimeBuffer<rm_msgs::GimbalCmd> cmd_rt_buffer_;
realtime_tools::RealtimeBuffer<rm_msgs::TrackData> track_rt_buffer_;
urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_;

rm_msgs::GimbalCmd cmd_gimbal_;
rm_msgs::TrackData data_track_;
std::string gimbal_des_frame_id_{}, imu_name_{};
double publish_rate_{};
bool state_changed_{};
bool pitch_des_in_limit_{}, yaw_des_in_limit_{};
int loop_count_{};

// Transform
geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_;
Expand All @@ -169,18 +185,16 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
double gravity_;
bool enable_gravity_compensation_;

// Input feedforward
double yaw_k_v_;
double pitch_k_v_;

// Resistance compensation
double yaw_resistance_;
double velocity_saturation_point_, effort_saturation_point_;

// Chassis
double k_chassis_vel_;
std::shared_ptr<ChassisVel> chassis_vel_;

bool dynamic_reconfig_initialized_{};
GimbalConfig config_{};
realtime_tools::RealtimeBuffer<GimbalConfig> config_rt_buffer_;
dynamic_reconfigure::Server<rm_gimbal_controllers::GimbalBaseConfig>* d_srv_{};

RampFilter<double>*ramp_rate_pitch_{}, *ramp_rate_yaw_{};

enum
{
RATE,
Expand Down
115 changes: 103 additions & 12 deletions rm_gimbal_controllers/src/bullet_solver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,15 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
.g = getParam(controller_nh, "g", 0.),
.delay = getParam(controller_nh, "delay", 0.),
.dt = getParam(controller_nh, "dt", 0.),
.timeout = getParam(controller_nh, "timeout", 0.) };
.timeout = getParam(controller_nh, "timeout", 0.),
.ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0),
.gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0),
.max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0),
.min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0),
.max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5),
.track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.),
.track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.),
.min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) };
max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
config_rt_buffer_.initRT(config_);

Expand Down Expand Up @@ -80,6 +88,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
new realtime_tools::RealtimePublisher<visualization_msgs::Marker>(controller_nh, "model_desire", 10));
path_real_pub_.reset(
new realtime_tools::RealtimePublisher<visualization_msgs::Marker>(controller_nh, "model_real", 10));
shoot_beforehand_cmd_pub_.reset(
new realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>(controller_nh, "shoot_beforehand_cmd", 10));
fly_time_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64>(controller_nh, "fly_time", 10));

identified_target_change_sub_ =
controller_nh.subscribe<std_msgs::Bool>("/change", 10, &BulletSolver::identifiedTargetChangeCB, this);
}

double BulletSolver::getResistanceCoefficient(double bullet_speed) const
Expand All @@ -100,7 +114,7 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const
}

bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw,
double v_yaw, double r1, double r2, double dz, int armors_num)
double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z)
{
config_ = *config_rt_buffer_.readFromRT();
bullet_speed_ = bullet_speed;
Expand All @@ -115,18 +129,48 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
selected_armor_ = 0;
double r = r1;
double z = pos.z;
double max_switch_angle = config_.max_switch_angle / 180 * M_PI;
double min_switch_angle = config_.min_switch_angle / 180 * M_PI;
track_target_ = std::abs(v_yaw) < max_track_target_vel_;
double switch_armor_angle = track_target_ ?
acos(r / target_rho) - M_PI / 12 +
(-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ :
M_PI / 12;
if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
(((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.))
if (std::abs(chassis_angular_vel_z) >= config_.max_chassis_angular_vel)
track_target_ = 0;
double switch_armor_angle =
track_target_ ?
(acos(r / target_rho) - max_switch_angle +
(-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) *
(1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) +
min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel :
min_switch_angle;
if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
(((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) &&
std::abs(v_yaw) >= 1.0)
{
selected_armor_ = v_yaw > 0. ? -1 : 1;
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
count_++;
if (identified_target_change_)
{
count_ = 0;
identified_target_change_ = false;
}
if (count_ >= config_.min_fit_switch_count)
{
if (count_ == config_.min_fit_switch_count)
switch_armor_time_ = ros::Time::now();
selected_armor_ = v_yaw > 0. ? -1 : 1;
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
}
}
is_in_delay_before_switch_ =
(((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) >
output_yaw_ + switch_armor_angle) &&
v_yaw > 0.) ||
((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) <
output_yaw_ - switch_armor_angle) &&
v_yaw < 0.)) &&
track_target_;
yaw += v_yaw * config_.track_rotate_target_delay;
pos.x += vel.x * config_.track_move_target_delay;
pos.y += vel.y * config_.track_move_target_delay;
int count{};
double error = 999;
if (track_target_)
Expand Down Expand Up @@ -180,6 +224,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
if (count >= 20 || std::isnan(error))
return false;
}
if (fly_time_pub_->trylock())
{
fly_time_pub_->msg_.data = fly_time_;
fly_time_pub_->unlockAndPublish();
}
return true;
}

Expand Down Expand Up @@ -301,6 +350,32 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
return error;
}

void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg)
{
if (msg->data)
identified_target_change_ = true;
}

void BulletSolver::judgeShootBeforehand(const ros::Time& time)
{
if (!track_target_)
shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec())
shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec())
shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
else if (is_in_delay_before_switch_)
shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
else
shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
if (shoot_beforehand_cmd_pub_->trylock())
{
shoot_beforehand_cmd_pub_->msg_.stamp = time;
shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_;
shoot_beforehand_cmd_pub_->unlockAndPublish();
}
}

void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/)
{
ROS_INFO("[Bullet Solver] Dynamic params change");
Expand All @@ -316,6 +391,14 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
config.delay = init_config.delay;
config.dt = init_config.dt;
config.timeout = init_config.timeout;
config.ban_shoot_duration = init_config.ban_shoot_duration;
config.gimbal_switch_duration = init_config.gimbal_switch_duration;
config.max_switch_angle = init_config.max_switch_angle;
config.min_switch_angle = init_config.min_switch_angle;
config.max_chassis_angular_vel = init_config.max_chassis_angular_vel;
config.track_rotate_target_delay = init_config.track_rotate_target_delay;
config.track_move_target_delay = init_config.track_move_target_delay;
config.min_fit_switch_count = init_config.min_fit_switch_count;
dynamic_reconfig_initialized_ = true;
}
Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10,
Expand All @@ -326,7 +409,15 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
.g = config.g,
.delay = config.delay,
.dt = config.dt,
.timeout = config.timeout };
.timeout = config.timeout,
.ban_shoot_duration = config.ban_shoot_duration,
.gimbal_switch_duration = config.gimbal_switch_duration,
.max_switch_angle = config.max_switch_angle,
.min_switch_angle = config.min_switch_angle,
.max_chassis_angular_vel = config.max_chassis_angular_vel,
.track_rotate_target_delay = config.track_rotate_target_delay,
.track_move_target_delay = config.track_move_target_delay,
.min_fit_switch_count = config.min_fit_switch_count };
config_rt_buffer_.writeFromNonRT(config_non_rt);
}
} // namespace rm_gimbal_controllers
Loading

0 comments on commit 99c6bd8

Please sign in to comment.