From 7d72dd388e89d3fe3df0075f3491524f0a1e2537 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Mon, 15 Jul 2024 14:59:56 +0800 Subject: [PATCH] Merge branch 'master' of github.com:rm-controls/rm_control into calculate_heat_trigger --- rm_common/include/rm_common/decision/command_sender.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index aeb6d481..827e9b8b 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -162,7 +162,7 @@ class Vel2DCommandSender : public CommandSenderBase max_angular_z_.init(xml_rpc_value); std::string topic; nh.getParam("power_limit_topic", topic); - target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.); + target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.); chassis_power_limit_subscriber_ = nh.subscribe(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this); } @@ -187,6 +187,15 @@ class Vel2DCommandSender : public CommandSenderBase vel_direction_ = 1.; msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_; }; + void setAngularZVel(double scale, double limit) + { + if (track_data_.v_yaw > target_vel_yaw_threshold_) + vel_direction_ = -1.; + if (track_data_.v_yaw < -target_vel_yaw_threshold_) + vel_direction_ = 1.; + double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_); + msg_.angular.z = scale * angular_z * vel_direction_; + }; void set2DVel(double scale_x, double scale_y, double scale_z) { setLinearXVel(scale_x);