diff --git a/config/planner_param.yaml b/config/planner_param.yaml index 3412747..6a017a0 100644 --- a/config/planner_param.yaml +++ b/config/planner_param.yaml @@ -5,7 +5,7 @@ angle_resolution: 0.0087 dist_to_goal_th: 0.3 turn_direction_th: 0.1 angle_to_goal_th: 0.26 -max_yawrate_in_situ_turns: 0.4 +min_in_place_yawrate: 0.3 dist_from_head_to_obj: 0.5 velocity_samples: 3 yawrate_samples: 20 diff --git a/config/robot_param.yaml b/config/robot_param.yaml index 6fd9b66..aa6ffb6 100644 --- a/config/robot_param.yaml +++ b/config/robot_param.yaml @@ -2,5 +2,6 @@ robot_frame: "base_link" max_velocity: 1.0 min_velocity: 0.0 max_yawrate: 1.0 +min_in_place_yawrate: 0.3 max_acceleration: 0.5 max_d_yawrate: 3.2 diff --git a/include/point_follow_planner/point_follow_planner.h b/include/point_follow_planner/point_follow_planner.h index b8c2ad8..fb17237 100644 --- a/include/point_follow_planner/point_follow_planner.h +++ b/include/point_follow_planner/point_follow_planner.h @@ -104,7 +104,7 @@ class PointFollowPlanner double max_velocity_; double min_velocity_; double max_yawrate_; - double max_yawrate_in_situ_turns_; + double min_in_place_yawrate_; double max_acceleration_; double max_d_yawrate_; double angle_resolution_; diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp index 2361bc1..b76bba7 100644 --- a/src/point_follow_planner.cpp +++ b/src/point_follow_planner.cpp @@ -17,7 +17,7 @@ PointFollowPlanner::PointFollowPlanner(void) private_nh_.param("max_velocity", max_velocity_, {0.55}); private_nh_.param("min_velocity", min_velocity_, {0.0}); private_nh_.param("max_yawrate", max_yawrate_, {1.0}); - private_nh_.param("max_yawrate_in_situ_turns", max_yawrate_in_situ_turns_, max_yawrate_); + private_nh_.param("min_in_place_yawrate", min_in_place_yawrate_, {0.3}); private_nh_.param("max_acceleration", max_acceleration_, {0.5}); private_nh_.param("max_d_yawrate", max_d_yawrate_, {3.2}); private_nh_.param("angle_resolution", angle_resolution_, {0.2}); @@ -38,7 +38,7 @@ PointFollowPlanner::PointFollowPlanner(void) ROS_INFO_STREAM("max_velocity: " << max_velocity_); ROS_INFO_STREAM("min_velocity: " << min_velocity_); ROS_INFO_STREAM("max_yawrate: " << max_yawrate_); - ROS_INFO_STREAM("max_yawrate_in_situ_turns: " << max_yawrate_in_situ_turns_); + ROS_INFO_STREAM("min_in_place_yawrate: " << min_in_place_yawrate_); ROS_INFO_STREAM("max_acceleration: " << max_acceleration_); ROS_INFO_STREAM("max_d_yawrate: " << max_d_yawrate_); ROS_INFO_STREAM("angle_resolution: " << angle_resolution_); @@ -411,7 +411,9 @@ geometry_msgs::Twist PointFollowPlanner::calc_cmd_vel() { const double angle_to_goal = atan2(goal.y(), goal.x()); cmd_vel.angular.z = - std::min(std::max(angle_to_goal, -max_yawrate_in_situ_turns_), max_yawrate_in_situ_turns_); + angle_to_goal > 0 ? std::min(angle_to_goal, max_yawrate_) : std::max(angle_to_goal, -max_yawrate_); + cmd_vel.angular.z = cmd_vel.angular.z > 0 ? std::max(cmd_vel.angular.z, min_in_place_yawrate_) + : std::min(cmd_vel.angular.z, -min_in_place_yawrate_); generate_trajectory(best_traj, cmd_vel.angular.z, goal); trajectories.push_back(best_traj); } @@ -427,7 +429,9 @@ geometry_msgs::Twist PointFollowPlanner::calc_cmd_vel() has_reached_ = true; if (turn_direction_th_ < fabs(goal[2])) { - cmd_vel.angular.z = std::min(std::max(goal[2], -max_yawrate_in_situ_turns_), max_yawrate_in_situ_turns_); + cmd_vel.angular.z = goal[2] > 0 ? std::min(goal[2], max_yawrate_) : std::max(goal[2], -max_yawrate_); + cmd_vel.angular.z = cmd_vel.angular.z > 0 ? std::max(cmd_vel.angular.z, min_in_place_yawrate_) + : std::min(cmd_vel.angular.z, -min_in_place_yawrate_); } else { @@ -451,7 +455,9 @@ bool PointFollowPlanner::can_adjust_robot_direction(const Eigen::Vector3d &goal) if (fabs(angle_to_goal) < angle_to_goal_th_) return false; - const double yawrate = std::min(std::max(angle_to_goal, -max_yawrate_in_situ_turns_), max_yawrate_in_situ_turns_); + double yawrate = angle_to_goal > 0 ? std::min(angle_to_goal, max_yawrate_) : std::max(angle_to_goal, -max_yawrate_); + yawrate = yawrate > 0 ? std::max(yawrate, min_in_place_yawrate_) : std::min(yawrate, -min_in_place_yawrate_); + std::vector traj; generate_trajectory(traj, yawrate, goal);