Skip to content

Commit

Permalink
Merge pull request #170 from BlanchardLj/calculate_local_heat
Browse files Browse the repository at this point in the history 
Calculate local heat.
  • Loading branch information
@d0h0s
d0h0s authored Jul 25, 2024
2 parents c1fdac1 + 3d3dedb commit 683d191
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 5 deletions.
2 changes: 2 additions & 0 deletions rm_shooter_controllers/cfg/Shooter.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,5 +14,7 @@ gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold
gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1)
gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1)
gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999)
gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999)
gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999)

exit(gen.generate(PACKAGE, "shooter", "Shooter"))
11 changes: 9 additions & 2 deletions rm_shooter_controllers/include/rm_shooter_controllers/standard.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include <rm_shooter_controllers/ShooterConfig.h>
#include <rm_msgs/ShootCmd.h>
#include <rm_msgs/ShootState.h>
#include <rm_msgs/LocalHeatState.h>

#include <utility>

Expand All @@ -57,7 +58,7 @@ struct Config
{
double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold,
forward_push_threshold, exit_push_threshold;
double extra_wheel_speed;
double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold;
};

class Controller : public controller_interface::MultiInterfaceController<hardware_interface::EffortJointInterface,
Expand All @@ -76,22 +77,27 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
void block(const ros::Time& time, const ros::Duration& period);
void setSpeed(const rm_msgs::ShootCmd& cmd);
void normalize();
void judgeBulletShoot(const ros::Time& time, const ros::Duration& period);
void commandCB(const rm_msgs::ShootCmdConstPtr& msg)
{
cmd_rt_buffer_.writeFromNonRT(*msg);
}

void reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/);

hardware_interface::EffortJointInterface* effort_joint_interface_{};
std::vector<effort_controllers::JointVelocityController*> ctrls_friction_l_, ctrls_friction_r_;
effort_controllers::JointPositionController ctrl_trigger_;
std::vector<double> wheel_speed_offset_l_, wheel_speed_offset_r_;
int push_per_rotation_{};
int push_per_rotation_{}, count_{};
double push_wheel_speed_threshold_{};
double freq_threshold_{};
bool dynamic_reconfig_initialized_ = false;
bool state_changed_ = false;
bool maybe_block_ = false;
bool has_shoot_ = false, has_shoot_last_ = false;
bool wheel_speed_raise_ = true, wheel_speed_drop_ = true;
double last_wheel_speed_{};

ros::Time last_shoot_time_, block_time_, last_block_time_;
enum
Expand All @@ -106,6 +112,7 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
realtime_tools::RealtimeBuffer<Config> config_rt_buffer;
realtime_tools::RealtimeBuffer<rm_msgs::ShootCmd> cmd_rt_buffer_;
rm_msgs::ShootCmd cmd_;
std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>> local_heat_state_pub_;
std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootState>> shoot_state_pub_;
ros::Subscriber cmd_subscriber_;
dynamic_reconfigure::Server<rm_shooter_controllers::ShooterConfig>* d_srv_{};
Expand Down
56 changes: 53 additions & 3 deletions rm_shooter_controllers/src/standard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,13 +52,17 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
.anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.),
.forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1),
.exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1),
.extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.) };
.extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.),
.wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.),
.wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) };
config_rt_buffer.initRT(config_);
push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0);
push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.);
freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.);

cmd_subscriber_ = controller_nh.subscribe<rm_msgs::ShootCmd>("command", 1, &Controller::commandCB, this);
local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>(
controller_nh, "/local_heat_state/shooter_state", 10));
shoot_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::ShootState>(controller_nh, "state", 10));
// Init dynamic reconfigure
d_srv_ = new dynamic_reconfigure::Server<rm_shooter_controllers::ShooterConfig>(controller_nh);
Expand Down Expand Up @@ -138,6 +142,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
block(time, period);
break;
}
judgeBulletShoot(time, period);
if (shoot_state_pub_->trylock())
{
shoot_state_pub_->msg_.stamp = time;
Expand Down Expand Up @@ -252,7 +257,6 @@ void Controller::block(const ros::Time& time, const ros::Duration& period)
(time - last_block_time_).toSec() > config_.block_overtime)
{
normalize();

state_ = PUSH;
state_changed_ = true;
ROS_INFO("[Shooter] Exit BLOCK");
Expand All @@ -274,6 +278,48 @@ void Controller::normalize()
push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle));
}

void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period)
{
if (state_ != STOP)
{
if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold &&
wheel_speed_drop_)
{
wheel_speed_raise_ = true;
wheel_speed_drop_ = false;
}

if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold &&
abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_)
{
wheel_speed_drop_ = true;
wheel_speed_raise_ = false;
has_shoot_ = true;
}
}
double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_;
last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity());
count_++;
if (has_shoot_last_)
{
has_shoot_ = true;
}
has_shoot_last_ = has_shoot_;
if (count_ == 2)
{
if (local_heat_state_pub_->trylock())
{
local_heat_state_pub_->msg_.stamp = time;
local_heat_state_pub_->msg_.has_shoot = has_shoot_;
local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel;
local_heat_state_pub_->unlockAndPublish();
}
has_shoot_last_ = false;
count_ = 0;
}
if (has_shoot_)
has_shoot_ = false;
}
void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/)
{
ROS_INFO("[Shooter] Dynamic params change");
Expand All @@ -289,6 +335,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3
config.forward_push_threshold = init_config.forward_push_threshold;
config.exit_push_threshold = init_config.exit_push_threshold;
config.extra_wheel_speed = init_config.extra_wheel_speed;
config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold;
config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold;
dynamic_reconfig_initialized_ = true;
}
Config config_non_rt{ .block_effort = config.block_effort,
Expand All @@ -299,7 +347,9 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3
.anti_block_threshold = config.anti_block_threshold,
.forward_push_threshold = config.forward_push_threshold,
.exit_push_threshold = config.exit_push_threshold,
.extra_wheel_speed = config.extra_wheel_speed };
.extra_wheel_speed = config.extra_wheel_speed,
.wheel_speed_drop_threshold = config.wheel_speed_drop_threshold,
.wheel_speed_raise_threshold = config.wheel_speed_raise_threshold };
config_rt_buffer.writeFromNonRT(config_non_rt);
}

Expand Down

0 comments on commit 683d191

Please sign in to comment.