void PurePursuit::getNextWaypoint()
{
int path_size = static_cast<int>(current_waypoints_.size());
// if waypoints are not given, do nothing.
if (path_size == 0)
{
next_waypoint_number_ = -1;
return;
}
// look for the next waypoint.
for (int i = 0; i < path_size; i++)
{
// if search waypoint is the last
if (i == (path_size - 1))
{
ROS_INFO("search waypoint is the last");
next_waypoint_number_ = i;
return;
}
// if there exists an effective waypoint
if (getPlaneDistance(current_waypoints_.at(i).pose.pose.position, current_pose_.position) > lookahead_distance_)
{
next_waypoint_number_ = i;
return;
}
}
// if this program reaches here , it means we lost the waypoint!
next_waypoint_number_ = -1;
return;
}
static int getNextWaypoint()
{
int path_size = static_cast<int>(_current_waypoints.getSize());
double lookahead_threshold = getLookAheadThreshold(0);
// if waypoints are not given, do nothing.
if (_current_waypoints.isEmpty())
{
return -1;
}
// look for the next waypoint.
for(int i = 0; i < path_size; i++)
{
//if search waypoint is the last
if (i == (path_size - 1))
{
ROS_INFO("search waypoint is the last");
return i;
}
// if there exists an effective waypoint
if (getPlaneDistance(_current_waypoints.getWaypointPosition(i), _current_pose.pose.position) > lookahead_threshold)
{
return i;
}
}
//if this program reaches here , it means we lost the waypoint!
return -1;
}
static double calcRadius(geometry_msgs::Point target)
{
double radius;
double denominator = 2 * calcRelativeCoordinate(target, _current_pose.pose).y;
double numerator = pow(getPlaneDistance(target, _current_pose.pose.position), 2);
if (denominator != 0)
radius = numerator / denominator;
else
radius = 0;
//ROS_INFO("radius : %lf", radius);
return radius;
}
// ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//
ofVec3f MousePosOnPlane::getMousePosOnPlane( float _mouseX, float _mouseY, ofVec3f _planeNormal, ofVec3f _pointOnPlane,
ofMatrix4x4* _modelViewMatrix, ofMatrix4x4* _projectionMatrix, ofRectangle* _viewPort )
{
ofMatrix4x4 modelViewMat;
ofMatrix4x4 projectionMat;
GLint viewport[4];
if( _modelViewMatrix == NULL ) { glGetFloatv(GL_MODELVIEW_MATRIX, modelViewMat.getPtr() ); }
else { modelViewMat.set( *_modelViewMatrix ); }
if( _projectionMatrix == NULL ) { glGetFloatv(GL_PROJECTION_MATRIX, projectionMat.getPtr() ); }
else { projectionMat.set( *_projectionMatrix ); }
if( _viewPort == NULL )
{
glGetIntegerv(GL_VIEWPORT, viewport);
}
else
{
viewport[0] = _viewPort->x;
viewport[1] = _viewPort->y;
viewport[2] = _viewPort->width;
viewport[3] = _viewPort->height;
}
float dx, dy, dz = 0.0;
ofVec3f mouseRayPosition;
ofVec3f mouseRayVector;
int ry = viewport[3] - _mouseY - 1;
gluES::gluUnProject( _mouseX, ry, 0.0,
modelViewMat.getPtr(), projectionMat.getPtr(), viewport,
&mouseRayPosition.x, &mouseRayPosition.y, &mouseRayPosition.z);
gluES::gluUnProject( _mouseX, ry, 1.0,
modelViewMat.getPtr(), projectionMat.getPtr(), viewport,
&mouseRayVector.x, &mouseRayVector.y, &mouseRayVector.z);
mouseRayVector -= mouseRayPosition;
mouseRayVector.normalize();
float planeDistance = getPlaneDistance( &_planeNormal, &_pointOnPlane );
float tmpDist = rayIntersectPlane( &_planeNormal, planeDistance, &mouseRayPosition, &mouseRayVector );
return mouseRayPosition + (mouseRayVector * tmpDist);
}
static double calcCurvature(geometry_msgs::Point target)
{
double kappa;
double denominator = pow(getPlaneDistance(target, _current_pose.pose.position), 2);
double numerator = 2 * calcRelativeCoordinate(target, _current_pose.pose).y;
if (denominator != 0)
kappa = numerator / denominator;
else
kappa = 0;
//ROS_INFO("kappa : %lf", kappa);
return kappa;
}
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;
}
double PurePursuit::calcCurvature(geometry_msgs::Point target) const
{
double kappa;
double denominator = pow(getPlaneDistance(target, current_pose_.position), 2);
double numerator = 2 * calcRelativeCoordinate(target, current_pose_).y;
if (denominator != 0)
kappa = numerator / denominator;
else
{
if (numerator > 0)
kappa = KAPPA_MIN_;
else
kappa = -KAPPA_MIN_;
}
ROS_INFO("kappa : %lf", kappa);
return kappa;
}
//linear interpolation of next target
static bool interpolateNextTarget(int next_waypoint,geometry_msgs::Point *next_target)
{
double search_radius = getLookAheadThreshold(0);
geometry_msgs::Point zero_p;
geometry_msgs::Point end = _current_waypoints.getWaypointPosition(next_waypoint);
geometry_msgs::Point start = _current_waypoints.getWaypointPosition(next_waypoint - 1);
//let the linear equation be "y = slope * x + intercept"
double slope = 0;
double intercept = 0;
getLinearEquation(start,end,&slope, &intercept);
//let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position
//the distance "d" between the foot of a perpendicular line and the center of circle is ...
// | y0 - slope * x0 - intercept |
//d = -------------------------------
// √( 1 + slope^2)
double d = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept);
//ROS_INFO("slope : %lf ", slope);
//ROS_INFO("intercept : %lf ", intercept);
//ROS_INFO("distance : %lf ", d);
if (d > search_radius)
return false;
//unit vector of point 'start' to point 'end'
tf::Vector3 v((end.x - start.x), (end.y - start.y), 0);
tf::Vector3 unit_v = v.normalize();
//normal unit vectors of v
tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90); //rotate to counter clockwise 90 degree
tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90); //rotate to counter clockwise 90 degree
//the foot of a perpendicular line
geometry_msgs::Point h1;
h1.x = _current_pose.pose.position.x + d * unit_w1.getX();
h1.y = _current_pose.pose.position.y + d * unit_w1.getY();
h1.z = _current_pose.pose.position.z;
geometry_msgs::Point h2;
h2.x = _current_pose.pose.position.x + d * unit_w2.getX();
h2.y = _current_pose.pose.position.y + d * unit_w2.getY();
h2.z = _current_pose.pose.position.z;
double error = pow(10, -5); //0.00001
//ROS_INFO("error : %lf", error);
//ROS_INFO("whether h1 on line : %lf", h1.y - (slope * h1.x + intercept));
//ROS_INFO("whether h2 on line : %lf", h2.y - (slope * h2.x + intercept));
//check which of two foot of a perpendicular line is on the line equation
geometry_msgs::Point h;
if (fabs(h1.y - (slope * h1.x + intercept)) < error)
{
h = h1;
// ROS_INFO("use h1");
}
else if (fabs(h2.y - (slope * h2.x + intercept)) < error)
{
// ROS_INFO("use h2");
h = h2;
}
else
{
return false;
}
//get intersection[s]
//if there is a intersection
if (d == search_radius)
{
*next_target = h;
return true;
}
else
{
//if there are two intersection
//get intersection in front of vehicle
double s = sqrt(pow(search_radius, 2) - pow(d, 2));
geometry_msgs::Point target1;
target1.x = h.x + s * unit_v.getX();
target1.y = h.y + s * unit_v.getY();
target1.z = _current_pose.pose.position.z;
geometry_msgs::Point target2;
target2.x = h.x - s * unit_v.getX();
target2.y = h.y - s * unit_v.getY();
target2.z = _current_pose.pose.position.z;
//ROS_INFO("target1 : ( %lf , %lf , %lf)", target1.x, target1.y, target1.z);
//ROS_INFO("target2 : ( %lf , %lf , %lf)", target2.x, target2.y, target2.z);
displayLinePoint(slope, intercept, target1, target2, h); //debug tool
//check intersection is between end and start
double interval = getPlaneDistance(end,start);
if (getPlaneDistance(target1, end) < interval)
{
//ROS_INFO("result : target1");
*next_target = target1;
return true;
}
else if (getPlaneDistance(target2, end) < interval)
{
//ROS_INFO("result : target2");
*next_target = target2;
return true;
}
else
{
//ROS_INFO("result : false ");
return false;
}
}
}
// linear interpolation of next target
bool PurePursuit::interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target) const
{
constexpr double ERROR = pow(10, -5); // 0.00001
int path_size = static_cast<int>(current_waypoints_.size());
if (next_waypoint == path_size - 1)
{
*next_target = current_waypoints_.at(next_waypoint).pose.pose.position;
return true;
}
double search_radius = lookahead_distance_;
geometry_msgs::Point zero_p;
geometry_msgs::Point end = current_waypoints_.at(next_waypoint).pose.pose.position;
geometry_msgs::Point start = current_waypoints_.at(next_waypoint - 1).pose.pose.position;
// let the linear equation be "ax + by + c = 0"
// if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
double a = 0;
double b = 0;
double c = 0;
double get_linear_flag = getLinearEquation(start, end, &a, &b, &c);
if (!get_linear_flag)
return false;
// let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position
// the distance "d" between the foot of a perpendicular line and the center of circle is ...
// | a * x0 + b * y0 + c |
// d = -------------------------------
// √( a~2 + b~2)
double d = getDistanceBetweenLineAndPoint(current_pose_.position, a, b, c);
// ROS_INFO("a : %lf ", a);
// ROS_INFO("b : %lf ", b);
// ROS_INFO("c : %lf ", c);
// ROS_INFO("distance : %lf ", d);
if (d > search_radius)
return false;
// unit vector of point 'start' to point 'end'
tf::Vector3 v((end.x - start.x), (end.y - start.y), 0);
tf::Vector3 unit_v = v.normalize();
// normal unit vectors of v
tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90); // rotate to counter clockwise 90 degree
tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90); // rotate to counter clockwise 90 degree
// the foot of a perpendicular line
geometry_msgs::Point h1;
h1.x = current_pose_.position.x + d * unit_w1.getX();
h1.y = current_pose_.position.y + d * unit_w1.getY();
h1.z = current_pose_.position.z;
geometry_msgs::Point h2;
h2.x = current_pose_.position.x + d * unit_w2.getX();
h2.y = current_pose_.position.y + d * unit_w2.getY();
h2.z = current_pose_.position.z;
// ROS_INFO("error : %lf", error);
// ROS_INFO("whether h1 on line : %lf", h1.y - (slope * h1.x + intercept));
// ROS_INFO("whether h2 on line : %lf", h2.y - (slope * h2.x + intercept));
// check which of two foot of a perpendicular line is on the line equation
geometry_msgs::Point h;
if (fabs(a * h1.x + b * h1.y + c) < ERROR)
{
h = h1;
// ROS_INFO("use h1");
}
else if (fabs(a * h2.x + b * h2.y + c) < ERROR)
{
// ROS_INFO("use h2");
h = h2;
}
else
{
return false;
}
// get intersection[s]
// if there is a intersection
if (d == search_radius)
{
*next_target = h;
return true;
}
else
{
// if there are two intersection
// get intersection in front of vehicle
double s = sqrt(pow(search_radius, 2) - pow(d, 2));
geometry_msgs::Point target1;
target1.x = h.x + s * unit_v.getX();
target1.y = h.y + s * unit_v.getY();
target1.z = current_pose_.position.z;
geometry_msgs::Point target2;
target2.x = h.x - s * unit_v.getX();
target2.y = h.y - s * unit_v.getY();
target2.z = current_pose_.position.z;
// ROS_INFO("target1 : ( %lf , %lf , %lf)", target1.x, target1.y, target1.z);
// ROS_INFO("target2 : ( %lf , %lf , %lf)", target2.x, target2.y, target2.z);
// displayLinePoint(a, b, c, target1, target2, h); // debug tool
// check intersection is between end and start
double interval = getPlaneDistance(end, start);
if (getPlaneDistance(target1, end) < interval)
{
// ROS_INFO("result : target1");
*next_target = target1;
return true;
}
else if (getPlaneDistance(target2, end) < interval)
{
// ROS_INFO("result : target2");
*next_target = target2;
return true;
}
else
{
// ROS_INFO("result : false ");
return false;
}
}
}
