static bool verifyFollowing()
{
double slope = 0;
double intercept = 0;
getLinearEquation(_current_waypoints.getWaypointPosition(1),_current_waypoints.getWaypointPosition(2),&slope,&intercept);
double displacement = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept);
double relative_angle = getRelativeAngle(_current_waypoints.getWaypointPose(1),_current_pose.pose);
//ROS_INFO("side diff : %lf , angle diff : %lf",displacement,relative_angle);
if(displacement < g_displacement_threshold || relative_angle < g_relative_angle_threshold){
//ROS_INFO("Following : True");
return true;
}
else{
//ROS_INFO("Following : False");
return false;
}
}
//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;
}
}
}
