add README

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
autowarefoundation · mitukou1109 · Nov 1, 2024 · Nov 7, 2024 · Nov 7, 2024 · Nov 7, 2024
commit 409a7911fb8bd51eacf51145f7ad087c3110d804
diff --git a/planning/autoware_path_generator/README.md b/planning/autoware_path_generator/README.md
@@ -0,0 +1,41 @@
+# Path Generator
+
+The `path_generator` node receives a route from `mission_planner` and converts the centerline into a path.
+If the route has waypoints set, it generates a path passing through them.
+
+This package is a simple alternative of `behavior_path_generator`.
+
+## Path generation
+
+When input data is ready, it first searches for the lanelet closest to the vehicle.
+If found, it gets the lanelets within a distance of `backward_path_length` behind and `forward_path_length` in front.
+Their centerlines are concatenated to generate a path.
+
+If waypoints exist in the route, it replaces the overlapped segment of the centerline with them.
+The overlap interval is determined as shown in the following figure.
+
+![waypoint_group_overlap_interval_determination](./media/waypoint_group_overlap_interval_determination.drawio.svg)
+
+## Input topics
+
+| Name                 | Type                                        | Description                      |
+| :------------------- | :------------------------------------------ | :------------------------------- |
+| `~/input/odometry`   | `nav_msgs::msg::Odometry`                   | ego pose                         |
+| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin`     | vector map information           |
+| `~/input/route`      | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal |
+
+## Output topics
+
+| Name            | Type                                       | Description    | QoS Durability |
+| :-------------- | :----------------------------------------- | :------------- | :------------- |
+| `~/output/path` | `tier4_planning_msgs::msg::PathWithLaneId` | generated path | `volatile`     |
+
+## Parameters
+
+| Name                                   | Type     | Description                                                                                                           |
+| :------------------------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------- |
+| `planning_hz`                          | double | planning frequency                                                                                                    |
+| `backward_path_length`                 | double | length of the generated path behind vehicle                                                                           |
+| `forward_path_length`                  | double | length of the generated path in front of vehicle                                                                      |
+| `waypoint_group_separation_threshold`  | double | maximum distance at which consecutive waypoints are considered to belong to the same group (see [here](#path-generation) for details) |
+| `waypoint_group_interval_margin_ratio` | double | ratio for determining length of switching section from centerline to waypoints (see [here](#path-generation) for details)             |
diff --git a/...e_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg b/...e_path_generator/media/waypoint_group_overlap_interval_determination.drawio.svg
diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
  return generate_path(lanelets, s_start, s_end, params);
 }

 std::optional<PathWithLaneId> PathGenerator::generate_path(
  const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end,
  const Params & params) const
 {
  std::vector<PathPointWithLaneId> path_points_with_lane_id{};

  const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) {
    PathPointWithLaneId path_point_with_lane_id{};
    path_point_with_lane_id.lane_ids.push_back(lanelet.id());
    path_point_with_lane_id.point.pose.position =
      lanelet::utils::conversion::toGeomMsgPt(path_point);
    path_point_with_lane_id.point.longitudinal_velocity_mps =
      planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value();
    path_points_with_lane_id.push_back(std::move(path_point_with_lane_id));
  };

  const auto waypoint_groups = utils::get_waypoint_groups(
    lanelets, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold,
    params.waypoint_group_interval_margin_ratio);

  auto extended_lanelets = lanelets;
  auto s_offset = 0.;

  for (const auto & [waypoints, interval] : waypoint_groups) {
    if (interval.first > 0.) {
      continue;
    }
    const auto prev_lanelet =
      utils::get_previous_lanelet_within_route(lanelets.front(), planner_data_);
    if (!prev_lanelet) {
      break;
    }
    extended_lanelets.insert(extended_lanelets.begin(), *prev_lanelet);
    s_offset = lanelet::geometry::length2d(*prev_lanelet);
    break;
  }

  const lanelet::LaneletSequence extended_lanelet_sequence(extended_lanelets);
  std::optional<size_t> overlapping_waypoint_group_index = std::nullopt;

  for (auto lanelet_it = extended_lanelet_sequence.begin();
       lanelet_it != extended_lanelet_sequence.end(); ++lanelet_it) {
    const auto & centerline = lanelet_it->centerline();
    auto s = get_arc_length_along_centerline(extended_lanelet_sequence, centerline.front());

    for (auto point_it = centerline.begin(); point_it != centerline.end(); ++point_it) {
      if (point_it != centerline.begin()) {
        s += lanelet::geometry::distance2d(*std::prev(point_it), *point_it);
      } else if (lanelet_it != extended_lanelet_sequence.begin()) {
        continue;
      }

      if (overlapping_waypoint_group_index) {
        const auto & [waypoints, interval] = waypoint_groups[*overlapping_waypoint_group_index];
        if (s >= interval.first + s_offset && s <= interval.second + s_offset) {
          continue;
        }
        overlapping_waypoint_group_index = std::nullopt;
      }

      for (size_t i = 0; i < waypoint_groups.size(); ++i) {
        const auto & [waypoints, interval] = waypoint_groups[i];
        if (s < interval.first + s_offset || s > interval.second + s_offset) {
          continue;
        }
        for (const auto & waypoint : waypoints) {
          const auto s_waypoint =
            get_arc_length_along_centerline(extended_lanelet_sequence, waypoint);
          for (auto waypoint_lanelet_it = extended_lanelet_sequence.begin();
               waypoint_lanelet_it != extended_lanelet_sequence.end(); ++waypoint_lanelet_it) {
            if (
              s_waypoint > get_arc_length_along_centerline(
                             extended_lanelet_sequence, waypoint_lanelet_it->centerline().back())) {
              continue;
            }
            add_path_point(waypoint, *waypoint_lanelet_it);
            break;
          }
        }
        overlapping_waypoint_group_index = i;
        break;
      }

      if (overlapping_waypoint_group_index) {
        continue;
      }
      add_path_point(*point_it, *lanelet_it);
    }
  }

  s_offset -= get_arc_length_along_centerline(
    extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint(
                                 path_points_with_lane_id.front().point.pose.position));

  auto trajectory = Trajectory::Builder().build(path_points_with_lane_id);
  if (!trajectory) {
    return std::nullopt;
  }

  trajectory->crop(s_offset + s_start, s_end - s_start);

  PathWithLaneId path{};
  path.header.frame_id = planner_data_.route_frame_id;
  path.header.stamp = now();
  path.points = trajectory->restore();

  for (const auto & left_bound_point : extended_lanelet_sequence.leftBound()) {
    path.left_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(left_bound_point));
  }
  for (const auto & right_bound_point : extended_lanelet_sequence.rightBound()) {
    path.right_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(right_bound_point));
  }

  return path;
 }
 }  // namespace autoware::path_generator

 #include "rclcpp_components/register_node_macro.hpp"
diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
 // Copyright 2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 }
 }  // namespace

 std::optional<lanelet::ConstLanelets> get_lanelets_within_route(
  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data,
  const geometry_msgs::msg::Pose & current_pose, const double backward_distance,
  const double forward_distance)
 {
  if (!exists(planner_data.route_lanelets, lanelet)) {
    return std::nullopt;
  }

  const auto arc_coordinates = lanelet::utils::getArcCoordinates({lanelet}, current_pose);
  const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet);

  const auto backward_lanelets = get_lanelets_within_route_up_to(
    lanelet, planner_data, backward_distance - arc_coordinates.length);
  if (!backward_lanelets) {
    return std::nullopt;
  }

  const auto forward_lanelets = get_lanelets_within_route_after(
    lanelet, planner_data, forward_distance - (lanelet_length - arc_coordinates.length));
  if (!forward_lanelets) {
    return std::nullopt;
  }

  lanelet::ConstLanelets lanelets(std::move(*backward_lanelets));
  lanelets.push_back(lanelet);
  std::move(forward_lanelets->begin(), forward_lanelets->end(), std::back_inserter(lanelets));

  return lanelets;
 }

 std::optional<lanelet::ConstLanelets> get_lanelets_within_route_up_to(
  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance)
 {
  if (!exists(planner_data.route_lanelets, lanelet)) {
    return std::nullopt;
  }

  lanelet::ConstLanelets lanelets{};
  auto current_lanelet = lanelet;
  auto length = 0.;

  while (rclcpp::ok() && length < distance) {
    const auto prev_lanelet = get_previous_lanelet_within_route(current_lanelet, planner_data);
    if (!prev_lanelet) {
      break;
    }

    lanelets.push_back(*prev_lanelet);
    current_lanelet = *prev_lanelet;
    length += lanelet::utils::getLaneletLength2d(*prev_lanelet);
  }

  std::reverse(lanelets.begin(), lanelets.end());
  return lanelets;
 }

 std::optional<lanelet::ConstLanelets> get_lanelets_within_route_after(
  const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance)
      "have failed.");
  }

  if (
    next_lanelets.empty() ||
    next_lanelets.front().id() == planner_data.preferred_lanelets.front().id() ||
    !exists(planner_data.route_lanelets, next_lanelets.front())) {
    return std::nullopt;
  }
	// Copyright 2024 TIER IV, Inc.
Check warning on line 1 in planning/autoware_path_generator/src/node.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Overall Code Complexity `This module has a mean cyclomatic complexity of 5.45 across 11 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.`
	//
	// Licensed under the Apache License, Version 2.0 (the "License");
	// you may not use this file except in compliance with the License.
	return generate_path(lanelets, s_start, s_end, params);
	}

	std::optional<PathWithLaneId> PathGenerator::generate_path(
	const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end,
	const Params & params) const
	{
	std::vector<PathPointWithLaneId> path_points_with_lane_id{};

	const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) {
	PathPointWithLaneId path_point_with_lane_id{};
	path_point_with_lane_id.lane_ids.push_back(lanelet.id());
	path_point_with_lane_id.point.pose.position =
	lanelet::utils::conversion::toGeomMsgPt(path_point);
	path_point_with_lane_id.point.longitudinal_velocity_mps =
	planner_data_.traffic_rules_ptr->speedLimit(lanelet).speedLimit.value();
	path_points_with_lane_id.push_back(std::move(path_point_with_lane_id));
	};

	const auto waypoint_groups = utils::get_waypoint_groups(
	lanelets, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold,
	params.waypoint_group_interval_margin_ratio);

	auto extended_lanelets = lanelets;
	auto s_offset = 0.;

	for (const auto & [waypoints, interval] : waypoint_groups) {
	if (interval.first > 0.) {
	continue;
	}
	const auto prev_lanelet =
	utils::get_previous_lanelet_within_route(lanelets.front(), planner_data_);
	if (!prev_lanelet) {
	break;
	}
	extended_lanelets.insert(extended_lanelets.begin(), *prev_lanelet);
	s_offset = lanelet::geometry::length2d(*prev_lanelet);
	break;
	}

	const lanelet::LaneletSequence extended_lanelet_sequence(extended_lanelets);
	std::optional<size_t> overlapping_waypoint_group_index = std::nullopt;

	for (auto lanelet_it = extended_lanelet_sequence.begin();
	lanelet_it != extended_lanelet_sequence.end(); ++lanelet_it) {
	const auto & centerline = lanelet_it->centerline();
	auto s = get_arc_length_along_centerline(extended_lanelet_sequence, centerline.front());

	for (auto point_it = centerline.begin(); point_it != centerline.end(); ++point_it) {
	if (point_it != centerline.begin()) {
	s += lanelet::geometry::distance2d(std::prev(point_it), point_it);
	} else if (lanelet_it != extended_lanelet_sequence.begin()) {
	continue;
	}

	if (overlapping_waypoint_group_index) {
	const auto & [waypoints, interval] = waypoint_groups[*overlapping_waypoint_group_index];
	if (s >= interval.first + s_offset && s <= interval.second + s_offset) {
	continue;
	}
	overlapping_waypoint_group_index = std::nullopt;
	}

	for (size_t i = 0; i < waypoint_groups.size(); ++i) {
	const auto & [waypoints, interval] = waypoint_groups[i];
	if (s < interval.first + s_offset \|\| s > interval.second + s_offset) {
	continue;
	}
	for (const auto & waypoint : waypoints) {
	const auto s_waypoint =
	get_arc_length_along_centerline(extended_lanelet_sequence, waypoint);
	for (auto waypoint_lanelet_it = extended_lanelet_sequence.begin();
	waypoint_lanelet_it != extended_lanelet_sequence.end(); ++waypoint_lanelet_it) {
	if (
	s_waypoint > get_arc_length_along_centerline(
	extended_lanelet_sequence, waypoint_lanelet_it->centerline().back())) {
	continue;
	}
	add_path_point(waypoint, *waypoint_lanelet_it);
	break;
	}
	}
	overlapping_waypoint_group_index = i;
	break;
	}

	if (overlapping_waypoint_group_index) {
	continue;
	}
	add_path_point(point_it, lanelet_it);
	}
	}

	s_offset -= get_arc_length_along_centerline(
	extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint(
	path_points_with_lane_id.front().point.pose.position));

	auto trajectory = Trajectory::Builder().build(path_points_with_lane_id);
	if (!trajectory) {
	return std::nullopt;
	}

	trajectory->crop(s_offset + s_start, s_end - s_start);

	PathWithLaneId path{};
	path.header.frame_id = planner_data_.route_frame_id;
	path.header.stamp = now();
	path.points = trajectory->restore();

	for (const auto & left_bound_point : extended_lanelet_sequence.leftBound()) {
	path.left_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(left_bound_point));
	}
	for (const auto & right_bound_point : extended_lanelet_sequence.rightBound()) {
	path.right_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(right_bound_point));
	}

	return path;
	}
Check warning on line 363 in planning/autoware_path_generator/src/node.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Complex Method `PathGenerator::generate_path has a cyclomatic complexity of 27, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.` Check warning on line 363 in planning/autoware_path_generator/src/node.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Bumpy Road Ahead `PathGenerator::generate_path has 5 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.` Check warning on line 363 in planning/autoware_path_generator/src/node.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main) ❌ New issue: Deep, Nested Complexity `PathGenerator::generate_path has a nested complexity depth of 6, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.`
	} // namespace autoware::path_generator

	#include "rclcpp_components/register_node_macro.hpp"