void Player::evoluciona(float fdelta)
{
double delta=fdelta;
point p=vector2point(pos);
point v=vector2point(speed);
point c=vector2point(camaleonPos);
if (not licked) {
p+=v*delta;
pos=point2vector(p);
return;
}
if (not tensioning) {
p+=v*delta;
pos=point2vector(p);
if (prodesc(v,p-c)>=0)
tensioning=true;
return;
}
p-=c;
double modulov=abs(v);
double desp=modulov*delta;
double r=abs(p);
double alfa=desp/r;
int signo=prodvec(p,v)>0?1:-1;
p*=std::polar(1.0,signo*alfa);
v=p*std::polar(1.0,signo*M_PI/2.0);
v=(modulov/abs(v))*v;
p+=c;
pos=point2vector(p);
speed=point2vector(v);
}
waypoint_follower::lane createLaneWaypoint(std::vector<WP> waypoints)
{
waypoint_follower::lane lane_waypoint;
lane_waypoint.header.frame_id = "/map";
lane_waypoint.header.stamp = ros::Time(0);
for (unsigned int i = 0; i < waypoints.size(); i++)
{
waypoint_follower::waypoint wp;
wp.pose.header = lane_waypoint.header;
wp.pose.pose.position = waypoints[i].pose.position;
wp.pose.pose.orientation = waypoints[i].pose.orientation;
wp.twist.header = lane_waypoint.header;
double vel_kmh = decelerate(point2vector(waypoints[i].pose.position),
point2vector(waypoints[waypoints.size() -1 ].pose.position),
waypoints[i].velocity_kmh);
wp.twist.twist.linear.x = kmph2mps(vel_kmh);
lane_waypoint.waypoints.push_back(wp);
}
return lane_waypoint;
}
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
}