예제 #1
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;
}
예제 #2
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
}
예제 #3
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();
}