Skip to content

Commit

Permalink
fix(behavior_velocity_planner): sort unique_lane_ids for merge_from_p…
Browse files Browse the repository at this point in the history 
…rivate_area (#1323)

* fix(behavior_velocity_planner): sort unique_lane_ids for merge_from_private_area

Signed-off-by: tomoya.kimura <[email protected]>

* use same function

Signed-off-by: tomoya.kimura <[email protected]>

* Apply suggestions from code review

* fix

Signed-off-by: tomoya.kimura <[email protected]>

* Apply suggestions from code review

Co-authored-by: taikitanaka3 <[email protected]>

* Apply suggestions from code review

Co-authored-by: taikitanaka3 <[email protected]>

Co-authored-by: taikitanaka3 <[email protected]>

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@tkimura4 @kosuke55
tkimura4 authored and kosuke55 committed Sep 15, 2022
1 parent be72a49 commit 2560d15
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 48 deletions.
36 changes: 18 additions & 18 deletions planning/behavior_velocity_planner/include/utilization/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -313,36 +313,36 @@ std::vector<T> concatVector(const std::vector<T> & vec1, const std::vector<T> &
return concat_vec;
}

boost::optional<int64_t> getNearestLaneId(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose,
boost::optional<size_t> & nearest_segment_idx);

template <class T>
std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> getRegElemMapOnPath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
{
std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> reg_elem_map_on_path;
std::set<int64_t> unique_lane_ids;
auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);

// Add current lane id
lanelet::ConstLanelets current_lanes;
if (
lanelet::utils::query::getCurrentLanelets(
lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
nearest_segment_idx) {
for (const auto & ll : current_lanes) {
if (
ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
unique_lane_ids.insert(ll.id());
}
}
boost::optional<size_t> nearest_segment_idx;
const auto nearest_lane_id =
getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx);

std::vector<int64_t> unique_lane_ids;
if (nearest_lane_id) {
unique_lane_ids.emplace_back(*nearest_lane_id);
}

// Add forward path lane_id
const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
for (size_t i = start_idx; i < path.points.size(); i++) {
unique_lane_ids.insert(
path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was.
const int64_t lane_id = path.points.at(i).lane_ids.at(0);
if (
std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
unique_lane_ids.emplace_back(lane_id);
}
}

for (const auto lane_id : unique_lane_ids) {
Expand Down
31 changes: 13 additions & 18 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <scene_module/crosswalk/manager.hpp>
#include <utilization/util.hpp>

#include <memory>
#include <set>
Expand Down Expand Up @@ -50,34 +51,28 @@ std::vector<lanelet::ConstLanelet> CrosswalkModuleManager::getCrosswalksOnPath(
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
{
std::vector<lanelet::ConstLanelet> crosswalks;
std::set<int64_t> unique_lane_ids;

auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
path.points, planner_data_->current_pose.pose, std::numeric_limits<double>::max(), M_PI_2);

// Add current lane id
lanelet::ConstLanelets current_lanes;
if (
lanelet::utils::query::getCurrentLanelets(
lanelet::utils::query::laneletLayer(lanelet_map), planner_data_->current_pose.pose,
&current_lanes) &&
nearest_segment_idx) {
for (const auto & ll : current_lanes) {
if (
ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
unique_lane_ids.insert(ll.id());
}
}
const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId(
path, lanelet_map, planner_data_->current_pose.pose, nearest_segment_idx);

std::vector<int64_t> unique_lane_ids;
if (nearest_lane_id) {
unique_lane_ids.emplace_back(*nearest_lane_id);
}

// Add forward path lane_id
const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
for (size_t i = start_idx; i < path.points.size(); i++) {
unique_lane_ids.insert(
path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was.
const int64_t lane_id = path.points.at(i).lane_ids.at(0);
if (
std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
unique_lane_ids.emplace_back(lane_id);
}
}

for (const auto lane_id : unique_lane_ids) {
const auto ll = lanelet_map->laneletLayer.get(lane_id);

Expand Down
56 changes: 44 additions & 12 deletions planning/behavior_velocity_planner/src/utilization/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -593,34 +593,66 @@ LineString2d extendLine(
{(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}};
}

std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
boost::optional<int64_t> getNearestLaneId(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose,
boost::optional<size_t> & nearest_segment_idx)
{
std::set<int64_t> unique_lane_ids;
auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
boost::optional<int64_t> nearest_lane_id;

nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);

// Add current lane id
if (!nearest_segment_idx) {
return boost::none;
}

lanelet::ConstLanelets current_lanes;
if (
lanelet::utils::query::getCurrentLanelets(
lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
nearest_segment_idx) {
for (const auto & ll : current_lanes) {
if (
ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
unique_lane_ids.insert(ll.id());
if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
nearest_lane_id = ll.id();
return nearest_lane_id;
}
}

// if the lane_id of nearest_segment_idx does not involved in current_lanes,
// search the lane_id of nearest_segment_idx + 1
*nearest_segment_idx += 1;
for (const auto & ll : current_lanes) {
if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
nearest_lane_id = ll.id();
return nearest_lane_id;
}
}
}
return boost::none;
}

std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
{
boost::optional<size_t> nearest_segment_idx;
const auto nearest_lane_id =
getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx);

std::vector<int64_t> unique_lane_ids;
if (nearest_lane_id) {
unique_lane_ids.emplace_back(*nearest_lane_id);
}

// Add forward path lane_id
const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
for (size_t i = start_idx; i < path.points.size(); i++) {
unique_lane_ids.insert(
path.points.at(i).lane_ids.at(0)); // should we iterate ids? keep as it was.
const int64_t lane_id = path.points.at(i).lane_ids.at(0);
if (
std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
unique_lane_ids.emplace_back(lane_id);
}
}

std::vector<lanelet::ConstLanelet> lanelets;
Expand Down

0 comments on commit 2560d15

Please sign in to comment.