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