diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml
index ac4b6d69..bed77aad 100644
--- a/rm_chassis_controllers/package.xml
+++ b/rm_chassis_controllers/package.xml
@@ -19,6 +19,7 @@
controller_interface
effort_controllers
tf2_geometry_msgs
+ nav_msgs
angles
robot_localization
diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg
index 4e2a2719..9465c224 100755
--- a/rm_shooter_controllers/cfg/Shooter.cfg
+++ b/rm_shooter_controllers/cfg/Shooter.cfg
@@ -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"))
diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
index 4cd20c85..69bd2545 100644
--- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
+++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -48,6 +48,7 @@
#include
#include
#include
+#include
#include
@@ -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 ctrls_friction_l_, ctrls_friction_r_;
effort_controllers::JointPositionController ctrl_trigger_;
std::vector 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
@@ -106,6 +112,7 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer;
realtime_tools::RealtimeBuffer cmd_rt_buffer_;
rm_msgs::ShootCmd cmd_;
+ std::shared_ptr> local_heat_state_pub_;
std::shared_ptr> shoot_state_pub_;
ros::Subscriber cmd_subscriber_;
dynamic_reconfigure::Server* d_srv_{};
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index cd270d54..70efdcb9 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -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("command", 1, &Controller::commandCB, this);
+ local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher(
+ controller_nh, "/local_heat_state/shooter_state", 10));
shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10));
// Init dynamic reconfigure
d_srv_ = new dynamic_reconfigure::Server(controller_nh);
@@ -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;
@@ -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");
@@ -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");
@@ -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,
@@ -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);
}