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