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