1. 前言
之前忘记发布了,现在补上。
该方法还是基于autoware1.10.0,openplanner的代码部分需要修改。这部分需要加载vector map,所以需要autoware网站上提供的地图工具。具体vector map工具请参考autoware官网。
各个配置可以参考之前的文章。
2. 配置
(1)Autoware的Runtime Manager的Setup项目配置
(2)Autoware的Runtime Manager的Map项目配置
这里特别说明一下vector map,是由autoware官网工具产生的,可以参考官网,会生成一些,路沿、车道线,轨迹等信息。
(3)Autoware的Runtime Manager的Sensing项目配置
(4)Autoware的Runtime Manager的Computing项目中planning的配置。
ndt_matching可以打开gpu
lidar_tracker项目需要勾选lidar_kf_contour_track(跟踪障碍物),相应的app配置为默认
Vector Map Filter Distance用于检测障碍物到路径的距离,如下图所示,黑色的粗线为航迹,那么障碍物在轨迹前或后方时,小于两倍的设置距离会被跟踪,而在轨迹范围内时,小于1倍的设置距离会被跟踪。
这里需要修改lidar_kf_contour_track引用的一个库文件PlannerHelpers.cpp的GetRelativeInfoLimited函数代码片段增加一句,如下所示。
info.bAfter = false;
info.bBefore = false;
if(info.iFront == 0)
{
info.bBefore = true;
}
else if(info.iFront == _trajectory.size()-1)
{
int s = _trajectory.size();
double angle_befor_last = UtilityH::FixNegativeAngle(atan2(_trajectory.at(s-2).pos.y - _trajectory.at(s-1).pos.y, _trajectory.at(s-2).pos.x - _trajectory.at(s-1).pos.x));
double angle_from_perp = UtilityH::FixNegativeAngle(atan2(info.perp_point.pos.y - _trajectory.at(s-1).pos.y, info.perp_point.pos.x - _trajectory.at(s-1).pos.x));
double diff_last_perp = UtilityH::AngleBetweenTwoAnglesPositive(angle_befor_last, angle_from_perp);
info.after_angle = diff_last_perp;
if(diff_last_perp > M_PI_2)
{
info.bAfter = true;
}
info.iFront = trajectory.size() - 1; // 增加这句来防止越界
}
op_global_planner的app设置。
Replanning用于到达目标点后,可以重新规划到下一个目标点。如果有了两个目标点,那么会循环,下方修改代码后,可以不循环。
Smooth用于平滑全局路径。
op_common_params的app设置
Plan Ditance用于设置朱路径两侧的衍生出的局部轨迹的长度;
Path Density用于局部轨迹上两个轨迹点的距;
Horizontal Density用于设置两个局部轨迹的间距;
Rollouts Number用于设置局部轨迹的数量;
Max Velocity轨迹上的执行速度,避障时局部轨迹速度减半,单位m/s;
Follow DIstance关键参数,沿着轨迹设置从多远检测障碍物;
Avoid Distance感知到路径上的障碍物,判断多远开始绕行;
Avoidance Limit停车是,障碍物距离多远可以开车;
Lateral Safety与Longitudinal Safty设置安全框尺寸,分别代表车辆安全框宽与长。
op_trajectory_generator的app设置
Tip Margin用于设置车身到路径分叉点的距离;
Roll In Margin用于设置局部轨迹弯折的距离;
op_motion_predictor的app设置
Detect curbs from map用于将路沿判定为障碍物。
由于采用vector map后,只能运行一次,为了业务端可以反复执行路径,需要对op_global_planner_core.cpp的MainLoop()及BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()做修改。
op_global_planner_core.cpp的MainLoop():
void GlobalPlanner::MainLoop()
{
...
if(m_GoalsPos.size() > 0)
{
if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
{
// -----原有代码-----
// if(m_params.bEnableReplanning)
// -----上述代码修改为下面一行代码------
if(m_params.bEnableReplanning && m_iCurrentGoalIndex < m_GoalsPos.size() - 1)
{
PlannerHNS::RelativeInfo info;
bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info);
if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())
{
double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
if(remaining_distance <= REPLANNING_DISTANCE)
{
bMakeNewPlan = true;
if(m_GoalsPos.size() > 0)
m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();
std::cout << "Current Goal Index = " << m_iCurrentGoalIndex << std::endl << std::endl;
}
}
}
}
...
}
BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState():
BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
// -------------新增代码支持任务结束后,重新规划到目标点----------------
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID == -1)
return FindBehaviorState(this->m_Behavior);
else
{
pCParams->prevGoalID = pCParams->currentGoalID;
return FindBehaviorState(FORWARD_STATE);
}
// -------------原有代码--------------------
// return FindBehaviorState(this->m_Behavior);
}
pure_pursuit模块在final_waypoints为空时会出异常,支持循环寻迹:
首先在pure_pursuit_core.h中添加 ros::Subscriber sub5_;
private:
// handle
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
// class
PurePursuit pp_;
// publisher
ros::Publisher pub1_, pub2_, pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_;
// subscriber
ros::Subscriber sub1_, sub2_, sub3_, sub4_;
// -----2019.10.12-----
ros::Subscriber sub5_; // xiaochun add
//---------------------
// constant
const int LOOP_RATE_; // processing frequency
增加回调函数声明
// callbacks
void callbackFromConfig(
const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
void callbackFromCurrentVelocity(
const geometry_msgs::TwistStampedConstPtr& msg);
void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg_input); //modify by hxc
void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg); // add by hxc
添加补充的航迹点
void PurePursuitNode::initForROS()
{
// ros parameter settings
private_nh_.param("is_linear_interpolation", is_linear_interpolation_, bool(true));
// ROS_INFO_STREAM("is_linear_interpolation : " << is_linear_interpolation_);
private_nh_.param("publishes_for_steering_robot", publishes_for_steering_robot_, bool(false));
nh_.param("vehicle_info/wheel_base", wheel_base_, double(2.7));
// setup subscriber
sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this);
sub2_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this);
sub3_ = nh_.subscribe("config/waypoint_follower", 10, &PurePursuitNode::callbackFromConfig, this);
sub4_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this);
sub5_ = nh_.subscribe("loop_waypoints", 10, &PurePursuitNode::loopWaypointsCallback, this); // xiaochun add
// setup publisher
pub1_ = nh_.advertise<geometry_msgs::TwistStamped>("twist_raw", 10);
pub2_ = nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd", 10);
pub11_ = nh_.advertise<visualization_msgs::Marker>("next_waypoint_mark", 0);
pub12_ = nh_.advertise<visualization_msgs::Marker>("next_target_mark", 0);
pub13_ = nh_.advertise<visualization_msgs::Marker>("search_circle_mark", 0);
pub14_ = nh_.advertise<visualization_msgs::Marker>("line_point_mark", 0); // debug tool
pub15_ = nh_.advertise<visualization_msgs::Marker>("trajectory_circle_mark", 0);
pub16_ = nh_.advertise<std_msgs::Float32>("angular_gravity", 0);
pub17_ = nh_.advertise<std_msgs::Float32>("deviation_of_current_position", 0);
// pub7_ = nh.advertise<std_msgs::Bool>("wf_stat", 0);
}
增加sub5_的定义。
pure_pursuit_core.cpp 中添加
// ----------------2019.10.12-----------------------
static autoware_msgs::Lane loopWaypoints;
void PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)
{
loopWaypoints = *msg;
}
对应sub5_的回调函数
修改void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr &msg_input)函数,防止异常
void PurePursuitNode::callbackFromWayPoints(const autoware_msgs::LaneConstPtr &msg_input)
{
/****************** xiaochun add ********************/
autoware_msgs::Lane::Ptr msg(new autoware_msgs::Lane);
*msg = *msg_input;
if (!loopWaypoints.waypoints.empty())
{
msg->waypoints.insert(msg->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());
loopWaypoints.waypoints.clear();
}
if (msg->waypoints.empty()) return; // 如果waypoints为空,pp_会出异常
/****************** xiaochun add ********************/
if (!msg->waypoints.empty())
command_linear_velocity_ = msg->waypoints.at(0).twist.twist.linear.x; // 从航点中得到速度
else
command_linear_velocity_ = 0;
pp_.setCurrentWaypoints(msg->waypoints);
is_waypoint_set_ = true;
}
由于pure_pursuit的参数设置中有Waypoint及Dialog两个选项,由于寻迹需要采用Waypoint模式,而Waypoint采用航继点的速度,为了保证测试,希望寻迹也能用Dialog模式,所以需要修改代码,如下:
double PurePursuitNode::computeLookaheadDistance() const
{
// -----------新增代码------------
return const_lookahead_distance_;
// -----------原有代码-------------
if (param_flag_ == enumToInteger(Mode::dialog)) // 如果是dialog模式, 就直接返回lookahead_distance
return const_lookahead_distance_;
double maximum_lookahead_distance = current_linear_velocity_ * 10; // 否则返回介于min和max闭区间的数值
double ld = current_linear_velocity_ * lookahead_distance_ratio_;
return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ :
ld > maximum_lookahead_distance ? maximum_lookahead_distance : ld;
}
(5)增加障碍物检测,笛卡尔聚类方法
lidar_kf_contour_track障碍物通过消息 ClouldCluster发出的,所以对应lidar_kf_contour_track接收,双方的代码有些不一致,需要修改。
首先需要配置lidar_euclidean_cluster_detect.launch文件的主要参数
<launch>
<arg name="points_node" default="/points_raw"/><!--CHANGE THIS TO READ WHETHER FROM VSCAN OR POINTS_RAW -->
<arg name="remove_ground" default="true"/><!-- 删除地面上的点 -->
<arg name="downsample_cloud" default="false"/> <!-- Apply VoxelGrid Filter with the value given by "leaf_size"-->
<arg name="leaf_size" default="0.1"/><!-- Voxel Grid Filter leaf size-->
<arg name="cluster_size_min" default="20"/><!-- Minimum number of points to consider a cluster as valid-->
<arg name="cluster_size_max" default="100000"/><!-- Maximum number of points to allow inside a cluster-->
<arg name="sync" default="false"/>
<arg name="use_diffnormals" default="false"/>
<arg name="pose_estimation" default="true"/>
<arg name="clip_min_height" default="-1.0"/><!-- lidar的高度,根据实际情况定,注意为负值,这个参数与删除地面直接相关 -->
<arg name="clip_max_height" default="0.3"/><!-- 检测的物体的高度,从激光雷达中心开始 -->
<!-- hgl added-->
<arg name="lidar_front_car_range" default="0.5" /><!-- lidar到车前方的距离,为了去除扫描到车身上的点 -->
<arg name="lidar_back_car_range" default="-3.0" /><!-- lidar到车后方的距离,为了去除扫描到车身上的点 -->
<arg name="lidar_left_car_range" default="0.65" /><!-- lidar到车左侧的距离,为了去除扫描到车身上的点 -->
<arg name="lidar_right_car_range" default="-0.65" /><!-- lidar到车右侧的距离,为了去除扫描到车身上的点 -->
<arg name="lidar_back_range" default="-5.0" /><!-- lidar到车后方的有效距离,为了去除车后方的点 -->
<arg name="lidar_total_range" default="30.0" /><!-- lidar的有效探测范围 -->
<!-- hgl added-->
<arg name="keep_lanes" default="false"/>
<arg name="keep_lane_left_distance" default="5"/>
<arg name="keep_lane_right_distance" default="5"/>
<arg name="max_boundingbox_side" default="10"/>
<arg name="cluster_merge_threshold" default="1.5"/>
<arg name="clustering_distance" default="0.75"/>
<arg name="use_vector_map" default="false"/>
<arg name="vectormap_frame" default="map"/>
<arg name="wayarea_gridmap_topic" default="grid_map_wayarea"/>
<arg name="wayarea_gridmap_layer" default="wayarea"/>
<arg name="wayarea_no_road_value" default="255"/>
<arg name="output_frame" default="map"/><!-- 默认障碍物在velodyne坐标系下,而lidar_kf_contour_track需要地图坐标系下的障碍物信息,所以修改为map
-->
<arg name="remove_points_upto" default="0.0"/>
<arg name="use_gpu" default="true"/>
<arg name="use_multiple_thres" default="false"/>
<arg name="clustering_ranges" default="[15,30,45,60]"/><!-- Distances to segment pointcloud -->
<arg name="clustering_distances"
default="[0.5,1.1,1.6,2.1,2.6]"/><!-- Euclidean Clustering threshold distance for each segment -->
首先增加一些参数,用于修改车辆周围的点云
static bool _velodyne_transform_available;
static bool _downsample_cloud;
static bool _pose_estimation;
static double _leaf_size;
static int _cluster_size_min;
static int _cluster_size_max;
//******************** add by hgl ********************//
static double _lidar_front_car_range;
static double _lidar_back_car_range;
static double _lidar_left_car_range;
static double _lidar_right_car_range;
static double _lidar_back_range;
static double _lidar_total_range;
//******************** add by hgl ********************//
static bool _remove_ground; // only ground
在main()函数增加参数定义及赋值
ROS_INFO("clip_max_height: %f", _clip_max_height);
/******************* hgl added *******************/
private_nh.param("lidar_front_car_range", _lidar_front_car_range, 0.5);
ROS_INFO("lidar_front_car_range: %f", _lidar_front_car_range);
private_nh.param("lidar_back_car_range", _lidar_back_car_range, -3.0);
ROS_INFO("lidar_back_car_range: %f", _lidar_back_car_range);
private_nh.param("lidar_left_car_range", _lidar_left_car_range, 0.6);
ROS_INFO("lidar_left_car_range: %f", _lidar_left_car_range);
private_nh.param("lidar_right_car_range", _lidar_right_car_range, -0.6);
ROS_INFO("lidar_right_car_range: %f", _lidar_right_car_range);
private_nh.param("lidar_back_range", _lidar_back_range, -5.0);
ROS_INFO("lidar_back_range: %f", _lidar_back_range);
private_nh.param("lidar_total_range", _lidar_total_range, 30.0);
ROS_INFO("lidar_total_range: %f", _lidar_total_range);
/******************* hgl added *******************/
private_nh.param("keep_lanes", _keep_lanes, false);
增加一个函数来限定激光雷达的测量范围,在lidar_euclidean_cluster_detect.cpp中添加filteredPointsBydistance函数,用于过滤掉车辆上的扫描点及一定范围外的扫描点。
void filteredPointsBydistance(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr)
{
out_cloud_ptr->points.clear();
for (unsigned int i = 0; i < in_cloud_ptr->points.size(); i++)
{
if (in_cloud_ptr->points[i].x < _lidar_front_car_range &&
in_cloud_ptr->points[i].x > _lidar_back_car_range &&
in_cloud_ptr->points[i].y < _lidar_left_car_range &&
in_cloud_ptr->points[i].y > _lidar_right_car_range)
continue;
if (in_cloud_ptr->points[i].x < _lidar_back_range) continue;
double r = sqrt(pow(in_cloud_ptr->points[i].x, 2) + pow(in_cloud_ptr->points[i].y, 2));
if (r > _lidar_total_range)
continue;
// if (in_cloud_ptr->points[i].x > 5 ||
// in_cloud_ptr->points[i].x < -5.0 ||
// in_cloud_ptr->points[i].y > 2 ||
// in_cloud_ptr->points[i].y < -2)
// continue;
// if (in_cloud_ptr->points[i].y > 3 ||
// in_cloud_ptr->points[i].y < -3)
// continue;
out_cloud_ptr->points.push_back(in_cloud_ptr->points[i]);
}
}
修改velodyne_callback函数,在点云最开始处理filteredPointsBydistance函数。
pcl::PointCloud<pcl::PointXYZ>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr removed_points_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inlanes_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nofloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr onlyfloor_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr diffnormals_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clipped_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);// hgl modified filteredPointsBydistance函数的结果
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clustered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
autoware_msgs::Centroids centroids;
autoware_msgs::CloudClusterArray cloud_clusters;
pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr);
_velodyne_header = in_sensor_cloud->header;
/******************* hgl modified ************************/
filteredPointsBydistance(current_sensor_cloud_ptr, filtered_cloud_ptr); //hgl add
if (_remove_points_upto > 0.0)
{
removePointsUpTo(filtered_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto); // removePointsUpTo(current_sensor_cloud_ptr, removed_points_cloud_ptr, _remove_points_upto);
} else
removed_points_cloud_ptr = filtered_cloud_ptr; // removed_points_cloud_ptr = current_sensor_cloud_ptr;
/******************* hgl modified ************************/
if (_downsample_cloud)
downsampleCloud(removed_points_cloud_ptr, downsampled_cloud_ptr, _leaf_size);
else
downsampled_cloud_ptr = removed_points_cloud_ptr;
clipCloud(downsampled_cloud_ptr, clipped_cloud_ptr, _clip_min_height, _clip_max_height);
对发出的聚类(障碍物)进行适配,修改publishCloudClusters函数,ClouldCluster主要需要的数据类型有:
std_msgs/Header header
uint32 id
string label
float64 score
sensor_msgs/PointCloud2 cloud #聚类(障碍物)点云数据
geometry_msgs/PointStamped min_point
geometry_msgs/PointStamped max_point
geometry_msgs/PointStamped avg_point # 与centroid_point相同
geometry_msgs/PointStamped centroid_point # 点云中心点,取平均得到。
float64 estimated_angle # 估计的朝向
geometry_msgs/Vector3 dimensions # 聚类(障碍物)的长宽高
geometry_msgs/Vector3 eigen_values
geometry_msgs/Vector3[] eigen_vectors
#Array of 33 floats containing the FPFH descriptor
std_msgs/Float32MultiArray fpfh_descriptor
jsk_recognition_msgs/BoundingBox bounding_box
geometry_msgs/PolygonStamped convex_hull
# Indicator information
# INDICATOR_LEFT 0
# INDICATOR_RIGHT 1
# INDICATOR_BOTH 2
# INDICATOR_NONE 3
uint32 indicator_state
为了适配lidar_euclidean_cluster_detect,对publishCloudClusters做一下修改:
cluster_transformed.header.frame_id = in_target_frame;
cluster_transformed.dimensions = i->dimensions;
cluster_transformed.eigen_values = i->eigen_values;
cluster_transformed.eigen_vectors = i->eigen_vectors;
/********************* xiaochu add ************************/
cluster_transformed.avg_point.point.x = cluster_transformed.centroid_point.point.x;
cluster_transformed.avg_point.point.y = cluster_transformed.centroid_point.point.y;
cluster_transformed.avg_point.point.z = cluster_transformed.centroid_point.point.z;
cluster_transformed.indicator_state = 3;
static unsigned int id = 0; // ID 一定要添加,否则lidar_kf_contour_track算法不能对障碍物做出区分,导致障碍物失效。
cluster_transformed.id = id++;
/********************* xiaochu add ************************/
clusters_transformed.clusters.push_back(cluster_transformed);
3. 调试笔记:
(1)op_common_params设置中,车辆安全框的尺寸可以参数(参考之前)设置。激光雷达会有部分扫描点集中在车辆,lidar_kf_contour_track会将这部分扫描点通过IsCar函数默认为车辆,聚类方法lidar_euclidean_cluster_detect会将这部分点聚类。当人靠近车辆时,euclidean_cluster方法会将靠近车的人和车上的扫描点聚成一类,即将靠近车的人认为是车上的点。
为了解决问题,这里的处理方法为将lidar_kf_contour_track_core.cpp中的
if(!IsCar(obj, m_CurrentPos, m_Map)) continue;
语句去掉,然后在lidar_euclidean_cluster_detect.cpp中,增加删除车辆上的语句。详见上面笛卡尔聚类增加的filteredPointsBydistance函数。
(2)避障的横向控制问题
开避障后,在调试过程中,发现转弯处发现连续变道,问题原因是一侧的障碍物比另一侧的障碍物多,trajectoryCosts.at(ic).lateral_cost总是影响主路径的选择,我们尝试过权重从1.2开始降到0.0001,发现没有效果,所以最终设置为0。最终将TrajectoryDynamicCosts.cpp中的NormalizeCosts()一条语句,修改如下:
trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + /*m_WeightLat*/0*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0;
(3)轴距修改
车辆的轴距并没有在RuntimeManager显示,在op_common_params.launch中需要设置
<arg name="wheelBaseLength" default="1.45" />
<arg name="turningRadius" default="3.0" />
<arg name="maxSteerAngle" default="0.612" />
这里wheelBaseLength指的是轴距,需要手动修改。我们车的轴距1.45。
(4)修改障碍物绕行,距离车头2/3处开始返回主路径
在车辆运行过程中,我们发现车辆在绕行障碍物的过程中,车辆大约会在运行到车头距离障碍物2/3处开始返回主路径,有时会导致车辆的安全框碰到障碍物,车辆停止运行。
这里在op_planner下的TrajectoryDynamicCosts.cpp中CalculateLateralAndLongitudinalCostsStatic函数中,有一处跟车身相关的设置,carInfo.length除以了1.5,所以会在车头距离障碍物2/3处开始返回主路径。这里将除以1.5改为除以1,就可以让车辆行驶到车尾与障碍物平行开始返回主路径。
(5)绕行贴近障碍物停车
openplanner中并没有考虑车辆绕行过程中,将车辆安全框的宽度考虑进行,导致车辆在选择路径的时候,会选择距离障碍物最近的路径,绕行过程中由于障碍物不规则,会导致障碍物的边缘进入安全框,停车。所以需要在op_planner下的TrajectoryDynamicCosts.cpp中的CalculateLateralAndLongitudinalCostsStatic函数,某处判断加入车辆安全框的宽度(冲车辆边缘多出来的部分),具体修改如下代码。
以下修改针对(4)(5)两点。
if(lateralDist - 0.2 <= critical_lateral_distance // 减去0.2,就是为了加入考量车辆安全框的宽度
&& longitudinalDist >= -carInfo.length / 1.0 /*/1.5*/
&& longitudinalDist < params.minFollowingDistance)
(6)用我们制作的vector map,在vector map导入时,process died的错误
产生错误的原因是因为,我们在制作vector map时,如果两个航迹点的距离较近,那么resolution的值会很小,导致skip_factor/resolution的值会很小,由于skip是有符号整数,会导致skip溢出变为负值,所以在下边加一条语句,防止溢出。将PlanningHelpers.cpp下的PlanningHelpers::GetClosestNextPointIndexFast函数,修改如下
int skip = 1;
if(resolution > 0)
skip = skip_factor/resolution;
// xiaochun add
if (skip <= 0)
{
skip = __INT_MAX__;
}
// xiaochun ad
(7)出现车辆遇到长障碍物回拐问题
进过我们长时间测试,发现如果出现10米甚至更长的障碍物时,车辆会出现回拐的现象,原因是障碍物变成了一条线,车辆认为可以通过。我们修改op_planner下的TrajectoryDynamicCosts.cpp中的CalculateLateralAndLongitudinalCostsStatic函数
if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
trajectoryCosts.at(iCostIndex).bBlocked = true;
//-------------------------add by hxc------------------------
if (((obj_info.perp_distance - distance_from_center) * (car_info.perp_distance - distance_from_center) > 0) && lateralDist < fabs(car_info.perp_distance - distance_from_center) && longitudinalDist >= -carInfo.length && longitudinalDist <= 0)
{
trajectoryCosts.at(iCostIndex).bBlocked = true;
}
//-----------------------------------------------------------
致谢:
感谢小春在整个过程中做出的努力,很多工作都是小春完成的。
原创博文,转载请标明出处。
评论(1)
您还未登录,请登录后发表或查看评论