autoware.universe源码略读(3.13)--perception:map_based_prediction

Overview

这个模块看起来是一个根据地图形状和周围环境预测其他车辆和行人未来路径（及其概率）的模块。 这里虽然只分成了两个文件：path_generator和map_based_prediction_node，但是，这第二个文件有1000多行。。所以涉及到的内容还是特别特别多的。最新的版本已经把这个模块删除掉了，所以只能参考README文件来了解一下大概的设计思路了map_based_prediction
当然，这里必不可少地使用到了lanelet2库，所以代码读起来还很吃力，流程图是下面这个流程

PathGenerator

生成路径所用到的类

（Struct）FrenetPoint

作为测绘人，见到过那么多坐标系，但是这个Frenet坐标系确实是第一次听说，总结下来这就是一个更直观的方式表示道路位置的方式，其中参考线可以是曲线的。这里不多这个坐标系做过多的介绍，可以参考

1. 科普：Frenet坐标
2. Frenet坐标系相关知识系统学习

所以这个结构体里，s相关的就是纵向，也就是相对于起始点的信息；而d就是横向的信息

（Class Constructor）PathGenerator::PathGenerator

构造函数，这里单纯的设置了几个成员变量的值，没有什么好说的。

（mFunc）PathGenerator::generatePathForNonVehicleObject

这里返回的就是redictedPath，即预测生成的路径。这里针对的是非车辆对象，在autoware里针对非车辆对象是只进行直线的预测，所以这里进一步调用了generateStraightPath函数来生成路径

（mFunc）PathGenerator::generatePathToTargetPoint

这里生成的是到目标点的路线，看起来也是针对行人的？ 这里先规划了一条直线，然后根据速度来计算一个arrival_time

const Eigen::Vector2d pedestrian_to_entry_point(point.x() - obj_pos.x, point.y() - obj_pos.y);
const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;

然后根据采样间隔和速度，依次对路线中间的点进行采样，生成predicted_path，最后这个变量里包含的是路径path， 置信度confidence和时间间隔time_step

（mFunc）PathGenerator::generatePathForCrosswalkUser

这个是针对人行横道类型的路线规划，所以输入的有const EntryPoint & reachable_crosswalk这个人行道的端点，这里也是先生成了一条直线，具体是行人到端点的一条直线

const Eigen::Vector2d pedestrian_to_entry_point(reachable_crosswalk.first.x() - obj_pos.x, reachable_crosswalk.first.y() - obj_pos.y);
const Eigen::Vector2d entry_to_exit_point(	// 两个端点之间的连线reachable_crosswalk.second.x() - reachable_crosswalk.first.x(),reachable_crosswalk.second.y() - reachable_crosswalk.first.y());
const auto velocity = std::max(std::hypot(obj_vel.x, obj_vel.y), min_crosswalk_user_velocity_);
const auto arrival_time = pedestrian_to_entry_point.norm() / velocity;

这里还分成了第二段，第二段就是人行道上一个端点到另一个端点的过程，这里其实还要和人行道能不能通行相关吧？不过因为这里只是路线的预测，所以就还没有联动，只是单纯规划一下路线。 第二段就挺简单了，直接两个端点连线计算每个采样点的位置

world_frame_pose.position.x =reachable_crosswalk.first.x() +velocity * entry_to_exit_point.normalized().x() * (dt - arrival_time);
world_frame_pose.position.y =reachable_crosswalk.first.y() +velocity * entry_to_exit_point.normalized().y() * (dt - arrival_time);
world_frame_pose.position.z = obj_pos.z;
world_frame_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation;
predicted_path.path.push_back(world_frame_pose);

（mFunc）PathGenerator::generatePathForLowSpeedVehicle

这里是针对低速车辆的路线预测，不过这里一直在push_back的值不是不变的么。。所以低速情况就默认不动了？

（mFunc）PathGenerator::generatePathForOffLaneVehicle

对车道线外的车辆的路径预测，也是直接生成了直线

return generateStraightPath(object);

（mFunc）PathGenerator::generatePathForOnLaneVehicle

对车道线上的车辆的路线预测就稍微复杂一些了，这里首先要确保有参考线的轨迹，也就是const PosePath & ref_paths，如果有的话，调用

return generatePolynomialPath(object, ref_paths);

（mFunc）PathGenerator::generateStraightPath

这个也很简单，就根据速度和设置的时间间隔，直接乘就好了

const auto future_obj_pose = tier4_autoware_utils::calcOffsetPose(object_pose, object_twist.linear.x * dt, object_twist.linear.y * dt, 0.0);

（mFunc）PathGenerator::generatePolynomialPath

这里涉及到了车道线上的路线规划了，首先就是把对象转换到Frenet坐标系下

const double ref_path_len = motion_utils::calcArcLength(ref_path);
const auto current_point = getFrenetPoint(object, ref_path);

然后调用generateFrenetPath生成在Frenet坐标系下的路线

const auto frenet_predicted_path =generateFrenetPath(current_point, terminal_point, ref_path_len);

然后就是对这个路径插值，并且在满足条件（至少有两个点）的情况下转换成预测路径的类型

const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path);
if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) {const PredictedPath empty_path;return empty_path;
}
// Step4. Convert predicted trajectory from Frenet to Cartesian coordinate
return convertToPredictedPath(object, frenet_predicted_path, interpolated_ref_path);

（mFunc）PathGenerator::generateFrenetPath

这里生成的就是Frenet坐标系下的预测轨迹，因为这个不再是直线了，所以就先生成了多项式系数

const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration);
const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration);

不过这里我们可以看到，target_point输入的时候其实s和d都是0的，之后就是根据系数和时间，来分别计算s和d的值

const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) +lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) +lat_coeff(2) * std::pow(t, 5);
const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) +lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4);

至于这里为什么这么算，也可以参考 Frenet坐标系相关知识系统学习 这篇文章

（mFunc）PathGenerator::calcLatCoefficients

计算横向多项式系数的，关于这里为什么这么计算，还没有特别理解，找到了一篇文章
路径规划——曲线拟合详解（一）：多项式轨迹与QP优化（minimum-snap算法核心部分）
不过还是没太看明白具体是怎么算的

（mFunc）PathGenerator::calcLonCoefficients

生成径向多项式系数的，和上边那个一样

（mFunc）PathGenerator::interpolateReferencePath

这是根据参考路线生成插值路线的函数，输入的两个参数，base_path就是输入的参考路线，也就是之前的ref_path；而frenet_predicted_path则是生成的Frenet坐标系下的预测路径
这里先对ref_path进行处理，分别提取出xyz和s

for (size_t i = 0; i < base_path.size(); ++i) {base_path_x.push_back(base_path.at(i).position.x);base_path_y.push_back(base_path.at(i).position.y);base_path_z.push_back(base_path.at(i).position.z);base_path_s.push_back(motion_utils::calcSignedArcLength(base_path, 0, i));
}

然后是分别对x,y,z三个方向进行了样条插值，里面用到了common包中的函数autoware-common

// Spline Interpolation
std::vector<double> spline_ref_path_x =interpolation::spline(base_path_s, base_path_x, resampled_s);
std::vector<double> spline_ref_path_y =interpolation::spline(base_path_s, base_path_y, resampled_s);
std::vector<double> spline_ref_path_z =interpolation::spline(base_path_s, base_path_z, resampled_s);

最后就是把插值生成后的点依次放进路径就好了

（mFunc）PathGenerator::convertToPredictedPath

这个是把Frenet坐标系下路线转换成世界坐标系下预测路线的函数，核心的一行是

auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0);

（mFunc）PathGenerator::getFrenetPoint

最后这里是把坐标转换至Frenet坐标系下的函数，这里先找到参考路线上里对象最近的点

const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(ref_path, obj_point);

然后l计算的是径向的偏移量，也是用到了common包中的函数autoware-common

const double l =motion_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point);

然后就是根据yaw角信息以及速度信息等来分别计算s和d的值了

const float vx = static_cast<float>(object.kinematics.twist_with_covariance.twist.linear.x);
const float vy = static_cast<float>(object.kinematics.twist_with_covariance.twist.linear.y);
const float obj_yaw =static_cast<float>(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
const float lane_yaw =static_cast<float>(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation));
const float delta_yaw = obj_yaw - lane_yaw;
frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l;
frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point);
frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw);
frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw);
frenet_point.s_acc = 0.0;
frenet_point.d_acc = 0.0;

map_based_prediction_node

这里面的内容太多了，还是分成小块一个部分一个部分看吧

（Struct）ObjectData

检测到的对象的结构体

struct ObjectData
{std_msgs::msg::Header header;lanelet::ConstLanelets current_lanelets;          // lanelets that the object is currently onlanelet::ConstLanelets future_possible_lanelets;  // lanelets that the object may enter in the futuregeometry_msgs::msg::Pose pose;geometry_msgs::msg::Twist twist;double time_delay;
};

（enum）Maneuver

下一步行动的类型，分成了三种

enum class Maneuver {LANE_FOLLOW = 0,        // 继续这个车道走LEFT_LANE_CHANGE = 1,   // 左变道RIGHT_LANE_CHANGE = 2,  // 右变道
};

（Struct） LaneletData

基于lanelet库的路径数据，包括路线和可能性

struct LaneletData
{lanelet::Lanelet lanelet;float probability;
};

（Struct）PredictedRefPath

预测的参考路线

struct PredictedRefPath
{float probability;  // 概率PosePath path;      // 路径(std::vector<geometry_msgs::msg::Pose>)Maneuver maneuver;  // 操作
};

（Func）toHexString

这个看起来是把UUID变量转换成了16进制的字符串形式

（Func）getLanelets

所以这里其实就是把std::vector<LaneletData>这个类型转成了std::vector<lanelet::ConstLanelets>这个类型。至于这里的ConstLanelet，看起来就是不可变的那种对象，所以可以理解为加了个const操作吧

（Func）getCrosswalkEntryPoint

这个里面得到的是人行横道的切入点，这里的切入点指的就是两端各自的中点

（Func）withinLanelet

这个函数通过检查物体的位置是否在车道多边形内来判断物体是否在车道线内。

using Point = boost::geometry::model::d2::point_xy<double>;
const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
const Point p_object{obj_pos.x, obj_pos.y};
auto polygon = lanelet.polygon2d().basicPolygon();
boost::geometry::correct(polygon);  // 修正多边形，确保是正确的形状
return boost::geometry::within(p_object, polygon);

（Func）withinRoadLanelet

这里和上一个函数的作用是一样的，也是检查对象在不在车道线内，不过增加了一个搜索的过程，相当于是增大可用范围的上一个函数

constexpr double search_radius = 10.0;  // [m]
const auto surrounding_lanelets = // 找到距离search_point最近的laneletslanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
for (const auto & lanelet : surrounding_lanelets) {if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) {lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);if (  // 不考虑crosswalk和walkwayattr.value() == lanelet::AttributeValueString::Crosswalk ||attr.value() == lanelet::AttributeValueString::Walkway) {continue;}}if (withinLanelet(object, lanelet.second)) {return true;}
}
return false;

（Func）isReachableEntryPoint

这个函数是根据检测到的行人和人行横道的端点，来判断在给定的时间内能到达的那一侧，主要步骤如下：

1. 先计算的就是行人到端点的距离distance_pedestrian_to_p1和distance_pedestrian_to_p2
2. 然后再进行进一步判断，要求不能和车道线相交的情况下，找到离得近的那个端点

if (first_intersect_load && second_intersect_load) {return {};
}
if (first_intersect_load && !second_intersect_load) {std::swap(ret.first, ret.second);
}

3. 最后还判断了下时间够不够，以及行人的朝向是不是向这边的

const Eigen::Vector2d pedestrian_to_crosswalk((ret.first.x() + ret.second.x()) / 2.0 - obj_pos.x,(ret.first.y() + ret.second.y()) / 2.0 - obj_pos.y);
const Eigen::Vector2d pedestrian_heading_direction(obj_vel.x * std::cos(yaw), obj_vel.x * std::sin(yaw));
const auto reachable =std::min(distance_pedestrian_to_p1, distance_pedestrian_to_p2) < velocity * time_horizon;
const auto heading_for_crosswalk =pedestrian_to_crosswalk.dot(pedestrian_heading_direction) > 0.0;
if ((reachable && heading_for_crosswalk) || (reachable && is_stop_object)) {return ret;
}

（Func）hasPotentialToReach

这个是在给定人行道端点的情况下判断能不能到，所以没有那么多筛选条件了，直接根据速度算能不能到就行

（Class Constructor）MapBasedPredictionNode::MapBasedPredictionNode

基于地图预测行人和车辆轨迹的类的构造函数，关于其中的参数

Parameter	Type	Description
enable_delay_compensation	bool	flag to enable the time delay compensation for the position of the object
prediction_time_horizon	double	predict time duration for predicted path [s]
prediction_sampling_delta_time	double	sampling time for points in predicted path [s]
min_velocity_for_map_based_prediction	double	apply map-based prediction to the objects with higher velocity than this value
min_crosswalk_user_velocity	double	minimum velocity use in path prediction for crosswalk users
dist_threshold_for_searching_lanelet	double	The threshold of the angle used when searching for the lane to which the object belongs [rad]
delta_yaw_threshold_for_searching_lanelet	double	The threshold of the distance used when searching for the lane to which the object belongs [m]
sigma_lateral_offset	double	Standard deviation for lateral position of objects [m]
sigma_yaw_angle	double	Standard deviation yaw angle of objects [rad]
object_buffer_time_length	double	Time span of object history to store the information [s]
history_time_length	double	Time span of object information used for prediction [s]
dist_ratio_threshold_to_left_bound	double	Conditions for using lane change detection of objects. Distance to the left bound of lanelet.
dist_ratio_threshold_to_right_bound	double	Conditions for using lane change detection of objects. Distance to the right bound of lanelet.
diff_dist_threshold_to_left_bound	double	Conditions for using lane change detection of objects. Differential value of horizontal position of objects.
diff_dist_threshold_to_right_bound	double	Conditions for using lane change detection of objects. Differential value of horizontal position of objects.

里面可以看到，PathGenerator类的对象在这里有被实例化

path_generator_ = std::make_shared<PathGenerator>(prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);

然后这里订阅的话题有检测到的对象**/perception/object_recognition/tracking/objects以及矢量地图/vector_map**，这里对对象的发布则变成了PredictedObjects类型，还有一个就是对象的行动maneuver了

（mFunc）MapBasedPredictionNode::convertToPredictedKinematics

将输入的跟踪到的TrackedObjectKinematics类型的对象转为了PredictedObjectKinematics，直接把值一一对应就好

（mFunc）MapBasedPredictionNode::convertToPredictedObject

这里也是类型的转换，不过是把TrackedObject转换成了PredictedObject，这里首先用到了上一个函数，把动态对象的相关变量进行了转换，然后其他的值也是一一对应就OK了

（mFunc）MapBasedPredictionNode::mapCallback

地图话题的回调函数，可以看到是直接根据消息加载的地图

lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);

然后是把人行横道和人行道查询了出来，存进了crosswalks_

PS：这里用的还是lanelet的库

（mFunc）MapBasedPredictionNode::objectsCallback

检测对象的回调函数，这里是这个部分进行数据处理的主要部分，所以其他成员函数在这里也是被各种调用使用到。 首先是拿到了一些坐标转换关系

world2map_transform			// 世界到地图
map2world_transform			// 地图到世界
debug_map2lidar_transform	// 地图到base_link，也就是机器人/雷达

接下来根据输入的对象的时间，来提出历史消息

const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
removeOldObjectsHistory(objects_detected_time);

接下来进行了对objects的遍历，里面涉及到了很多很多操作，首先就是把坐标系转到了map下

// transform object frame if it's based on map frame
if (in_objects->header.frame_id != "map") {geometry_msgs::msg::PoseStamped pose_in_map;geometry_msgs::msg::PoseStamped pose_orig;pose_orig.pose = object.kinematics.pose_with_covariance.pose;tf2::doTransform(pose_orig, pose_in_map, *world2map_transform);transformed_object.kinematics.pose_with_covariance.pose = pose_in_map.pose;
}

如果是行人或者自行车，就调用getPredictedObjectAsCrosswalkUser来生成路径

if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) {const auto predicted_object = getPredictedObjectAsCrosswalkUser(transformed_object);output.objects.push_back(predicted_object);

不然的话还是挺复杂的，涉及到了很多操作，先对对象进行位置的确定以及更新

// Update object yaw and velocity
updateObjectData(transformed_object);
// Get Closest Lanelet
const auto current_lanelets = getCurrentLanelets(transformed_object);
// Update Objects History
updateObjectsHistory(output.header, transformed_object, current_lanelets);

如果在车道线外了，就生成直线（这是个什么逻辑。。README里确实提到了这点）

PredictedPath predicted_path =path_generator_->generatePathForOffLaneVehicle(transformed_object);

然后对速度判断，速度太低的就归为低速运动车辆，最后生成的预测路线我感觉就是静止了？

std::fabs(transformed_object.kinematics.twist_with_covariance.twist.linear.x) <
min_velocity_for_map_based_prediction_) {
PredictedPath predicted_path =path_generator_->generatePathForLowSpeedVehicle(transformed_object);

不满足以上两种情况的话，就说明是在车道线上了，这个时候预测的轨迹是和车道线有关的

所以第一步就是得到ref_paths

// Get Predicted Reference Path for Each Maneuver and current lanelets
// return: <probability, paths>
const auto ref_paths =getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time);

找不到的话也认为是低速运动物体（其实就是给停那了），然后是为了Debug调试做准备的一步，这里还没太看懂

// Get Debug Marker for On Lane Vehicles
const auto max_prob_path = std::max_element(ref_paths.begin(), ref_paths.end(),[](const PredictedRefPath & a, const PredictedRefPath & b) {return a.probability < b.probability;});
const auto debug_marker =getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size());
debug_markers.markers.push_back(debug_marker);

接下来因为应该是生成了很多的ref_paths，所以遍历这个变量，对每一个成员都执行一次路线的生成，这样也得到了好多的预测路线

for (const auto & ref_path : ref_paths) {PredictedPath predicted_path =path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path);predicted_path.confidence = ref_path.probability;predicted_paths.push_back(predicted_path);
}

接下来把置信度归一化之后就直接push_back了，所以这里不需要筛选一下的么？疑惑.jpg。 至于不知道的物体，就按非车辆处理了，也就是直接直线预测（好暴力的办法）

（mFunc）MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser

这个函数是在检测对象为行人或者自行车的时候调用，首先就是生成预测路线，然后把对象转为PredictedObject类型
之后因为我们之前得到了所有的人行道，也就是crosswalks_，我们这里遍历所有的人行道，去寻找有没有对象所处在的道

for (const auto & crosswalk : crosswalks_) {if (withinLanelet(object, crosswalk)) {crossing_crosswalk = crosswalk;break;}
}

如果找到了这样一条路，我们进一步去判断能不能去到达一个端点，也就是entry_point两个中的一个，并且把路线push_back（前面一上来就生成了一条直线，所以这种情况下是两条线共存么？）

const auto entry_point = getCrosswalkEntryPoint(crossing_crosswalk.get());
if (hasPotentialToReach(object, entry_point.first, std::numeric_limits<double>::max(),min_crosswalk_user_velocity_)) {PredictedPath predicted_path =path_generator_->generatePathToTargetPoint(object, entry_point.first);predicted_path.confidence = 1.0;predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}
if (hasPotentialToReach(object, entry_point.second, std::numeric_limits<double>::max(),min_crosswalk_user_velocity_)) {PredictedPath predicted_path =path_generator_->generatePathToTargetPoint(object, entry_point.second);predicted_path.confidence = 1.0;predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

如果，此时这个人或者行人在车到线上，就先找到离得最近的人行道

const auto found_closest_crosswalk =lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk);

然后和上边一样，判断能不能到斑马线的一端，当然这里的逻辑是两端都能到达的话就两端都放进去。
然后在既不处在车道上，又不处在人行道上的情况，就去找所有有可能到达的人行道的端点，然后把所有的预测路线都push_back

for (const auto & crosswalk : crosswalks_) {const auto entry_point = getCrosswalkEntryPoint(crosswalk);const auto reachable_first = hasPotentialToReach(object, entry_point.first, prediction_time_horizon_, min_crosswalk_user_velocity_);const auto reachable_second = hasPotentialToReach(object, entry_point.second, prediction_time_horizon_, min_crosswalk_user_velocity_);if (!reachable_first && !reachable_second) {continue;}const auto reachable_crosswalk = isReachableEntryPoint(object, entry_point, lanelet_map_ptr_, prediction_time_horizon_,min_crosswalk_user_velocity_);if (!reachable_crosswalk) {continue;}PredictedPath predicted_path =path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());predicted_path.confidence = 1.0;if (predicted_path.path.empty()) {continue;}predicted_object.kinematics.predicted_paths.push_back(predicted_path);
}

这里确实应该有很多线，因为最后对置信度处理了一下，所以其实逻辑就是默认走直线，能过马路就过马路，不能过马路就尝试过马路

const auto n_path = predicted_object.kinematics.predicted_paths.size();
for (auto & predicted_path : predicted_object.kinematics.predicted_paths) {predicted_path.confidence = 1.0 / n_path;
}

（mFunc）MapBasedPredictionNode::updateObjectData

对对象的数据进行更新，这里主要更新的是朝向这个信息，这里针对的情况是object.kinematics.twist_with_covariance.twist.linear.x < 0.0的情况，也就是认为对象应该反过来的时候，然后如果orientation_availability == UNAVAILABLE的时候，会利用速度得到的下一个点的方向来计算一个方向出来，然后如果是SIGN_UNKNOWN也就是符号未知，那就加pi反过来就好

if (object.kinematics.orientation_availability ==autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) {const auto original_yaw =tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);// flip the angleobject.kinematics.pose_with_covariance.pose.orientation =tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw);
} else {const auto updated_object_yaw =tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position);object.kinematics.pose_with_covariance.pose.orientation =tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw);
}
object.kinematics.twist_with_covariance.twist.linear.x *= -1.0;

（mFunc）MapBasedPredictionNode::removeOldObjectsHistory

这个就是对所有历史的object遍历，然后根据当前时间来判断有哪些需要被删除，所以最后把需要删除的都删掉就好了。要么直接删除object_data，要么删除里面过时的

（mFunc）MapBasedPredictionNode::getCurrentLanelets

找到当前对象临近的路线，这里先用lanelet库的函数找到了所有临近的道路

// nearest lanelet
std::vector<std::pair<double, lanelet::Lanelet>> surrounding_lanelets =lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10);

然后进行了两个判断，把满足条件的所有lanelets都返回了

// Check if the close lanelets meet the necessary condition for start lanelets and
// Check if similar lanelet is inside the closest lanelet
if (!checkCloseLaneletCondition(lanelet, object, search_point) ||isDuplicated(lanelet, closest_lanelets)) {continue;
}
LaneletData closest_lanelet;
closest_lanelet.lanelet = lanelet.second;
closest_lanelet.probability = calculateLocalLikelihood(lanelet.second, object);
closest_lanelets.push_back(closest_lanelet);

（mFunc）MapBasedPredictionNode::checkCloseLaneletCondition

这里就是对找到的路进行各种判断，

1. 首先就是判断中点数量（用的是lanelet自己的变量）
2. 然后判断object周围的临近点云在不在这里面。然后这里还有一个处理，就是如果这个object是过去的信息，就来判断一下我们这里进行判断的路符不符合future_possible

// If the object is in the objects history, we check if the target lanelet is
// inside the current lanelets id or following lanelets
const std::string object_id = toHexString(object.object_id);
if (objects_history_.count(object_id) != 0) {const std::vector<lanelet::ConstLanelet> & possible_lanelet =objects_history_.at(object_id).back().future_possible_lanelets;bool not_in_possible_lanelet =std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) ==possible_lanelet.end();if (!possible_lanelet.empty() && not_in_possible_lanelet) {return false;}
}

3. 接下来计算路和object的一个夹角abs_norm_delta
4. 然后就是判断满不满足阈值要求了，分成了距离阈值和角度阈值

// Step4. Check if the closest lanelet is valid, and add all
// of the lanelets that are below max_dist and max_delta_yaw
const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x;
const bool is_yaw_reversed =M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0;
if (lanelet.first < dist_threshold_for_searching_lanelet_ &&(is_yaw_reversed || abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_)) {return true;
}

（mFunc）MapBasedPredictionNode::calculateLocalLikelihood

根据当前的lanelet和跟踪到的obejct来计算一个似然概率。首先计算两者之间的夹角abs_norm_delta_yaw，然后根据所有的路线中点，计算object到路线的一个横向偏移

const double lat_dist =std::fabs(motion_utils::calcLateralOffset(converted_centerline, obj_point));

然后计算这个卡方统计量，返回的是卡方统计值的倒数

// Compute Chi-squared distributed (Equation (8) in the paper)
const double sigma_d = sigma_lateral_offset_;  // Standard Deviation for lateral position
const double sigma_yaw = M_PI * sigma_yaw_angle_deg_ / 180.0;  // Standard Deviation for yaw angle
const Eigen::Vector2d delta(lat_dist, abs_norm_delta_yaw);
const Eigen::Matrix2d P_inv =(Eigen::Matrix2d() << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw)).finished();
const double MINIMUM_DISTANCE = 1e-6;
const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE);
return static_cast<float>(1.0 / dist);

感觉这里其实就相当于，根据距离的一个远近，为道路赋上了可能性这个值

（mFunc）MapBasedPredictionNode::updateObjectsHistory

接下来就是更新objects_history_这个变量了，这里可以简单理解为存放的是2s（这个值是根据前面remove的阈值来的） 内的所有objects的历史信息

（mFunc）MapBasedPredictionNode::getPredictedReferencePath

根据跟踪到的object，当前的道路消息current_lanelets_data和对象检测到的时间object_detected_time来生成所有可能的PredictedRefPath，这里是对所有的路线进行了遍历，首先确定的是搜索半径

const double search_dist = prediction_time_horizon_ * obj_vel +lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true};

接下来是先找到了左边的车道和右边的车道

lanelet::routing::LaneletPaths left_paths;
auto opt_left = routing_graph_ptr_->left(current_lanelet_data.lanelet);
if (!!opt_left) { // if left lanelet exists, change type to bool, keep valueleft_paths = routing_graph_ptr_->possiblePaths(*opt_left, possible_params);
}
lanelet::routing::LaneletPaths right_paths;
auto opt_right = routing_graph_ptr_->right(current_lanelet_data.lanelet);
if (!!opt_right) {right_paths = routing_graph_ptr_->possiblePaths(*opt_right, possible_params);
}

接下来找到所有的centerline，不过这里的搜索半径好大啊

lanelet::routing::LaneletPaths center_paths =routing_graph_ptr_->possiblePaths(current_lanelet_data.lanelet, possible_params);

接下来要预测object的行动了

const Maneuver predicted_maneuver =predictObjectManeuver(object, current_lanelet_data, object_detected_time);

然后计算这个行动对应的可能性

const auto maneuver_prob =calculateManeuverProbability(predicted_maneuver, left_paths, right_paths, center_paths);

接下来返回的是所有可能的paths，这里应该是有结合预测出来的行动进行判断的吧

const float path_prob = current_lanelet_data.probability;
const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) {addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths);
};
addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE);
addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE);
addReferencePathsLocal(center_paths, Maneuver::LANE_FOLLOW);

（mFunc）MapBasedPredictionNode::predictObjectManeuver

这里就是对车辆运动的预测了，首先这里如果没有历史信息的话，是认为继续沿着当前道路走

const std::string object_id = toHexString(object.object_id);
if (objects_history_.count(object_id) == 0) {return Maneuver::LANE_FOLLOW;
}

接下来找到一个合适的prev_id，这里的判断标准也是靠时间判断，如果找不到合适的，依旧认为是沿着当前的路继续走。然后根据角度，找到上一时刻的最近邻的prev_lanelet

const auto & prev_info = object_info.at(static_cast<size_t>(prev_id));
const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay);  // 补偿时间延迟
const lanelet::ConstLanelets prev_lanelets =object_info.at(static_cast<size_t>(prev_id)).current_lanelets;
if (prev_lanelets.empty()) {  return Maneuver::LANE_FOLLOW;
}
lanelet::ConstLanelet prev_lanelet = prev_lanelets.front();
double closest_prev_yaw = std::numeric_limits<double>::max();
for (const auto & lanelet : prev_lanelets) {const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, prev_pose.position);const double delta_yaw = tf2::getYaw(prev_pose.orientation) - lane_yaw;const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);if (normalized_delta_yaw < closest_prev_yaw) {closest_prev_yaw = normalized_delta_yaw;prev_lanelet = lanelet;}
}

接下来判断车辆变道没有，这里一开始看可能觉得写错了，但这里实际上的逻辑是只有当在上一时刻周边的路线内找到当前路线了，才认为是发生变道了。如果没找到的话就说明这个信息是无效的，或者理解为道差得太多了，那就返回继续走就好了

lanelet::routing::LaneletPaths possible_paths =routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false);
bool has_lane_changed = true;
for (const auto & path : possible_paths) {for (const auto & lanelet : path) {if (lanelet == current_lanelet) {has_lane_changed = false;break;}}
}
if (has_lane_changed) {return Maneuver::LANE_FOLLOW;
}

最后就是判断到底是向左还是向右变道了，逻辑参考README吧

（mFunc）MapBasedPredictionNode::compensateTimeDelay

对时间延迟进行补偿的函数，这个逻辑简单，就是根据速度和时差来补偿位姿，没什么好说的

（mFunc）MapBasedPredictionNode::calcRightLateralOffset

计算向右的横向偏移的，直接调用了common包中的函数

（mFunc）MapBasedPredictionNode::calcLeftLateralOffset

就是上个函数反过来了

（mFunc）MapBasedPredictionNode::updateFuturePossibleLanelets

把所有的paths加到历史信息的future_possible_lanelets里了

（mFunc）MapBasedPredictionNode::addReferencePaths

将候选路径转换为参考路径，并设置参考路径的概率和行驶动作。也没什么好说的

（mFunc）MapBasedPredictionNode::calculateManeuverProbability

这个是计算车辆行为所对应的概率，不过这个里面就是单纯的根据预测出来的路线和对应的左、中、右还有没有路来设置的， 没有涉及到什么复杂的地方，当然这个概率最后是归一化了，所以就是把概率最大（归一化前）的概率设为1了

（mFunc）MapBasedPredictionNode::convertPathType

这里其实是对路线类型进行转换的也很简单，就是取出来对应的坐标并且计算个角度然后赋值就好了，当然这里用到了prev_lanelets，代码注释给的解释是插值的时候是从上一段开始的，所以这里的目的是让插值更平滑么？插值的操作在最后调用resamplePath函数就行了

// Resample Path
const auto resampled_converted_path = resamplePath(converted_path);
converted_paths.push_back(resampled_converted_path);

（mFunc）MapBasedPredictionNode::resamplePath

对路径重采样的操作，这里也是根据长度，分别对xyz进行插值操作

const auto resampled_x = interpolation::lerp(base_s, base_x, resampled_s);
const auto resampled_y = interpolation::lerp(base_s, base_y, resampled_s);
const auto resampled_z = interpolation::lerp(base_s, base_z, resampled_s);

（mFunc）MapBasedPredictionNode::isDuplicated

最后针对LaneletsData和自定义的PredictedPath类型的道路都进行了这个是否重复的判断，这个判断其实很简单，就是遍历道路容器依次计算有没有间距小于阈值的，如果距离小于阈值了就认为是有重复的

总结

到这里终于把这个包看完了，这个包其实就是对检测到的物体进行预测的一个东西，不过这里做的有图的，也就是基于地图的。所以里面涉及到的比较复杂的就是车辆变道以及行人过马路了，底层的预测轨迹还是基于这个速度时间来的。相比于高级的算法，这里针对简单场景下充分考虑所有情况显得更加重要。