static void doPurePursuit()
{
//search next waypoint
int next_waypoint = getNextWaypoint();
displayNextWaypoint(next_waypoint);
displaySearchRadius(getLookAheadThreshold(0));
//ROS_INFO_STREAM("next waypoint = " << next_waypoint << "/" << path_size - 1);
//linear interpolation and calculate angular velocity
geometry_msgs::Point next_target;
geometry_msgs::TwistStamped twist;
std_msgs::Bool wf_stat;
bool interpolate_flag = false;
if (!g_linear_interpolate_mode)
{
next_target = _current_waypoints.getWaypointPosition(next_waypoint);
}
else
{
interpolate_flag = interpolateNextTarget(next_waypoint, &next_target);
}
if (g_linear_interpolate_mode && !interpolate_flag)
{
ROS_INFO_STREAM("lost target! ");
wf_stat.data = false;
_stat_pub.publish(wf_stat);
twist.twist.linear.x = 0;
twist.twist.angular.z = 0;
twist.header.stamp = ros::Time::now();
g_cmd_velocity_publisher.publish(twist);
return;
}
//ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z);
displayNextTarget(next_target);
displayTrajectoryCircle(generateTrajectoryCircle(next_target));
twist.twist = calcTwist(calcCurvature(next_target), getCmdVelocity(0));
wf_stat.data = true;
_stat_pub.publish(wf_stat);
twist.header.stamp = ros::Time::now();
g_cmd_velocity_publisher.publish(twist);
//ROS_INFO("linear : %lf, angular : %lf",twist.twist.linear.x,twist.twist.angular.z);
#ifdef LOG
std::ofstream ofs("/tmp/pure_pursuit.log", std::ios::app);
ofs << _current_waypoints.getWaypointPosition(next_waypoint).x << " "
<< _current_waypoints.getWaypointPosition(next_waypoint).y << " " << next_target.x << " " << next_target.y
<< std::endl;
#endif
}
bool PurePursuit::canGetCurvature(double *output_kappa)
{
// search next waypoint
getNextWaypoint();
if (next_waypoint_number_ == -1)
{
ROS_INFO("lost next waypoint");
return false;
}
// check whether curvature is valid or not
bool is_valid_curve = false;
for (const auto &el : current_waypoints_)
{
if (getPlaneDistance(el.pose.pose.position, current_pose_.position) > minimum_lookahead_distance_)
{
is_valid_curve = true;
break;
}
}
if (!is_valid_curve)
{
return false;
}
// if is_linear_interpolation_ is false or next waypoint is first or last
if (!is_linear_interpolation_ || next_waypoint_number_ == 0 ||
next_waypoint_number_ == (static_cast<int>(current_waypoints_.size() - 1)))
{
next_target_position_ = current_waypoints_.at(next_waypoint_number_).pose.pose.position;
*output_kappa = calcCurvature(next_target_position_);
return true;
}
// linear interpolation and calculate angular velocity
bool interpolation = interpolateNextTarget(next_waypoint_number_, &next_target_position_);
if (!interpolation)
{
ROS_INFO_STREAM("lost target! ");
return false;
}
// ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z);
*output_kappa = calcCurvature(next_target_position_);
return true;
}