diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index 405ca0a..98c3a15 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/toid_behaviors/src/rotate_acorns.cpp @@ -21,7 +21,7 @@ void RotateAcorns::configureCB() node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_); 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_); 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_); 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_); 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 &, geometry_msgs::msg::Twist & out_vel) { + { + std::lock_guard _lock(mutex_); + target_angle_ = new_target_angle_; + } const double current_yaw = tf2::getYaw(pose.orientation); double min_turn_angle = min_turn_angle_; double angular_speed = angular_speed_;