/**
* @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
* All points in the road are updated
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for (auto &ld : laserData)
{
//wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
//qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
int robot = road.getIndexOfNextPoint();
QVec memo = innermodel->transform6D("world", "robot");
for(int it = robot; it<road.size(); ++it)
{
road[it].isVisible = true;
innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one to world RS
//m.print("m");
//pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
//Check if they are inside the laser polygon
for (int i = 0; i < newPoints.nCols(); i++)
{
// qDebug() << __FUNCTION__ << "----------------------------------";
// qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// qDebug() << __FUNCTION__ << polygon;
if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
{
road[it].isVisible = false;
//qFatal("fary");
break;
}
}
// if( road[it].isVisible == false)
// {
// for (int k = it; k < road.size(); ++k)
// road[k].isVisible = false;
// break;
// }
}
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
//road.print();
return true;
}
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 void ChangeWaypoint(EControl detection_result)
{
int obs = _obstacle_waypoint;
if (obs != -1){
std::cout << "====got obstacle waypoint====" << std::endl;
std::cout << "=============================" << std::endl;
}
if (detection_result == STOP){ // STOP for obstacle
// stop_waypoint is about _others_distance meter away from obstacles
int stop_waypoint = obs - ((int)(_others_distance / _path_change.getInterval()));
std::cout << "stop_waypoint: " << stop_waypoint << std::endl;
// change waypoints to stop by the stop_waypoint
_path_change.changeWaypoints(stop_waypoint);
_path_change.avoidSuddenBraking();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
} else if (detection_result == DECELERATE) { // DECELERATE for obstacles
_path_change.setPath(_path_dk.getCurrentWaypoints());
_path_change.setDeceleration();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
} else { // ACELERATE or KEEP
_path_change.setPath(_path_dk.getCurrentWaypoints());
_path_change.avoidSuddenAceleration();
_path_change.avoidSuddenBraking();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
}
return;
}
/**
* @brief Check if any of the waypoints has nan coordinates
*
* @param road ...
* @return bool
*/
bool ElasticBand::checkIfNAN(const WayPoints &road)
{
for (auto it = road.begin(); it != road.end(); ++it)
if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z()))
{
road.print();
return true;
}
return false;
}
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
}
/**
* @brief Stops the robot
*
* @return bool
*/
bool SpecificWorker::stopCommand(CurrentTarget &target, WayPoints &myRoad, TrajectoryState &state)
{
controller->stopTheRobot(omnirobot_proxy);
myRoad.setFinished(true);
myRoad.reset();
myRoad.endRoad();
#ifdef USE_QTGUI
//myRoad.clearDraw(viewer->innerViewer);
//drawGreenBoxOnTarget(target.getTranslation());
#endif
target.reset();
state.setElapsedTime(taskReloj.elapsed());
state.setState("IDLE");
qDebug() << __FUNCTION__ << "Robot at pose:" << innerModel->transform6D("world", "robot");
return true;
}
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;
}
}
/**
* @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS)
*
* @param road ...
* @return void
*/
void ElasticBand::addPoints(WayPoints &road, const CurrentTarget ¤tTarget)
{
int offset = 1;
for (int i = 0; i < road.size() - offset; i++)
{
if (i > 0 and road[i].isVisible == false)
break;
WayPoint &w = road[i];
WayPoint &wNext = road[i + 1];
float dist = (w.pos - wNext.pos).norm2();
if (dist > ROAD_STEP_SEPARATION) //SHOULD GET FROM IM
{
float l = 0.9 * ROAD_STEP_SEPARATION / dist; //Crucial que el punto se ponga mas cerca que la condición de entrada
WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l));
road.insert(i + 1, wNew);
}
}
//ELIMINATED AS REQUESTED BY MANSO
//Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants
//The angle formed by this point and the last one has to be the same es specified in the target
//We solve this equations for (x,z)
// (x' -x)/(z'-z) = tg(a) = t
// sqr(x'-x) + sqr(z'-z) = sqr(r)
// z = z' - (r/(sqrt(t*t -1)))
// x = x' - r(sqrt(1-(1/t*t+1)))
// if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) )
// {
// qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos;
// float radius = 500;
// float ta = tan(currentTarget.getRotation().y());
// float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1)));
// float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1));
// WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) );
// road.insert(road.end()-1,wNew);
// road.last().hasRotation = true;
// qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta;
//
// }
//else
//qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y();
}
/**
* @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Compute distances from robot to all points ahead. If any of them is laser-visible and significantly shorter than de distance along de road, try it!
WayPoints::iterator robot = road.getIterToClosestPointToRobot();
WayPoints::iterator best = road.begin();
for (WayPoints::iterator it = robot + 1; it != road.end(); ++it)
{
//qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it);
if ( it->isVisible )
{
if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() > 300) //Half robot SACARRRR
{
qDebug() << __FUNCTION__ << "Candidato";
//Check if the robot passes through the straight line
if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS))
{
//Is so remove all intermadiate points between robot and new subtarget
qDebug() << __FUNCTION__ << "Confirmado";
best = it;
}
}
}
else
break;
}
if (best != road.begin() and (robot + 1) != road.end())
road.erase(robot + 1, best);
return false;
}
static double getCmdVelocity(int waypoint)
{
if (_param_flag == MODE_DIALOG)
{
ROS_INFO_STREAM("dialog : " << _initial_velocity << " km/h (" << kmph2mps(_initial_velocity) << " m/s )");
return kmph2mps(_initial_velocity);
}
if (_current_waypoints.isEmpty())
{
ROS_INFO_STREAM("waypoint : not loaded path");
return 0;
}
double velocity = _current_waypoints.getWaypointVelocityMPS(waypoint);
//ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )");
return velocity;
}
void BaseWaypointCallback(const waypoint_follower::laneConstPtr &msg)
{
ROS_INFO("subscribed base_waypoint\n");
_path_dk.setPath(*msg);
_path_change.setPath(*msg);
if (_path_flag == false) {
std::cout << "waypoint subscribed" << std::endl;
_path_flag = true;
}
}
/**
* @brief Removes points from the band if two of them are too close, ROBOT_RADIUS/3.
*
* @param road ...
* @return void
*/
void ElasticBand::cleanPoints(WayPoints &road)
{
int i;
int offset = 2;
//if( road.last().hasRotation ) offset = 3; else offset = 2;
for (i = 1; i < road.size() -
offset; i++) // exlude 1 to avoid deleting the nextPoint and the last two to avoid deleting the target rotation
{
if (road[i].isVisible == false)
break;
WayPoint &w = road[i];
WayPoint &wNext = road[i + 1];
float dist = (w.pos - wNext.pos).norm2();
if (dist < ROAD_STEP_SEPARATION / 3.)
{
road.removeAt(i + 1);
}
}
}
bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData,
const CurrentTarget ¤tTarget, uint iter)
{
//qDebug() << __FILE__ << __FUNCTION__ << "road size"<< road.size();
if (road.isFinished() == true)
return false;
/////////////////////////////////////////////
//Tags all points in the road ar visible or blocked, depending on laser visibility. Only visible points are processed in this iteration
/////////////////////////////////////////////
//checkVisiblePoints(innermodel, road, laserData);
/////////////////////////////////////////////
//Check if there is a sudden shortcut to take
/////////////////////////////////////////////
//shortCut(innermodel, road, laserData);
/////////////////////////////////////////////
//Add points to achieve an homogenoeus chain
/////////////////////////////////////////////
addPoints(road, currentTarget);
/////////////////////////////////////////////
//Remove point too close to each other
/////////////////////////////////////////////
cleanPoints(road);
/////////////////////////////////////////////
//Compute the scalar magnitudes
/////////////////////////////////////////////
computeForces(innermodel, road, laserData);
/////////////////////////////////////////////
//Delete half the tail behind, if greater than 6, to release resources
/////////////////////////////////////////////
if (road.getIndexOfClosestPointToRobot() > 6)
{
for (auto it = road.begin(); it != road.begin() + (road.getIndexOfCurrentPoint() / 2); ++it)
road.backList.append(it->pos);
road.erase(road.begin(), road.begin() + (road.getIndexOfCurrentPoint() / 2));
}
return true;
}
void PathVset::changeWaypoints(int stop_waypoint)
{
int i = 0;
int close_waypoint_threshold = 4;
int fill_in_zero = 20;
double changed_vel;
double interval = getInterval();
// change waypoints to decelerate
for (int num = stop_waypoint; num > _closest_waypoint - close_waypoint_threshold; num--){
if (!checkWaypoint(num, "changeWaypoints"))
continue;
changed_vel = sqrt(2.0*_decel*(interval*i)); // sqrt(2*a*x)
//std::cout << "changed_vel[" << num << "]: " << mps2kmph(changed_vel) << " (km/h)";
//std::cout << " distance: " << (_obstacle_waypoint-num)*interval << " (m)";
//std::cout << " current_vel: " << mps2kmph(_current_vel) << std::endl;
waypoint_follower::waypoint initial_waypoint = _path_dk.getCurrentWaypoints().waypoints[num];
if (changed_vel > _velocity_limit || //
changed_vel > initial_waypoint.twist.twist.linear.x){ // avoid acceleration
//std::cout << "too large velocity!!" << std::endl;
current_waypoints_.waypoints[num].twist.twist.linear.x = initial_waypoint.twist.twist.linear.x;
} else {
current_waypoints_.waypoints[num].twist.twist.linear.x = changed_vel;
}
i++;
}
// fill in 0
for (int j = 1; j < fill_in_zero; j++){
if (!checkWaypoint(stop_waypoint+j, "changeWaypoints"))
continue;
current_waypoints_.waypoints[stop_waypoint+j].twist.twist.linear.x = 0.0;
}
std::cout << "---changed waypoints---" << std::endl;
return;
}
// 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);
}
bool
SpecificWorker::gotoCommand(InnerModel *innerModel, CurrentTarget &target, TrajectoryState &state, WayPoints &myRoad,
RoboCompLaser::TLaserData &lData)
{
QTime reloj = QTime::currentTime();
/////////////////////////////////////////////////////
// check for ending conditions
//////////////////////////////////////////////////////
if (myRoad.isFinished() == true)
{
controller->stopTheRobot(omnirobot_proxy);
qDebug() << __FUNCTION__ << "Changing to SETHEADING command";
target.setState(CurrentTarget::State::SETHEADING);
return true;
}
if (myRoad.isBlocked() == true) //Road BLOCKED, go to BLOCKED state and wait it the obstacle moves
{
controller->stopTheRobot(omnirobot_proxy);
//currentTargetBack.setTranslation(innerModel->transform("world", QVec::vec3(0, 0, -250), "robot"));
target.setState(CurrentTarget::State::BLOCKED);
return false;
}
// Get here when robot is stuck
// if(myRoad.requiresReplanning == true)
// {
// //qDebug() << __FUNCTION__ << "STUCK, PLANNING REQUIRED";
// //computePlan(innerModel);
// }
//////////////////////////////////////////
// Check if there is a plan for the target
//////////////////////////////////////////
bool coolPlan = true;
if (target.isWithoutPlan() == true)
{
state.setState("PLANNING");
QVec localT = target.getTranslation();
coolPlan = plannerPRM.computePath(localT, innerModel);
if (coolPlan == false)
{
qDebug() << __FUNCTION__ << "Path NOT found. Resetting to IDLE state";
target.setState(CurrentTarget::State::STOP);
return false;
}
target.setTranslation(localT);
//qDebug() << __FUNCTION__ << "Plan obtained of " << planner->getPath().size() << " points";
// take inner to current values
updateInnerModel(innerModel, state);
target.setWithoutPlan(false);
//Init road REMOVE TRASH FROM HERE
myRoad.reset();
myRoad.readRoadFromList(plannerPRM.getPath());
myRoad.requiresReplanning = false;
myRoad.computeDistancesToNext();
myRoad.startRoad();
state.setPlanningTime(reloj.elapsed());
state.setState("EXECUTING");
}
///////////////////////////////////
// Update the band
/////////////////////////////////
elasticband.update(innerModel, myRoad, laserData, target);
///////////////////////////////////
// compute all measures relating the robot to the road
/////////////////////////////////
myRoad.update();
//myRoad.printRobotState(innerModel, target);
/////////////////////////////////////////////////////
//move the robot according to the current force field
//////////////////////////////////////////////////////
controller->update(innerModel, lData, omnirobot_proxy, myRoad);
#ifdef USE_QTGUI
waypointsdraw.draw(myRoad, viewer, target);
#endif
state.setEstimatedTime(myRoad.getETA());
return true;
}
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();
}
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
}
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
if (road.size() < 3)
return 0;
// To avoid moving the rotation element attached to the last
int lastP;
if (road.last().hasRotation)
lastP = road.size() - 2;
else
lastP = road.size() - 1;
// Go through all points in the road
float totalChange = 0.f;
for (int i = 1; i < lastP; i++)
{
if (road[i].isVisible == false)
break;
WayPoint &w0 = road[i - 1];
WayPoint &w1 = road[i];
WayPoint &w2 = road[i + 1];
// Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature
QVec atractionForce(3);
float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext);
atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos);
//Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs
computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT);
QVec repulsionForce = QVec::zeros(3);
QVec jacobian(3);
// space interval to compute the derivative. Related to to robot's size
float h = DELTA_H;
if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ )
{
jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX,
0,
w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h));
// repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist.
repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist);
}
float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes
float beta = 0.55; //Posibite values between 0.1 and 1 The bigger in magnitude, more separation from obstacles
QVec change = (atractionForce * alpha) + (repulsionForce * beta);
if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z()))
{
road.print();
qDebug() << atractionForce << repulsionForce;
qFatal("change");
}
//Now we remove the tangencial component of the force to avoid recirculation of band points
//QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector();
//QVec nChange = pp * (pp * change);
w1.pos = w1.pos - change;
totalChange = totalChange + change.norm2();
}
return totalChange;
}
bool LineFollower::update(InnerModel *innerModel, RoboCompLaser::TLaserData &laserData, RoboCompOmniRobot::OmniRobotPrx omnirobot_proxy, WayPoints &road)
{
static QTime reloj = QTime::currentTime(); //TO be used for a more accurate control (predictive).
/*static*/ long epoch = 100;
static float lastVadvance = 0.f;
const float umbral = 25.f; //salto maximo de velocidad
static float lastVrot = 0.f;
const float umbralrot = 0.08f; //salto maximo de rotación
//Estimate the space that will be blindly covered and reduce Adv speed to remain within some boundaries
//qDebug() << __FILE__ << __FUNCTION__ << "entering update with" << road.at(road.getIndexOfClosestPointToRobot()).pos;
//Check robot state
if( (road.isFinished() == true ) or (road.requiresReplanning== true) or (road.isLost == true))
{
if( road.isFinished() ) qDebug() << "road finished";
if( road.requiresReplanning ) qDebug() << "requiresReplanning";
if( road.isLost ) qDebug() << "robot is lost";
stopTheRobot(omnirobot_proxy);
return false;
}
///CHECK ROBOT INMINENT COLLISION
float vside = 0;
int j=0;
road.setBlocked(false);
for(auto i : laserData)
{
//printf("laser dist %f || baseOffsets %f \n",i.dist,baseOffsets[j]);
if(i.dist < 10) i.dist = 30000;
if( i.dist < baseOffsets[j] + 50 )
{
if(i.angle>-1.30 && i.angle<1.30){
qDebug() << __FILE__ << __FUNCTION__<< "Robot stopped to avoid collision because distance to obstacle is less than " << baseOffsets[j] << " "<<i.dist << " " << i.angle;
stopTheRobot(omnirobot_proxy);
road.setBlocked(true); //AQUI SE BLOQUEA PARA REPLANIFICAR
qDebug()<<"DETECTADO OBSTACULO, REPLANIFICANDO";
break;
}
}
else
{
if (i.dist < baseOffsets[j] + 150)
{
if (i.angle > 0)
{
vside = -80;
}
else
{
vside = 80;
}
}
}
j++;
}
/////////////////////////////////////////////////
////// CHECK CPU AVAILABILITY
/////////////////////////////////////////////////
if ( time.elapsed() > delay ) //Initial wait in secs so the robot waits for everything is setup. Maybe it could be moved upwards
{
float MAX_ADV_SPEED = 200.f;
float MAX_ROT_SPEED = 0.3;
if( (epoch-100) > 0 ) //Damp max speeds if elapsed time is too long
{
MAX_ADV_SPEED = 200 * exponentialFunction(epoch-100, 200, 0.2);
MAX_ROT_SPEED = 0.3 * exponentialFunction(epoch-100, 200, 0.2);
}
float vadvance = 0;
float vrot = 0;
/////////////////////////////////////////////////
////// ROTATION SPEED
////////////////////////////////////////////////
// VRot is computed as the sum of three terms: angle with tangent to road + atan(perp. distance to road) + road curvature
// as descirbed in Thrun's paper on DARPA challenge
vrot = road.getAngleWithTangentAtClosestPoint() + atan( road.getRobotPerpendicularDistanceToRoad()/800.) + 0.8 * road.getRoadCurvatureAtClosestPoint() ; //350->800.
// Limiting filter
if( vrot > MAX_ROT_SPEED )
vrot = MAX_ROT_SPEED;
if( vrot < -MAX_ROT_SPEED )
vrot = -MAX_ROT_SPEED;
/////////////////////////////////////////////////
////// ADVANCE SPEED
////////////////////////////////////////////////
// Factor to be used in speed control when approaching the end of the road
float teta;
if( road.getRobotDistanceToTarget() < 1000)
teta = exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1);
else
teta= 1;
// Factor to be used in speed control when approaching the end of the road
//VAdv is computed as a reduction of MAX_ADV_SPEED by three computed functions:
// * road curvature reduces forward speed
// * VRot reduces forward speed
// * reduction is 1 if there are not obstacle.
// * teta that applies when getting close to the target (1/roadGetCurvature)
// * a Delta that takes 1 if approaching the target is true, 0 otherwise. It applies only if at less than 1000m to the target
vadvance = MAX_ADV_SPEED * exp(-fabs(1.6 * road.getRoadCurvatureAtClosestPoint()))
* exponentialFunction(vrot, 0.8, 0.01)
* teta;
//* exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1)
//* sunk;
if(fabs(vrot - lastVrot) > umbralrot)
{
//qDebug()<<"lastrot "<<lastVrot << "\n vrot "<< vrot;
if(vrot > lastVrot)
vrot = lastVrot + umbralrot;
else vrot = lastVrot - umbralrot;
}
lastVrot=vrot;
//Pre-limiting filter to avoid displacements in very closed turns
if( fabs(vrot) == 0.3)
vadvance = 0;
vside = 0;
//stopping speed jump
if(fabs(vadvance - lastVadvance) > umbral)
{
//qDebug()<<"lastadvanced "<<lastVadvance << "\n vadvance "<< vadvance;
if(vadvance > lastVadvance)
vadvance = lastVadvance + umbral;
else vadvance = lastVadvance - umbral;
}
lastVadvance=vadvance;
// Limiting filter
if( vadvance > MAX_ADV_SPEED )
vadvance = MAX_ADV_SPEED;
//vside = vrot*MAX_ADV_SPEED;
/////////////////////////////////////////////////
////// LOWEST-LEVEL COLLISION AVOIDANCE CONTROL
////////////////////////////////////////////////
//bool collision = avoidanceControl(innerModel, laserData, vadvance, vrot);
// if( collision )
// road.setBlocked(true);
/////////////////////////////////////////////////
/// SIDEWAYS LASTMINUTE AVOIDING WITH THE OMNI BASE
/////////////////////////////////////////////////
//TODO: PROBAR EN URSUS A VER COMO QUEDA..
// std::sort(laserData.begin(), laserData.end(), [](auto a, auto b){ return a.dist < b.dist;});
// if(laserData.front().dist > 300 && vside == 0)// and fabs(laserData.front().angle)>0.3)
// {
// if( laserData.front().angle > 0) vside = -30;
// else vside = 30;
// }
/////////////////////////////////////////////////
////// EXECUTION
////////////////////////////////////////////////
// qDebug() << "------------------LineFollower Report ---------------;";
// qDebug() <<" VAdv: " << vadvance << "|\nVRot: " << vrot << "\nVSide: " << vside;
// qDebug() << "---------------------------------------------------;";
try { omnirobot_proxy->setSpeedBase(vside, vadvance, vrot);}
catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; }
}
else //Too long delay. Stopping robot.
{
qDebug() << __FILE__ << __FUNCTION__ << "Processing delay" << epoch << "ms. too high. Stopping the robot for safety";
try { omnirobot_proxy->setSpeedBase( 0, 0, 0); }
catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; }
}
epoch = reloj.restart(); //epoch time in ms
return false;
}
//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 void WayPointCallback(const waypoint_follower::laneConstPtr &msg)
{
_current_waypoints.setPath(*msg);
_waypoint_set = true;
//ROS_INFO_STREAM("waypoint subscribed");
}
