C++ WayPoints类代码示例

/**
 * @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;
}

/**
 * @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;
}

/**
 * @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;
}

/**
 * @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 &currentTarget)
{
	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;
}

/**
 * @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 &currentTarget, 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;
}

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

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

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

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

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