Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sync changes from pi

Browse Source
This commit is contained in:
Pimpest
2026-04-15 18:03:10 +02:00
parent ac402a584c
commit 0a7d8525a0
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
toid_behaviors/src/rotate_acorns.cpp
View File

@@ -21,7 +21,7 @@ void RotateAcorns::configureCB()
node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
nav2_util::declare_parameter_if_not_declared( nav2_util::declare_parameter_if_not_declared(
node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.2)); node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.1));
node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_); node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);
nav2_util::declare_parameter_if_not_declared( nav2_util::declare_parameter_if_not_declared(
@@ -29,7 +29,7 @@ void RotateAcorns::configureCB()
node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_); node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);
nav2_util::declare_parameter_if_not_declared( nav2_util::declare_parameter_if_not_declared(
node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(0.5));
node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_); node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
nav2_util::declare_parameter_if_not_declared( nav2_util::declare_parameter_if_not_declared(
@@ -155,6 +155,10 @@ ResultStatus RotateAcorns::updateVel(
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
geometry_msgs::msg::Twist & out_vel) geometry_msgs::msg::Twist & out_vel)
{ {
{
std::lock_guard _lock(mutex_);
target_angle_ = new_target_angle_;
}
const double current_yaw = tf2::getYaw(pose.orientation); const double current_yaw = tf2::getYaw(pose.orientation);
double min_turn_angle = min_turn_angle_; double min_turn_angle = min_turn_angle_;
double angular_speed = angular_speed_; double angular_speed = angular_speed_;