modules/planning/tasks/traffic_decider/stop_sign.cc
/**
 * @brief: add a watch vehicle which arrives at stop sign ahead of adc
 */
@brief: add a watch vehicle which arrives at stop sign ahead of adc
@brief : add a watch vehicle which arrives at stop sign ahead of adc
int StopSign::AddWatchVehicle(
    const PathObstacle& obstacle,
    StopSignLaneVehicles* watch_vehicles) {
  const std::string& obstacle_id = obstacle.Id();
  const PerceptionObstacle& perception_obstacle =
      obstacle.obstacle()->Perception();
  PerceptionObstacle::Type obstacle_type = perception_obstacle.type();
  std::string obstacle_type_name =
      PerceptionObstacle_Type_Name(obstacle_type);
  auto point = common::util::MakePointENU(
      perception_obstacle.position().x(),
      perception_obstacle.position().y(),
      perception_obstacle.position().z());
  double obstacle_s = 0.0;
  double obstacle_l = 0.0;
  hdmap::LaneInfoConstPtr obstacle_lane;
  const hdmap::HDMap *hdmap = hdmap::HDMapUtil::BaseMapPtr();
  if (hdmap->GetNearestLaneWithHeading(
      point, 5.0, perception_obstacle.theta(), M_PI / 3.0,
      &obstacle_lane, &obstacle_s, &obstacle_l) != 0) {
    ADEBUG << "obstacle_id[" << obstacle_id
        << "] type[" << obstacle_type_name
        << "]: Failed to find nearest lane from map for position: "
        << point.DebugString() << "; heading:" << perception_obstacle.theta();
    return -1;
  }
  std::string obstable_lane_id =  obstacle_lane.get()->id().id();
  auto it = std::find_if(
      associate_lanes_.begin(),
      associate_lanes_.end(),
      [&obstable_lane_id](const LaneInfo* lane) {
          return lane->id().id() == obstable_lane_id; });
  if (it == associate_lanes_.end()) {
    ADEBUG << "obstacle_id[" << obstacle_id
        << "] type[" << obstacle_type_name
        << "] lane_id[" << obstable_lane_id
        << "] not associated with current stop_sign. skip";
    return -1;
  }
  auto speed = std::hypot(perception_obstacle.velocity().x(),
                          perception_obstacle.velocity().y());
  if (speed > FLAGS_stop_min_speed) {
    ADEBUG << "obstacle_id[" << obstacle_id
        << "] type[" << obstacle_type_name
        << "] velocity[" << speed
        << "] not stopped. skip";
    return -1;
  }
  double stop_line_s = 0;
  double obstacle_end_s = obstacle_s + perception_obstacle.length() / 2;
  double distance_to_stop_line = stop_line_s - obstacle_end_s;
  if (distance_to_stop_line > FLAGS_max_valid_stop_distance) {
    ADEBUG << "distance_to_stop_line["
        << distance_to_stop_line
        << "]; stop_line_s" << stop_line_s
        << "]; obstacle_end_s[" << obstacle_end_s
        << "] too far from stop line. skip";
    return -1;
  }
  (*watch_vehicles)[obstacle_lane->id().id()].push_back(obstacle_id);
  return 0;
}
int StopSign :: AddWatchVehicle ( const PathObstacle & obstacle , StopSignLaneVehicles * watch_vehicles ) { const std :: string & obstacle_id = obstacle . Id ( ) ; const PerceptionObstacle & perception_obstacle = obstacle . obstacle ( ) -> Perception ( ) ; PerceptionObstacle :: Type obstacle_type = perception_obstacle . type ( ) ; std :: string obstacle_type_name = PerceptionObstacle_Type_Name ( obstacle_type ) ; auto point = common :: util :: MakePointENU ( perception_obstacle . position ( ) . x ( ) , perception_obstacle . position ( ) . y ( ) , perception_obstacle . position ( ) . z ( ) ) ; double obstacle_s = 0.0 ; double obstacle_l = 0.0 ; hdmap :: LaneInfoConstPtr obstacle_lane ; const hdmap :: HDMap * hdmap = hdmap :: HDMapUtil :: BaseMapPtr ( ) ; if ( hdmap -> GetNearestLaneWithHeading ( point , 5.0 , perception_obstacle . theta ( ) , M_PI / 3.0 , & obstacle_lane , & obstacle_s , & obstacle_l ) != 0 ) { ADEBUG << " " << obstacle_id << " " << obstacle_type_name << " " << point . DebugString ( ) << " " << perception_obstacle . theta ( ) ; return -1 ; } std :: string obstable_lane_id = obstacle_lane . get ( ) -> id ( ) . id ( ) ; auto it = std :: find_if ( associate_lanes_ . begin ( ) , associate_lanes_ . end ( ) , [ & obstable_lane_id ] ( const LaneInfo * lane ) { return lane -> id ( ) . id ( ) == obstable_lane_id ; } ) ; if ( it == associate_lanes_ . end ( ) ) { ADEBUG << " " << obstacle_id << " " << obstacle_type_name << " " << obstable_lane_id << " " ; return -1 ; } auto speed = std :: hypot ( perception_obstacle . velocity ( ) . x ( ) , perception_obstacle . velocity ( ) . y ( ) ) ; if ( speed > FLAGS_stop_min_speed ) { ADEBUG << " " << obstacle_id << " " << obstacle_type_name << " " << speed << " " ; return -1 ; } double stop_line_s = 0 ; double obstacle_end_s = obstacle_s + perception_obstacle . length ( ) / 2 ; double distance_to_stop_line = stop_line_s - obstacle_end_s ; if ( distance_to_stop_line > FLAGS_max_valid_stop_distance ) { ADEBUG << " " << distance_to_stop_line << " " << stop_line_s << " " << obstacle_end_s << " " ; return -1 ; } ( * watch_vehicles ) [ obstacle_lane -> id ( ) . id ( ) ] . push_back ( obstacle_id ) ; return 0 ; }
@brief: add a watch vehicle which arrives at stop sign ahead of adc
@brief : add a watch vehicle which arrives at stop sign ahead of adc
// check obstacle is on an associate lane guarded by stop sign // check stop close enough to stop line of the stop_sign // TODO(all): find stop_line_s of associated stop sign // use a vector since motocycles/bicycles can be more than one