Calculate the sampling width for the planner
| 635 | |
| 636 | // Calculate the sampling width for the planner |
| 637 | std::vector<double> FissPlannerNode::getSamplingWidthFromTargetLane(const int lane_id, const double vehicle_width, const double current_lane_width, |
| 638 | const double left_lane_width, const double right_lane_width) |
| 639 | { |
| 640 | double left_bound, right_bound; |
| 641 | |
| 642 | switch (lane_id) |
| 643 | { |
| 644 | // all lanes |
| 645 | case LaneID::ALL_LANES: |
| 646 | left_bound = current_lane_width/2 + left_lane_width; |
| 647 | right_bound = -current_lane_width/2 - right_lane_width; |
| 648 | ROS_INFO("Local Planner: Sampling On ALL Lanes"); |
| 649 | break; |
| 650 | |
| 651 | // stay within the current lane |
| 652 | case LaneID::CURR_LANE: |
| 653 | left_bound = current_lane_width/2; |
| 654 | right_bound = -current_lane_width/2; |
| 655 | ROS_INFO("Local Planner: Sampling On The Current Lane"); |
| 656 | break; |
| 657 | |
| 658 | // change to left lane |
| 659 | case LaneID::LEFT_LANE: |
| 660 | left_bound = current_lane_width/2 + left_lane_width; |
| 661 | right_bound = current_lane_width/2; |
| 662 | ROS_INFO("Local Planner: Sampling On The Left Lane"); |
| 663 | break; |
| 664 | // change to right lane |
| 665 | case LaneID::RIGHT_LANE: |
| 666 | left_bound = -current_lane_width/2; |
| 667 | right_bound = -current_lane_width/2 - right_lane_width; |
| 668 | ROS_INFO("Local Planner: Sampling On The Right Lane"); |
| 669 | break; |
| 670 | } |
| 671 | |
| 672 | return {left_bound - vehicle_width/2, right_bound + vehicle_width/2}; |
| 673 | } |
| 674 | |
| 675 | // Select the ideal lane to proceed |
| 676 | FrenetPath FissPlannerNode::selectLane(const std::vector<FrenetPath>& best_traj_list, const int current_lane) |
nothing calls this directly
no outgoing calls
no test coverage detected