Multiple Reference Lines Extraction

[Sarrasor]

I'm working on a custom planning algorithm. There is a need to process all available nearby lanes on a current road.

Software

OS: Ubuntu 18.04 Apollo version: 5.0.0

Problem

The problem is, current Map and Routing modules implementation returns a different number of reference lines, depending on the routing request.

Desired behavior

The desired behavior is to have the routing module (or, possibly, pnc_map/reference_line_provider) return all available reference lines on the current road segment. The end goal is to have all available nearby reference lines on the stage of planning behavior execution

Explanation

Consider the following situation:

A road segment has three lanes, named Lane1, Lane2 and Lane3 The current region of interest is bounded by forward and backward offsets

If I send a routing request from Lane1 to Lane1, the routing module provides only one road segment, and, as a consequence, the planning module will receive only one reference line:

At the same time, if I send a routing request from Lane1 to Lane3, the module will provide three reference lines:

I am aware that this behavior is as expected for the current implementation. My question is, what should I modify for it to behave as I need?

[daohu527]

Actually, I did not understand your question. I hope to make the following clarifications before you continue.

Display error

it may be a display error in the second example. Because you need to change lanes, the normal behavior is to output a "s" curve, but due to display problems, the routing line is the start and end of each lane. In fact, I think this is a display error. The actual reference lines are as follows

----
     -----
            -----

[Sarrasor]

Actually, I did not understand your question. I hope to make the following clarifications before you continue.

Display error

it may be a display error in the second example. Because you need to change lanes, the normal behavior is to output a "s" curve, but due to display problems, the routing line is the start and end of each lane. In fact, I think this is a display error. The actual reference lines are as follows

----
     -----
            -----

As far as I know, the "s-curve" behavior you have shown should be generated via start_s and end_s parameters for each segment routing returns.

Here is the routing response that is generated for the situation "Lane1 -> Lane1":

header {
  timestamp_sec: 1610466826.4643917
  module_name: "routing"
  sequence_num: 1
}
road {
  id: "road_14"
  passage {
    segment {
      id: "lane_411"
      start_s: 8.393051683651116
      end_s: 33.357921926775589
    }
    can_exit: true
    change_lane_type: FORWARD
  }
}
measurement {
  distance: 24.964870243124473
}
routing_request {
  header {
    timestamp_sec: 1610466826.4618313
    module_name: "dreamview"
    sequence_num: 4
  }
  waypoint {
    id: "lane_411"
    s: 8.393051683651116
    pose {
      x: 358508.72
      y: 6180759
    }
  }
  waypoint {
    id: "lane_411"
    s: 33.357921926775589
    pose {
      x: 358531.58713421
      y: 6180748.98577267
    }
  }
}
map_version: "gejson2proto_3.0"
status {
  error_code: OK
  msg: "Success!"
}

And here for the situation "Lane1 -> Lane3":

header {
  timestamp_sec: 1610466238.5185287
  module_name: "routing"
  sequence_num: 0
}
road {
  id: "road_14"
  passage {
    segment {
      id: "lane_411"
      start_s: 8.393051683651116
      end_s: 33.338512236424194
    }
    can_exit: false
    change_lane_type: RIGHT
  }
  passage {
    segment {
      id: "lane_410"
      start_s: 8.3578443830557472
      end_s: 33.19866333927078
    }
    can_exit: false
    change_lane_type: RIGHT
  }
  passage {
    segment {
      id: "lane_409"
      start_s: 8.3273970661356351
      end_s: 33.077721840759018
    }
    can_exit: true
    change_lane_type: FORWARD
  }
}
measurement {
  distance: 24.955539394477135
}
routing_request {
  header {
    timestamp_sec: 1610466214.0216548
    module_name: "dreamview"
    sequence_num: 3
  }
  waypoint {
    id: "lane_411"
    s: 8.393051683651116
    pose {
      x: 358508.72
      y: 6180759
    }
  }
  waypoint {
    id: "lane_409"
    s: 33.077721840759018
    pose {
      x: 358528.82730516227
      y: 6180742.7564521823
    }
  }
}
map_version: "gejson2proto_3.0"
status {
  error_code: OK
  msg: "Success!"
}

As can be observed, start_s and end_s are approximately equal for all three segments. So, I do not think it is a display error. Is there anything I misunderstood about the routing module?

In case your picture represents the real situation, I need to modify the routing module to return the following picture for the current situation:

 ------------
 ------------
 ------------

That is, I don't want routing to decide which lane should I take, only which road to follow. So, routing should return all possible reference lines for the current road. Having multiple reference lines, my algorithm will select one of them to follow.

The current version of my algorithm works in situations where multiple lanes are not needed (no obstacle lane following) or when the routing module returns several lanes ("Lane1->Lane2" situation). The issue occurs when, for example, I send a "Lane2->Lane2" request and there is an obstacle in front:

In the picture, you can see the planning trajectory stops before an obstacle, as expected, but the ego vehicle will not use nearby lanes since routing did not return them and I have not implemented lane borrowing

If routing returns more than one lane ("Lane2->Lane1"), the algorithm will select a free lane and perform the lane change:

So, I want to modify the routing module (or other modules) to always return the current and nearby reference lines

[daohu527]

After your introduction, I realized that I did not notice this situation. In fact, Apollo lane is some thing different. A road may have multiple lanes like below.

easy case

lane1_1 lane1_2 lane1_3
lane2_1 lane2_2 lane2_3
lane3_1 lane3_2 lane3_3

then the routing result will return like below

lane1_1
       lane2_2
              lane3_3

This is what I mean, but when there is only one lane, I need to confirm.

question

But your problem may not be related to the above. When the car encounters an obstacle, it can choose to brake or borrow. At this time, it is achieved through rerouting. I know whether you triggered rerouting or triggered the borrowing scene.

The planning module has many scenarios, such as borrowing a lane, overtaking, cruising, etc. You can look at "apollo\modules\planning\scenarios"

[Sarrasor]

question

But your problem may not be related to the above. When the car encounters an obstacle, it can choose to brake or borrow. At this time, it is achieved through rerouting. I know whether you triggered rerouting or triggered the borrowing scene.

The planning module has many scenarios, such as borrowing a lane, overtaking, cruising, etc. You can look at "apollo\modules\planning\scenarios"

Probably, you have meant to check out tasks in the lane_follow scenario, because there are no such scenarios as borrowing a lane, overtaking, cruising. However, there are tasks with such names, for example, path_lane_borrow_decider.

Path lane borrow decider task will decide if it is needed to borrow a lane, and path bounds decider will generate path bounds using this information. This behavior is not desired for me, since my planning module does not use path bounds.

In addition, there are cases, when the borrow decision leads to undesired behavior. For example, this situation:

This decision will cause the ego vehicle to go out of the right lane because there is not enough place to merge to the left after the right lane borrow.

After your introduction, I realized that I did not notice this situation. In fact, Apollo lane is some thing different. A road may have multiple lanes like below.

easy case

lane1_1 lane1_2 lane1_3
lane2_1 lane2_2 lane2_3
lane3_1 lane3_2 lane3_3

then the routing result will return like below

lane1_1
       lane2_2
              lane3_3

Well, it seems this behavior is exactly the one I want to change.

I have analyzed the pnc_map.cc and found out that it creates reference lines based on the routing result:

https://github.com/ApolloAuto/apollo/blob/259756578b4c01ee07b98f72ab9f874818bee200/modules/map/pnc_map/pnc_map.cc#L439-L444

So, if the routing response returns several passages, pnc_map will produce several reference lanes.

Moreover, there is a neighbor search that can be modified such that neighbor_lanes will contain all neighboring lanes to the current one. The only problem is, those lanes will be filtered out since there is only one passage in road.passage(i)

https://github.com/ApolloAuto/apollo/blob/259756578b4c01ee07b98f72ab9f874818bee200/modules/map/pnc_map/pnc_map.cc#L384-L413

So, now I think it is required to change something in the routing module to get it working. Maybe it is possible to modify the result_generator_ in the routing module such that it returns all neighboring passages to the current one?

[daohu527]

Therefore, you need to get the best lane. Whether it’s routing or planning, it’s the same.

Returning all possible combinations of routes may not be a good method. For complex roads, it may return a lot of permutations and combinations.

Let’s take human as an example. Routing will only generate 1-2 navigation paths. But when driving, humans will choose according to the reference route. I think choose to modify the planning decider to achieve this is better.

You will get a reference line and make the best choice based on the current situation. So you may only need to know the adjacent lanes, if two lanes are not suitable for overtaking, then choose to slow down.

[Sarrasor]

Thank you for your suggestions! There are several approaches to try, I would like to try as much as possible to see what works the best.

The problem now, I don't see a way to get the neighbors of a current reference line from within a decider. Any hints on that?

[daohu527]

It depends on the implementation of your algorithm. If it takes 100 meters to change lanes, and the adjacent road is only 50 meters, why choose it? If the adjacent road has 100 meters, then it is also possible to choose it. The lane in apollo is not the same as the ordinary road. A road may be divided into multiple lanes.

I also noticed that the refence line is dynamically generated, and the trigger condition is rerouting, so the solution you mentioned may be feasible, but you should pay attention to only generating the combination of the previous paragraph. If so, it will be more natural to implement in the planning module

lane1_1 lane1_2 lane1_3
lane2_1 lane2_2 lane2_3
lane3_1 lane3_2 lane3_3

For example, if we want from lane1_1 to lane 3_3, there are several combinations.

1_1 1_2 1_3 2_3 3_3
1_1 1_2 2_2 3_2 3_3
1_1 2_1 2_2 2_3 3_3
1_1 2_1 2_2 3_2 3_3
1_1 2_1 3_1 3_2 3_3

If the path is long enough, many reference lines will be generated. Reference lines that are far away have no reference meaning.

[Sarrasor]

I understand what you mean. The problem of permutations is solved differently in my case. The planning algorithm evaluates which lanes should be taken based on a cost function, so the only requirement is to pass all available reference lines to it in an unordered manner.

An additional idea to reduce the number of generated reference lanes is as follows. Consider the situation:

lane1_1 lane1_2 lane1_3
lane2_1 lane2_2 lane2_3
lane3_1 lane3_2 lane3_3

To reduce the number of generated reference lanes, it is possible to use a forward_lookup variable. The purpose of the lookup variable is to restrict the search region. Assume that every lanen_n has a length of 100. So, if we set the forward_lookup = 100, then the lanes of interest are as follows:

lane1_1
lane2_1
lane3_1

Similar for forward_lookup = 200:

lane1_1 lane1_2
lane2_1 lane2_2
lane3_1 lane3_2

The situation with different lane lengths should be possible to resolve with lane stitching.

Yet, I still do not understand which code should be modified to return all currently available neighboring reference lanes.