Does Openplanner support LaneDir from vector map data?
@Hatem
which .csv file and column are you talking about ?
@Hatem, lane.csv, 24th column
Yes, it is a custom column I introduced to the vector map to support semantic based cost calculation for the dynamic programming search.
the column data is either (F, R or L) for (forward, right and left ), it supposed to represent the direction of the lane from driver point of view.
this character should be assigned to at least the start of each lane.
the default is âFâ.
then when the map is loaded a cost is assigned to each lane according to the direction.
then in the planning step this cost is added to the default distance cost.
check BuildPlanningSearchTreeV2 in PlanningHelpers.cpp
and you can change the cost statistically in MappingHelpers.cpp by changing the values.
#define RIGHT_INITIAL_TURNS_COST 0
#define LEFT_INITIAL_TURNS_COST 0
In MappingHelpers::GetLanePoints function, this parameter(pL->LaneDir) is always zero.
I donât know why, you can trace the problem by:
1- make sure you have direction information in the lane.csv file.
2- data is parsed correctly
LaneDir should equal âFâ.
zero means the column doesnât exists in lane.csv
//data.LaneDir = _rec.; this line is commented out in DataRW.cpp
void AisanLanesFileReader::ParseNextLine(const vector_map_msgs::Lane& _rec, AisanLane& data)
{
data.BLID = _rec.blid;
data.BLID2 = _rec.blid2;
data.BLID3 = _rec.blid3;
data.BLID4 = _rec.blid4;
data.BNID = _rec.bnid;
data.ClossID = _rec.clossid;
data.DID = _rec.did;
data.FLID = _rec.flid;
data.FLID2 = _rec.flid2;
data.FLID3 = _rec.flid3;
data.FLID4 = _rec.flid4;
data.FNID = _rec.fnid;
data.JCT = _rec.jct;
data.LCnt = _rec.lcnt;
data.LaneChgFG = _rec.lanecfgfg;
//data.LaneDir = _rec.;
data.LaneType = _rec.lanetype;
//data.LeftLaneId = _rec.;
data.LimitVel = _rec.limitvel;
data.LinkWAID = _rec.linkwaid;
data.LnID = _rec.lnid;
data.Lno = _rec.lno;
data.RefVel = _rec.refvel;
//data.RightLaneId = _rec.;
data.RoadSecID = _rec.roadsecid;
data.Span = _rec.span;
//data.originalMapID = _rec.;
}
I remember now,
there are two functions (parse, read)
parse -> for the messages loaded by map loader in runtime manager, the problem this vector_map_msgs::Lane doesnât include some of the attributes.
but ReadNextLine does read all the attribute.
to solve this issue you should read the map to OpenPlanner in different way.
- in the op_global_planner.launch modify it to read the .csv files directly.
How to change the source in op_behavior_selector.launch?
In LIGHT_INDICATOR PlanningHelpers::GetIndicatorsFromPath() function, size of actionCost is always zero.
for the local planner nodes:
op_trajectory_generator
op_trajectory_evaluator
op_behavior_selector
op_motion_predictor
parameters are set in launch file op_common_params.launch
I think if you load the map correctly in the local planner side , then actionCost will contain values.
In void MappingHelpers::ConstructRoadNetworkFromROSMessageV2() function, i get correct values of actionCost, but in LIGHT_INDICATOR PlanningHelpers::GetIndicatorsFromPath action cost has zero size.
Sorry I didnât test this feature recently,
but one solution is to retrieve the corresponding actionCost for waypoints manually from the map.
or
try to pass âm_TotalPathâ to GetIndicatorsFromPath instead of âm_Pathâ .
hope it will work.
This is my solution:
In void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
std::vector<PlannerHNS::WayPoint> path;
PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path);
// this is start of my solution
for(int iii = 0; iii < path.size(); iii++)
{
PlannerHNS::WayPoint *wppp = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(path[iii], m_Map, true);
if(wppp) {
path[iii].actionCost = wppp->actionCost;
}
}
// end of solution
m_RollOuts.push_back(path);