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

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +6 -2
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 0a7d8525a0 - Show all commits

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_;