//generate the locus of pure pursuit
static std::vector<geometry_msgs::Point> generateTrajectoryCircle(geometry_msgs::Point target)
{
std::vector<geometry_msgs::Point> traj_circle_array;
double radius = calcRadius(target);
for (double i = 0; i < M_PI / 2; i += 0.1)
{
//calc a point of circumference
geometry_msgs::Point p;
p.x = radius * cos(i);
p.y = radius * sin(i);
//transform to (radius,0)
geometry_msgs::Point relative_p;
relative_p.x = p.x - radius;
relative_p.y = p.y;
//rotate -90°
geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90);
//transform to vehicle plane
geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose);
traj_circle_array.push_back(tf_p);
}
//reverse vector
std::reverse(traj_circle_array.begin(), traj_circle_array.end());
for (double i = 0; i > (-1) * M_PI / 2; i -= 0.1)
{
//calc a point of circumference
geometry_msgs::Point p;
p.x = radius * cos(i);
p.y = radius * sin(i);
//transform to (radius,0)
geometry_msgs::Point relative_p;
relative_p.x = p.x - radius;
relative_p.y = p.y;
//rotate -90°
geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90);
//transform to vehicle plane
geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose);
traj_circle_array.push_back(tf_p);
}
return traj_circle_array;
}
static EControl CrossWalkDetection(const int &crosswalk_id)
{
double search_radius = vmap.getDetectionPoints(crosswalk_id).width / 2;
// Search each calculated points in the crosswalk
for (const auto &p : vmap.getDetectionPoints(crosswalk_id).points) {
geometry_msgs::Point detection_point = calcRelativeCoordinate(p, _current_pose.pose);
if (g_sim_mode)
detection_point = calcRelativeCoordinate(p, _sim_ndt_pose.pose);
tf::Vector3 detection_vector = point2vector(detection_point);
detection_vector.setZ(0.0);
int stop_count = 0; // the number of points in the detection area
for (const auto &vscan : _vscan) {
tf::Vector3 vscan_vector(vscan.x, vscan.y, 0.0);
double distance = tf::tfDistance(vscan_vector, detection_vector);
if (distance < search_radius) {
stop_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = vscan.x;
vscan_temp.y = vscan.y;
vscan_temp.z = vscan.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_count > _threshold_points)
return STOP;
}
g_obstacle.clearStopPoints();
}
return KEEP; // find no obstacles
}
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
}
