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
}
static int FindCrossWalk()
{
if (!vmap.set_points || _closest_waypoint < 0)
return -1;
double find_distance = 2.0*2.0; // meter
double ignore_distance = 20.0*20.0; // meter
// Find near cross walk
for (int num = _closest_waypoint; num < _closest_waypoint+_search_distance; num++) {
geometry_msgs::Point waypoint = _path_dk.getWaypointPosition(num);
waypoint.z = 0.0; // ignore Z axis
for (int i = 1; i < vmap.getSize(); i++) {
// ignore far crosswalk
geometry_msgs::Point crosswalk_center = vmap.getDetectionPoints(i).center;
crosswalk_center.z = 0.0;
if (CalcSquareOfLength(crosswalk_center, waypoint) > ignore_distance)
continue;
for (auto p : vmap.getDetectionPoints(i).points) {
p.z = waypoint.z;
if (CalcSquareOfLength(p, waypoint) < find_distance) {
vmap.setDetectionCrossWalkID(i);
return num;
}
}
}
}
vmap.setDetectionCrossWalkID(-1);
return -1; // no near crosswalk
}
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 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;
}
}
// display the next waypoint by markers.
static void displayNextWaypoint(int i)
{
visualization_msgs::Marker marker;
marker.header.frame_id = MAP_FRAME;
marker.header.stamp = ros::Time();
marker.ns = "next_waypoint_marker";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position = _current_waypoints.getWaypointPosition(i);
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
marker.frame_locked = true;
_vis_pub.publish(marker);
}
//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;
}
}
}
static EControl vscanDetection()
{
if (_vscan.empty() == true || _closest_waypoint < 0)
return KEEP;
int decelerate_or_stop = -10000;
int decelerate2stop_waypoints = 15;
for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) {
g_obstacle.clearStopPoints();
if (!g_obstacle.isDecided())
g_obstacle.clearDeceleratePoints();
decelerate_or_stop++;
if (decelerate_or_stop > decelerate2stop_waypoints ||
(decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) ||
(decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1))
return DECELERATE;
if (i > _path_dk.getSize() - 1 )
return KEEP;
// Detection for cross walk
if (i == vmap.getDetectionWaypoint()) {
if (CrossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP) {
_obstacle_waypoint = i;
return STOP;
}
}
// waypoint seen by vehicle
geometry_msgs::Point waypoint = calcRelativeCoordinate(_path_dk.getWaypointPosition(i),
_current_pose.pose);
tf::Vector3 tf_waypoint = point2vector(waypoint);
tf_waypoint.setZ(0);
int stop_point_count = 0;
int decelerate_point_count = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
tf::Vector3 vscan_vector((double) item->x, (double) item->y, 0);
// for simulation
if (g_sim_mode) {
tf::Transform transform;
tf::poseMsgToTF(_sim_ndt_pose.pose, transform);
geometry_msgs::Point world2vscan = vector2point(transform * vscan_vector);
vscan_vector = point2vector(calcRelativeCoordinate(world2vscan, _current_pose.pose));
vscan_vector.setZ(0);
}
// 2D distance between waypoint and vscan points(obstacle)
// ---STOP OBSTACLE DETECTION---
double dt = tf::tfDistance(vscan_vector, tf_waypoint);
if (dt < _detection_range) {
stop_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
if (stop_point_count > _threshold_points) {
_obstacle_waypoint = i;
return STOP;
}
// without deceleration range
if (_deceleration_range < 0.01)
continue;
// deceleration search runs "decelerate_search_distance" waypoints from closest
if (i > _closest_waypoint+_deceleration_search_distance || decelerate_or_stop >= 0)
continue;
// ---DECELERATE OBSTACLE DETECTION---
if (dt > _detection_range && dt < _detection_range + _deceleration_range) {
bool count_flag = true;
// search overlaps between DETECTION range and DECELERATION range
for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++) {
if (i+waypoint_search < 0 || i+waypoint_search >= _path_dk.getSize() || !waypoint_search)
continue;
geometry_msgs::Point temp_waypoint = calcRelativeCoordinate(
_path_dk.getWaypointPosition(i+waypoint_search),
_current_pose.pose);
tf::Vector3 waypoint_vector = point2vector(temp_waypoint);
waypoint_vector.setZ(0);
// if there is a overlap, give priority to DETECTION range
if (tf::tfDistance(vscan_vector, waypoint_vector) < _detection_range) {
count_flag = false;
break;
}
}
if (count_flag) {
decelerate_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
}
// found obstacle to DECELERATE
if (decelerate_point_count > _threshold_points) {
_obstacle_waypoint = i;
decelerate_or_stop = 0; // for searching near STOP obstacle
g_obstacle.setDecided(true);
}
}
}
return KEEP; //no obstacles
}
static void DisplayDetectionRange(const int &crosswalk_id, const int &num, const EControl &kind)
{
// set up for marker array
visualization_msgs::MarkerArray marker_array;
visualization_msgs::Marker crosswalk_marker;
visualization_msgs::Marker waypoint_marker_stop;
visualization_msgs::Marker waypoint_marker_decelerate;
visualization_msgs::Marker stop_line;
crosswalk_marker.header.frame_id = "/map";
crosswalk_marker.header.stamp = ros::Time();
crosswalk_marker.id = 0;
crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST;
crosswalk_marker.action = visualization_msgs::Marker::ADD;
waypoint_marker_stop = crosswalk_marker;
waypoint_marker_decelerate = crosswalk_marker;
stop_line = crosswalk_marker;
stop_line.type = visualization_msgs::Marker::CUBE;
// set each namespace
crosswalk_marker.ns = "Crosswalk Detection";
waypoint_marker_stop.ns = "Stop Detection";
waypoint_marker_decelerate.ns = "Decelerate Detection";
stop_line.ns = "Stop Line";
// set scale and color
double scale = 2*_detection_range;
waypoint_marker_stop.scale.x = scale;
waypoint_marker_stop.scale.y = scale;
waypoint_marker_stop.scale.z = scale;
waypoint_marker_stop.color.a = 0.2;
waypoint_marker_stop.color.r = 0.0;
waypoint_marker_stop.color.g = 1.0;
waypoint_marker_stop.color.b = 0.0;
waypoint_marker_stop.frame_locked = true;
scale = 2*(_detection_range + _deceleration_range);
waypoint_marker_decelerate.scale.x = scale;
waypoint_marker_decelerate.scale.y = scale;
waypoint_marker_decelerate.scale.z = scale;
waypoint_marker_decelerate.color.a = 0.15;
waypoint_marker_decelerate.color.r = 1.0;
waypoint_marker_decelerate.color.g = 1.0;
waypoint_marker_decelerate.color.b = 0.0;
waypoint_marker_decelerate.frame_locked = true;
if (_obstacle_waypoint > -1) {
stop_line.pose.position = _path_dk.getWaypointPosition(_obstacle_waypoint);
stop_line.pose.orientation = _path_dk.getWaypointOrientation(_obstacle_waypoint);
}
stop_line.pose.position.z += 1.0;
stop_line.scale.x = 0.1;
stop_line.scale.y = 15.0;
stop_line.scale.z = 2.0;
stop_line.color.a = 0.3;
stop_line.color.r = 1.0;
stop_line.color.g = 0.0;
stop_line.color.b = 0.0;
stop_line.lifetime = ros::Duration(0.1);
stop_line.frame_locked = true;
if (crosswalk_id > 0)
scale = vmap.getDetectionPoints(crosswalk_id).width;
crosswalk_marker.scale.x = scale;
crosswalk_marker.scale.y = scale;
crosswalk_marker.scale.z = scale;
crosswalk_marker.color.a = 0.5;
crosswalk_marker.color.r = 0.0;
crosswalk_marker.color.g = 1.0;
crosswalk_marker.color.b = 0.0;
crosswalk_marker.frame_locked = true;
// set marker points coordinate
for (int i = 0; i < _search_distance; i++) {
if (num < 0 || i+num > _path_dk.getSize() - 1)
break;
geometry_msgs::Point point;
point = _path_dk.getWaypointPosition(num+i);
waypoint_marker_stop.points.push_back(point);
if (i > _deceleration_search_distance)
continue;
waypoint_marker_decelerate.points.push_back(point);
}
if (crosswalk_id > 0) {
for (const auto &p : vmap.getDetectionPoints(crosswalk_id).points)
crosswalk_marker.points.push_back(p);
}
// publish marker
marker_array.markers.push_back(crosswalk_marker);
marker_array.markers.push_back(waypoint_marker_stop);
marker_array.markers.push_back(waypoint_marker_decelerate);
if (kind == STOP)
marker_array.markers.push_back(stop_line);
_range_pub.publish(marker_array);
marker_array.markers.clear();
}
