apollo::planning::util 命名空间参考

函数
int	BuildStopDecision (const std::string &stop_wall_id, const double stop_line_s, const double stop_distance, const StopReasonCode &stop_reason_code, const std::vector< std::string > &wait_for_obstacles, const std::string &decision_tag, Frame *const frame, ReferenceLineInfo *const reference_line_info, double stop_wall_width)
int	BuildStopDecision (const std::string &stop_wall_id, const std::string &lane_id, const double lane_s, const double stop_distance, const StopReasonCode &stop_reason_code, const std::vector< std::string > &wait_for_obstacles, const std::string &decision_tag, Frame *const frame, ReferenceLineInfo *const reference_line_info)
std::pair< double, double >	WorldCoordToObjCoord (std::pair< double, double > input_world_coord, std::pair< double, double > obj_world_coord, double obj_world_angle)
double	WorldAngleToObjAngle (double input_world_angle, double obj_world_angle)
bool	IsVehicleStateValid (const VehicleState &vehicle_state)
bool	IsDifferentRouting (const PlanningCommand &first, const PlanningCommand &second)
double	GetADCStopDeceleration (apollo::common::VehicleStateProvider *vehicle_state, const double adc_front_edge_s, const double stop_line_s)
bool	CheckStopSignOnReferenceLine (const ReferenceLineInfo &reference_line_info, const std::string &stop_sign_overlap_id)
bool	CheckTrafficLightOnReferenceLine (const ReferenceLineInfo &reference_line_info, const std::string &traffic_light_overlap_id)
bool	CheckInsideJunction (const ReferenceLineInfo &reference_line_info)
void	GetFilesByPath (const boost::filesystem::path &path, std::vector< std::string > *files)
double	CalculateEquivalentEgoWidth (const ReferenceLineInfo &reference_line_info, double s, bool *is_left)
double	CalculateEquivalentEgoWidth (const apollo::hdmap::LaneInfoConstPtr lane_info, double s, bool *is_left)
bool	left_arc_bound_with_heading (double delta_x, double r, double heading, double *result)
bool	right_arc_bound_with_heading (double delta_x, double r, double heading, double *result)
bool	left_arc_bound_with_heading_with_reverse_kappa (double delta_x, double r, double heading, double kappa, double *result)
bool	right_arc_bound_with_heading_with_reverse_kappa (double delta_x, double r, double heading, double kappa, double *result)

函数说明

BuildStopDecision() [1/2]

int apollo::planning::util::BuildStopDecision	(	const std::string &	stop_wall_id,
		const double	stop_line_s,
		const double	stop_distance,
		const StopReasonCode &	stop_reason_code,
		const std::vector< std::string > &	wait_for_obstacles,
		const std::string &	decision_tag,
		Frame *const	frame,
		ReferenceLineInfo *const	reference_line_info,
		double	stop_wall_width
	)

在文件 common.cc 第 28 行定义.

                                              {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
 
  // check
  const auto& reference_line = reference_line_info->reference_line();
  if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
    AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
    return 0;
  }
 
  // create virtual stop wall
  const auto* obstacle =
      frame->CreateStopObstacle(reference_line_info, stop_wall_id,
                                stop_line_s, stop_wall_width);
  if (!obstacle) {
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
  }
  const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
  if (!stop_wall) {
    AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
    return -1;
  }
 
  // build stop decision
  const double stop_s = stop_line_s - stop_distance;
  const auto& stop_point = reference_line.GetReferencePoint(stop_s);
  const double stop_heading =
      reference_line.GetReferencePoint(stop_s).heading();
 
  ObjectDecisionType stop;
  auto* stop_decision = stop.mutable_stop();
  stop_decision->set_reason_code(stop_reason_code);
  stop_decision->set_distance_s(-stop_distance);
  stop_decision->set_stop_heading(stop_heading);
  stop_decision->mutable_stop_point()->set_x(stop_point.x());
  stop_decision->mutable_stop_point()->set_y(stop_point.y());
  stop_decision->mutable_stop_point()->set_z(0.0);
 
  for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
    stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
  }
 
  auto* path_decision = reference_line_info->path_decision();
  path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
 
  return 0;
}

BuildStopDecision() [2/2]

int apollo::planning::util::BuildStopDecision	(	const std::string &	stop_wall_id,
		const std::string &	lane_id,
		const double	lane_s,
		const double	stop_distance,
		const StopReasonCode &	stop_reason_code,
		const std::vector< std::string > &	wait_for_obstacles,
		const std::string &	decision_tag,
		Frame *const	frame,
		ReferenceLineInfo *const	reference_line_info
	)

在文件 common.cc 第 84 行定义.

                                                                    {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
 
  const auto& reference_line = reference_line_info->reference_line();
 
  // create virtual stop wall
  const auto* obstacle =
      frame->CreateStopObstacle(stop_wall_id, lane_id, lane_s);
  if (!obstacle) {
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
  }
 
  const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
  if (!stop_wall) {
    AERROR << "Failed to create obstacle for: " << stop_wall_id;
    return -1;
  }
 
  const auto& stop_wall_box = stop_wall->PerceptionBoundingBox();
  if (!reference_line.IsOnLane(stop_wall_box.center())) {
    ADEBUG << "stop point is not on lane. SKIP STOP decision";
    return 0;
  }
 
  // build stop decision
  auto stop_point = reference_line.GetReferencePoint(
      stop_wall->PerceptionSLBoundary().start_s() - stop_distance);
 
  ObjectDecisionType stop;
  auto* stop_decision = stop.mutable_stop();
  stop_decision->set_reason_code(stop_reason_code);
  stop_decision->set_distance_s(-stop_distance);
  stop_decision->set_stop_heading(stop_point.heading());
  stop_decision->mutable_stop_point()->set_x(stop_point.x());
  stop_decision->mutable_stop_point()->set_y(stop_point.y());
  stop_decision->mutable_stop_point()->set_z(0.0);
 
  auto* path_decision = reference_line_info->path_decision();
  path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
 
  return 0;
}

CalculateEquivalentEgoWidth() [1/2]

double apollo::planning::util::CalculateEquivalentEgoWidth	(	const apollo::hdmap::LaneInfoConstPtr	lane_info,
		double	s,
		bool *	is_left
	)

在文件 util.cc 第 204 行定义.

                                                                          {
  const auto& vehicle_param =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  double max_kappa = 1.0 / vehicle_param.min_turn_radius();
  double half_wb = 0.5 * vehicle_param.wheel_base();
  double front_l = vehicle_param.front_edge_to_center() - half_wb;
  double half_w = 0.5 * vehicle_param.width();
  double current_heading = lane_info->Heading(s);
  double heading_f = lane_info->Heading(s + front_l);
  double heading_b = lane_info->Heading(s - half_wb);
  // average kappa from front edge to center
  double kappa_f =
      apollo::common::math::NormalizeAngle(heading_f - current_heading) /
      front_l;
  // average kappa from center to rear axle
  double kappa_b =
      apollo::common::math::NormalizeAngle(current_heading - heading_b) /
      half_wb;
  *is_left = (kappa_b < 0.0);
  // prevent devide by 0, and quit if lane bends to difference direction
  if (kappa_f * kappa_b < 0.0 || fabs(kappa_f) < 1e-6) {
    return half_w;
  }
 
  kappa_f = std::min(fabs(kappa_f), max_kappa);
  kappa_b = std::min(fabs(kappa_b), max_kappa);
  double theta = apollo::common::math::Vec2d(1.0, half_wb * kappa_b).Angle();
  double sint = std::sin(theta);
  double cost = std::cos(theta);
  double r_f = 1.0 / kappa_f;
  double eq_half_w =
      apollo::common::math::Vec2d(front_l * cost - half_w * sint,
                                  r_f + front_l * sint + half_w * cost)
          .Length() -
      r_f;
  return std::max(eq_half_w, half_w);
}

CalculateEquivalentEgoWidth() [2/2]

double apollo::planning::util::CalculateEquivalentEgoWidth	(	const ReferenceLineInfo &	reference_line_info,
		double	s,
		bool *	is_left
	)

在文件 util.cc 第 160 行定义.

                                                            {
  const auto& vehicle_param =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  double max_kappa = 1.0 / vehicle_param.min_turn_radius();
  double half_wb = 0.5 * vehicle_param.wheel_base();
  double front_l = vehicle_param.front_edge_to_center() - half_wb;
  double half_w = 0.5 * vehicle_param.width();
  double current_heading =
      reference_line_info.reference_line().GetReferencePoint(s).heading();
  double heading_f = reference_line_info.reference_line()
                         .GetReferencePoint(s + front_l)
                         .heading();
  double heading_b = reference_line_info.reference_line()
                         .GetReferencePoint(s - half_wb)
                         .heading();
  // average kappa from front edge to center
  double kappa_f =
      apollo::common::math::NormalizeAngle(heading_f - current_heading) /
      front_l;
  // average kappa from center to rear axle
  double kappa_b =
      apollo::common::math::NormalizeAngle(current_heading - heading_b) /
      half_wb;
  *is_left = (kappa_b < 0.0);
  // prevent devide by 0, and quit if lane bends to difference direction
  if (kappa_f * kappa_b < 0.0 || fabs(kappa_f) < 1e-6) {
    return half_w;
  }
 
  kappa_f = std::min(fabs(kappa_f), max_kappa);
  kappa_b = std::min(fabs(kappa_b), max_kappa);
  double theta = apollo::common::math::Vec2d(1.0, half_wb * kappa_b).Angle();
  double sint = std::sin(theta);
  double cost = std::cos(theta);
  double r_f = 1.0 / kappa_f;
  double eq_half_w =
      apollo::common::math::Vec2d(front_l * cost - half_w * sint,
                                  r_f + front_l * sint + half_w * cost)
          .Length() -
      r_f;
  return std::max(eq_half_w, half_w);
}

CheckInsideJunction()

bool apollo::planning::util::CheckInsideJunction

(

const ReferenceLineInfo &

reference_line_info

)

在文件 util.cc 第 115 行定义.

                                                                       {
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
 
  hdmap::PathOverlap junction_overlap;
  reference_line_info.GetJunction(adc_front_edge_s, &junction_overlap);
  if (junction_overlap.object_id.empty()) {
    return false;
  }
 
  static constexpr double kIntersectionPassDist = 2.0;  // unit: m
  const double distance_adc_pass_intersection =
      adc_back_edge_s - junction_overlap.end_s;
  ADEBUG << "distance_adc_pass_intersection[" << distance_adc_pass_intersection
         << "] junction_overlap[" << junction_overlap.object_id << "] start_s["
         << junction_overlap.start_s << "]";
 
  return distance_adc_pass_intersection < kIntersectionPassDist;
}

CheckStopSignOnReferenceLine()

bool apollo::planning::util::CheckStopSignOnReferenceLine	(	const ReferenceLineInfo &	reference_line_info,
		const std::string &	stop_sign_overlap_id
	)

在文件 util.cc 第 84 行定义.

                                                                         {
  const std::vector<PathOverlap>& stop_sign_overlaps =
      reference_line_info.reference_line().map_path().stop_sign_overlaps();
  auto stop_sign_overlap_it =
      std::find_if(stop_sign_overlaps.begin(), stop_sign_overlaps.end(),
                   [&stop_sign_overlap_id](const PathOverlap& overlap) {
                     return overlap.object_id == stop_sign_overlap_id;
                   });
  return (stop_sign_overlap_it != stop_sign_overlaps.end());
}

CheckTrafficLightOnReferenceLine()

bool apollo::planning::util::CheckTrafficLightOnReferenceLine	(	const ReferenceLineInfo &	reference_line_info,
		const std::string &	traffic_light_overlap_id
	)

在文件 util.cc 第 99 行定义.

                                               {
  const std::vector<PathOverlap>& traffic_light_overlaps =
      reference_line_info.reference_line().map_path().signal_overlaps();
  auto traffic_light_overlap_it =
      std::find_if(traffic_light_overlaps.begin(), traffic_light_overlaps.end(),
                   [&traffic_light_overlap_id](const PathOverlap& overlap) {
                     return overlap.object_id == traffic_light_overlap_id;
                   });
  return (traffic_light_overlap_it != traffic_light_overlaps.end());
}

GetADCStopDeceleration()

double apollo::planning::util::GetADCStopDeceleration	(	apollo::common::VehicleStateProvider *	vehicle_state,
		const double	adc_front_edge_s,
		const double	stop_line_s
	)

在文件 util.cc 第 58 行定义.

                                                             {
  double adc_speed = vehicle_state->linear_velocity();
  const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
                                        ->GetConfig()
                                        .vehicle_param()
                                        .max_abs_speed_when_stopped();
  if (adc_speed < max_adc_stop_speed) {
    return 0.0;
  }
 
  double stop_distance = 0;
 
  if (stop_line_s > adc_front_edge_s) {
    stop_distance = stop_line_s - adc_front_edge_s;
  }
  if (stop_distance < 1e-5) {
    return std::numeric_limits<double>::max();
  }
  return (adc_speed * adc_speed) / (2 * stop_distance);
}

GetFilesByPath()

void apollo::planning::util::GetFilesByPath	(	const boost::filesystem::path &	path,
		std::vector< std::string > *	files
	)

在文件 util.cc 第 138 行定义.

                                                 {
  ACHECK(files);
  if (!boost::filesystem::exists(path)) {
    return;
  }
  if (boost::filesystem::is_regular_file(path)) {
    AINFO << "Found record file: " << path.c_str();
    files->push_back(path.c_str());
    return;
  }
  if (boost::filesystem::is_directory(path)) {
    for (auto& entry : boost::make_iterator_range(
             boost::filesystem::directory_iterator(path), {})) {
      GetFilesByPath(entry.path(), files);
    }
  }
}

IsDifferentRouting()

bool apollo::planning::util::IsDifferentRouting	(	const PlanningCommand &	first,
		const PlanningCommand &	second
	)

在文件 util.cc 第 46 行定义.

                                                       {
  if (first.has_header() && second.has_header()) {
    const auto& first_header = first.header();
    const auto& second_header = second.header();
    return (first_header.sequence_num() != second_header.sequence_num() ||
            first_header.module_name() != second_header.module_name() ||
            first_header.timestamp_sec() != second_header.timestamp_sec());
  }
  return true;
}

IsVehicleStateValid()

bool apollo::planning::util::IsVehicleStateValid

(

const VehicleState &

vehicle_state

)

在文件 util.cc 第 35 行定义.

                                                            {
  if (std::isnan(vehicle_state.x()) || std::isnan(vehicle_state.y()) ||
      std::isnan(vehicle_state.z()) || std::isnan(vehicle_state.heading()) ||
      std::isnan(vehicle_state.kappa()) ||
      std::isnan(vehicle_state.linear_velocity()) ||
      std::isnan(vehicle_state.linear_acceleration())) {
    return false;
  }
  return true;
}

left_arc_bound_with_heading()

bool apollo::planning::util::left_arc_bound_with_heading	(	double	delta_x,
		double	r,
		double	heading,
		double *	result
	)

在文件 util.cc 第 243 行定义.

                                                 {
  // calculate △L(positive or negative) with an arc with given radius, and given
  // init_heading the circle can be written as (x-Rsin)^2 + (y+Rcos)^2 = R^2 the
  // upper side of the circel = (sqrt(R^2 - (x-Rsin)^2) - Rcos) is what we need
  if (delta_x > r * (1.0 + std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::lowest();
    return false;
  }
 
  *result = std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2)) -
            r * std::cos(heading);
  return true;
}

left_arc_bound_with_heading_with_reverse_kappa()

bool apollo::planning::util::left_arc_bound_with_heading_with_reverse_kappa	(	double	delta_x,
		double	r,
		double	heading,
		double	kappa,
		double *	result
	)

在文件 util.cc 第 272 行定义.

                                                                    {
  // calculate △L(positive or negative) with an arc with given radius, and given
  // init_heading the circle can be written as (x-Rsin)^2 + (y+Rcos)^2 = R^2 the
  // upper side of the circel = (sqrt(R^2 - (x-Rsin)^2) - Rcos) is what we need
  if (heading > 0 || kappa < 0 ||
      delta_x > r * (1.0 - std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::lowest();
    return false;
  }
  if (delta_x < -r * std::sin(heading)) {
    *result = r * std::cos(heading) -
              std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2));
  } else {
    *result = std::sqrt(r * r - std::pow(delta_x + r * std::sin(heading), 2)) -
              r * (2 - std::cos(heading));
  }
  return true;
}

right_arc_bound_with_heading()

bool apollo::planning::util::right_arc_bound_with_heading	(	double	delta_x,
		double	r,
		double	heading,
		double *	result
	)

在文件 util.cc 第 258 行定义.

                                                  {
  // calculate △L(positive or negative) with an arc with given radius, and given
  // init_heading the circle can be written as (x+Rsin)^2 + (y-Rcos)^2 = R^2 the
  // upper side of the circel = (Rcos - sqrt(R^2 - (x+Rsin)^2) )is what we need
  if (delta_x > r * (1.0 - std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::max();
    return false;
  }
  *result = r * std::cos(heading) -
            std::sqrt(r * r - std::pow(delta_x + r * std::sin(heading), 2));
  return true;
}

right_arc_bound_with_heading_with_reverse_kappa()

bool apollo::planning::util::right_arc_bound_with_heading_with_reverse_kappa	(	double	delta_x,
		double	r,
		double	heading,
		double	kappa,
		double *	result
	)

在文件 util.cc 第 294 行定义.

                                                                     {
  // calculate △L(positive or negative) with an arc with given radius, and given
  // init_heading the circle can be written as (x+Rsin)^2 + (y-Rcos)^2 = R^2 the
  // upper side of the circel = (Rcos - sqrt(R^2 - (x+Rsin)^2) )is what we need
  if (heading < 0 || kappa > 0 ||
      delta_x > r * (1.0 - std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::max();
    return false;
  }
  if (delta_x < r * std::sin(heading)) {
    *result = std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2)) -
              r * std::cos(heading);
  } else {
    *result = r * (2 - std::cos(heading)) -
              std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2));
  }
  return true;
}

WorldAngleToObjAngle()

double apollo::planning::util::WorldAngleToObjAngle	(	double	input_world_angle,
		double	obj_world_angle
	)

在文件 math_util.cc 第 37 行定义.

                                                                              {
  return common::math::NormalizeAngle(input_world_angle - obj_world_angle);
}

WorldCoordToObjCoord()

std::pair< double, double > apollo::planning::util::WorldCoordToObjCoord	(	std::pair< double, double >	input_world_coord,
		std::pair< double, double >	obj_world_coord,
		double	obj_world_angle
	)

在文件 math_util.cc 第 26 行定义.

                                                                     {
  double x_diff = input_world_coord.first - obj_world_coord.first;
  double y_diff = input_world_coord.second - obj_world_coord.second;
  double rho = std::sqrt(x_diff * x_diff + y_diff * y_diff);
  double theta = std::atan2(y_diff, x_diff) - obj_world_angle;
 
  return std::make_pair(std::cos(theta) * rho, std::sin(theta) * rho);
}