Example #1
0
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
}
Example #2
0
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
}
Example #3
0
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;
}
Example #4
0
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;
  }
}
Example #5
0
// 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);
}
Example #6
0
//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;
    }
  }
}
Example #7
0
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
}
Example #8
0
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();
}