扫二维码与项目经理沟通
我们在微信上24小时期待你的声音
解答本文疑问/技术咨询/运营咨询/技术建议/互联网交流
(注:本人的第一篇文章,能力有限,写的不好,请见谅!)
成都创新互联公司是专业的乐陵网站建设公司,乐陵接单;提供网站制作、成都网站建设,网页设计,网站设计,建网站,PHP网站建设等专业做网站服务;采用PHP框架,可快速的进行乐陵网站开发网页制作和功能扩展;专业做搜索引擎喜爱的网站,专业的做网站团队,希望更多企业前来合作!void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)//跟踪对象
{
UtilityHNS::UtilityH::GetTickCount(m_SensingTimer);//计算程序运行时间
m_TrackedObjects.clear();
bTrackedObjects = true;
PlannerHNS::DetectedObject obj;
for(unsigned int i = 0 ; iobjects.size(); i++)
{
if(msg->objects.at(i).id >0)
{
PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj);
m_TrackedObjects.push_back(obj);//存放障碍物
}
// else
// {
// std::cout<< " Ego Car avoid detecting itself from motion prediction node! ID: "<< msg->objects.at(i).id<< std::endl;
// }
}
if(bMap)
{
if(m_PredictBeh.m_bStepByStep && m_bGoNextStep)
{
m_bGoNextStep = false;
m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map);
}
else if(!m_PredictBeh.m_bStepByStep)
{
m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map);
}
m_PredictedResultsResults.objects.clear();
autoware_msgs::DetectedObject pred_obj;
for(unsigned int i = 0 ; iobj, false, pred_obj);
if(m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track)
pred_obj.behavior_state = m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track->best_beh;
m_PredictedResultsResults.objects.push_back(pred_obj);
}
if(m_bEnableCurbObstacles)
{
curr_curbs_obstacles.clear();
GenerateCurbsObstacles(curr_curbs_obstacles);
//std::cout<< "Curbs No: "<< curr_curbs_obstacles.size()<< endl;
for(unsigned int i = 0 ; i
ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj)函数作用:障碍物属性信息格式的转换。
m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map)函数作用:基于地图和障碍物位置,生成障碍物的预测轨迹。
2. DoOneStep()函数void BehaviorPrediction::DoOneStep(const std::vector& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map)
{
if(!m_bUseFixedPrediction && maxDeceleration !=0)
m_PredictionDistance = -pow(currPose.v, 2)/(maxDeceleration);
ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);//提取轨迹/m_TrackedObjects
CalculateCollisionTimes(minSpeed);
if(m_bParticleFilter)
{
ParticleFilterSteps(m_ParticleInfo_II);//微粒过滤步骤
}
}
2.1 ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);void BehaviorPrediction::ExtractTrajectoriesFromMap(const std::vector& curr_obj_list,RoadNetwork& map, std::vector& old_obj_list)
{
PlannerH planner;
m_temp_list_ii.clear();//存放当前帧的障碍物列表
std::vectordelete_me_list = old_obj_list;//m_ParticleInfo_II,起始为空
for(unsigned int i=0; i< curr_obj_list.size(); i++)
{
bool bMatch = false;
for(unsigned int ip=0; ip< old_obj_list.size(); ip++)//遍历旧的障碍物列表,是否找到与当前新障碍物对应的旧障碍物
{
if(old_obj_list.at(ip)->obj.id == curr_obj_list.at(i).id)//如果有
{
bool bFound = false;
for(unsigned int k=0; k< m_temp_list_ii.size(); k++)//遍历当前障碍物表m_temp_list_ii,是否存在障碍物与旧障碍物相同
{
if(m_temp_list_ii.at(k) == old_obj_list.at(ip))//若有,不加入m_temp_list_ii
{
bFound = true;
break;
}
}
if(!bFound)//若m_temp_list_ii没有找到对应的障碍物,把new_obj加入m_temp_list_ii
{
old_obj_list.at(ip)->obj = curr_obj_list.at(i);
m_temp_list_ii.push_back(old_obj_list.at(ip));
}
DeleteFromList(delete_me_list, old_obj_list.at(ip));
old_obj_list.erase(old_obj_list.begin()+ip);
bMatch = true;
break;
}
}
if(!bMatch)//如果old_obj_list.at(ip)->obj.id !=curr_obj_list.at(i).id,curr_obj_list.at(i)加入 m_temp_list_ii
{
ObjParticles* pNewObj = new ObjParticles();
pNewObj->obj = curr_obj_list.at(i);
m_temp_list_ii.push_back(pNewObj);
}
}
DeleteTheRest(delete_me_list);
old_obj_list.clear();
old_obj_list = m_temp_list_ii;
//m_PredictedObjects.clear();遍历每一个障碍物生成多条预测轨迹
for(unsigned int ip=0; ip< old_obj_list.size(); ip++)
{
PredictCurrentTrajectory(map, old_obj_list.at(ip));
//m_PredictedObjects.push_back(old_obj_list.at(ip)->obj);
old_obj_list.at(ip)->MatchTrajectories();
}
}
2.2 planner.PredictTrajectoriesUsingDP();double PlannerH::PredictTrajectoriesUsingDP(const WayPoint& startPose, std::vectorclosestWPs, const double& maxPlanningDistance, std::vector>& paths, const bool& bFindBranches , const bool bDirectionBased, const bool pathDensity)
{
vector>tempCurrentForwardPathss;
vectorall_cell_to_delete;
vectorglobalPath;
vectorpLaneCells;
vectorunique_lanes;
std::vectorpath;
//遍历当前障碍物的每一个可行最近点
for(unsigned int j = 0 ; j< closestWPs.size(); j++)
{
pLaneCells.clear();
//从最近点开始用dp开始搜索,遍历获得几条路径
int nPaths = PlanningHelpers::PredictiveIgnorIdsDP(closestWPs.at(j), maxPlanningDistance, all_cell_to_delete, pLaneCells, unique_lanes);
for(unsigned int i = 0; i< pLaneCells.size(); i++)
{
path.clear();
//回溯路经给path
PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), closestWPs.at(j), globalPath, path, tempCurrentForwardPathss);
//遍历获得的路径上每个点,找到对应的 unique_lanes,没找到,加入
for(unsigned int k = 0; k< path.size(); k++)
{
bool bFoundLaneID = false;
//unique_lanes起始为空,判断unique_lanes中是否存在path.at(k).laneId,如存在,不加入unique_lanes中
for(unsigned int l_id = 0 ; l_id< unique_lanes.size(); l_id++)
{
if(unique_lanes.at(k).laneId == unique_lanes.at(l_id))
{
bFoundLaneID = true;
break;
}
}
if(!bFoundLaneID)
unique_lanes.push_back(path.at(k).laneId);//存放当前path中所有的不同path.at(k).laneId
}
if(path.size()>0)
{//把障碍物位置加入path中,设置属性
path.insert(path.begin(), startPose);
if(!bDirectionBased)
path.at(0).pos.a = path.at(1).pos.a;
path.at(0).beh_state = path.at(1).beh_state = PlannerHNS::BEH_FORWARD_STATE;
path.at(0).laneId = path.at(1).laneId;
PlanningHelpers::FixPathDensity(path, pathDensity);
PlanningHelpers::SmoothPath(path,0.4,0.3,0.1);
PlanningHelpers::CalcAngleAndCost(path);
paths.push_back(path);
}
}
}
if(bDirectionBased && bFindBranches)
{
WayPoint p1, p2;
if(paths.size()>0 && paths.at(0).size() >0)
p2 = p1 = paths.at(0).at(0);
else
p2 = p1 = startPose;
double branch_length = maxPlanningDistance*0.5;
p2.pos.y = p1.pos.y + branch_length*0.4*sin(p1.pos.a);
p2.pos.x = p1.pos.x + branch_length*0.4*cos(p1.pos.a);
vectorl_branch;
vectorr_branch;
//手工生成分支,以p1, p2,为起点,branch_length为距离生成终点
PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_RIGHT_DIR,r_branch);
PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_LEFT_DIR, l_branch);
PlanningHelpers::FixPathDensity(l_branch, pathDensity);
PlanningHelpers::SmoothPath(l_branch,0.4,0.3,0.1);
PlanningHelpers::CalcAngleAndCost(l_branch);
PlanningHelpers::FixPathDensity(r_branch, pathDensity);
PlanningHelpers::SmoothPath(r_branch,0.4,0.3,0.1);
PlanningHelpers::CalcAngleAndCost(r_branch);
paths.push_back(l_branch);
paths.push_back(r_branch);
}
DeleteWaypoints(all_cell_to_delete);
return paths.size();
}
2.3 PredictiveIgnorIdsDP()int PlanningHelpers::PredictiveIgnorIdsDP(WayPoint* pStart, const double& DistanceLimit,
vector& all_cells_to_delete,vector& end_waypoints, std::vector& lanes_ids)
{
if(!pStart) return 0;
vector>nextLeafToTrace;
WayPoint* pZero = 0;
WayPoint* wp = new WayPoint();
*wp = *pStart;
wp->cost = 0;
wp->pLeft = 0;
wp->pRight = 0;
nextLeafToTrace.push_back(make_pair(pZero, wp));//当前搜索列表
all_cells_to_delete.push_back(wp);
double distance = 0;
end_waypoints.clear();
double nCounter = 0;
while(nextLeafToTrace.size()>0)//当前搜索列表不为0
{
nCounter++;//搜寻次数
WayPoint* pH = nextLeafToTrace.at(0).second;//当前搜索列表第一个元素,设为当前搜索元素
assert(pH != 0); // 如果 pH == 0,则程序在此终止,下面的程序都不会执行
<<<<<<< HEAD
nextLeafToTrace.erase(nextLeafToTrace.begin()+0);//从当前搜索列表中除去当前搜索元素
for(unsigned int i =0; i< pH->pFronts.size(); i++)//遍历当前元素的下一个元素
=======
nextLeafToTrace.erase(nextLeafToTrace.begin()+0);
// All points in front of pH
for(unsigned int i =0; i< pH->pFronts.size(); i++)
>>>>>>>1e30b21e93b7e9bd87ade267651cd491da401bd6
{
if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))//若存在,且不再all_cells_to_delete中
{
if(pH->cost< DistanceLimit)//当前点距离代价小于DistanceLimit
{
wp = new WayPoint();
*wp = *pH->pFronts.at(i);
//计算代价
double d = distance2points(wp->pos, pH->pos);
distance += d;
wp->cost = pH->cost + d;
wp->pBacks.push_back(pH);
wp->pLeft = 0;
wp->pRight = 0;
bool bFoundLane = false;
// lanes_ids起始为空,遍历lanes_ids,判断是否存在laneid与wp->laneId相同,若存在,不加入 nextLeafToTrace。
for(unsigned int k = 0 ; k< lanes_ids.size(); k++)
{
if(wp->laneId == lanes_ids.at(k))
{
bFoundLane = true;
break;
}
}
// 如果在 lanes_ids 里找不到 wp 所在的 laneId
if(!bFoundLane)
nextLeafToTrace.push_back(make_pair(pH, wp));
all_cells_to_delete.push_back(wp);
}
else//当前点距离代价大于DistanceLimit
// 如果超出搜索距离
{
end_waypoints.push_back(pH);//加入end_waypoints
}
}
}
}
while(nextLeafToTrace.size()!=0)
nextLeafToTrace.pop_back();
//closed_nodes.clear();
return end_waypoints.size();
}
你是否还在寻找稳定的海外服务器提供商?创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源,准确流量调度确保服务器高可用性,企业级服务器适合批量采购,新人活动首月15元起,快前往官网查看详情吧
我们在微信上24小时期待你的声音
解答本文疑问/技术咨询/运营咨询/技术建议/互联网交流