diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3773b6d2a5..341fff96ba 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -14,8 +14,9 @@ demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-g localization/autoware_ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp lxg19892021@gmail.com zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-core-global-codeowners -planning/autoware_path_generator/** mitsuhiro.sakamoto@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners +planning/autoware_path_generator/** kosuke.takeuchi@tier4.jp mitsuhiro.sakamoto@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners sensing/autoware_gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +testing/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-core-global-codeowners testing/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners testing/autoware_test_utils/** egon.kang@autocore.ai kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp yukinari.hisaki.2@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-core-global-codeowners testing/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners diff --git a/planning/autoware_path_generator/CMakeLists.txt b/planning/autoware_path_generator/CMakeLists.txt index bac4fb7498..99ffc3c5bc 100644 --- a/planning/autoware_path_generator/CMakeLists.txt +++ b/planning/autoware_path_generator/CMakeLists.txt @@ -16,7 +16,7 @@ target_link_libraries(${PROJECT_NAME} path_generator_parameters ) -target_compile_options(${PROJECT_NAME} PRIVATE +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-declarations ) @@ -25,4 +25,18 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE path_generator_node ) -ament_auto_package() +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_path_generator_node_interface.cpp + test/test_lanelet.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + test_route +) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index b6ed2383e3..7a4ed323d6 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -7,3 +7,8 @@ waypoint_group: separation_threshold: 1.0 interval_margin_ratio: 10.0 + turn_signal: + search_time: 3.0 + search_distance: 30.0 + resampling_interval: 1.0 + angle_threshold_deg: 15.0 diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 349bf52dd2..08a90f3880 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -18,8 +18,11 @@ #include "autoware/path_generator/common_structs.hpp" #include +#include #include +#include +#include #include #include @@ -27,6 +30,12 @@ namespace autoware::path_generator { using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + +const std::unordered_map turn_signal_command_map = { + {"left", TurnIndicatorsCommand::ENABLE_LEFT}, + {"right", TurnIndicatorsCommand::ENABLE_RIGHT}, + {"straight", TurnIndicatorsCommand::DISABLE}}; namespace utils { @@ -222,6 +231,33 @@ bool is_path_valid( PathWithLaneId modify_path_for_smooth_goal_connection( const PathWithLaneId & path, const std::shared_ptr & planner_data); +/** + * @brief get earliest turn signal based on turn direction specified for lanelets + * @param path target path + * @param planner_data planner data + * @param current_pose current pose of ego vehicle + * @param current_vel current longitudinal velocity of ego vehicle + * @param search_distance base search distance + * @param search_time time to extend search distance + * @param angle_threshold_deg angle threshold for required end point determination + * @param base_link_to_front distance from base link to front of ego vehicle + * @return turn signal + */ +TurnIndicatorsCommand get_turn_signal( + const PathWithLaneId & path, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double current_vel, + const double search_distance, const double search_time, const double angle_threshold_deg, + const double base_link_to_front); + +/** + * @brief get required end point for turn signal activation + * @param lanelet target lanelet + * @param angle_threshold_deg yaw angle difference threshold + * @return required end point + */ + +std::optional get_turn_signal_required_end_point( + const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index 0cd1ade697..7d54f49778 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -21,12 +21,18 @@ autoware_internal_planning_msgs autoware_lanelet2_extension autoware_motion_utils + autoware_planning_test_manager + autoware_test_utils autoware_trajectory autoware_vehicle_info_utils generate_parameter_library rclcpp rclcpp_components + ament_cmake_ros + ament_lint_auto + autoware_lint_common + ament_cmake diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index 2c7192427b..a3ac8231cd 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -15,3 +15,16 @@ path_generator: interval_margin_ratio: type: double + + turn_signal: + search_time: + type: double + + search_distance: + type: double + + resampling_interval: + type: double + + angle_threshold_deg: + type: double diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 62374a70c0..862394517d 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -55,6 +55,9 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) path_publisher_ = create_publisher("~/output/path", 1); + turn_signal_publisher_ = + create_publisher("~/output/turn_indicators_cmd", 1); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const auto params = param_listener_->get_params(); @@ -71,12 +74,21 @@ void PathGenerator::run() return; } - const auto path = plan_path(input_data); + const auto param = param_listener_->get_params(); + const auto path = plan_path(input_data, param); if (!path) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); return; } + auto turn_signal = utils::get_turn_signal( + *path, planner_data_, input_data.odometry_ptr->pose.pose, + input_data.odometry_ptr->twist.twist.linear.x, param.turn_signal.search_distance, + param.turn_signal.search_time, param.turn_signal.angle_threshold_deg, + vehicle_info_.max_longitudinal_offset_m); + turn_signal.stamp = now(); + turn_signal_publisher_->publish(turn_signal); + path_publisher_->publish(*path); } @@ -188,10 +200,10 @@ bool PathGenerator::is_data_ready(const InputData & input_data) return true; } -std::optional PathGenerator::plan_path(const InputData & input_data) +std::optional PathGenerator::plan_path( + const InputData & input_data, const Params & params) { - const auto path = - generate_path(input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + const auto path = generate_path(input_data.odometry_ptr->pose.pose, params); if (!path) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 7a7db18563..7f7fc40505 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::Odometry; using ::path_generator::Params; using Trajectory = autoware::trajectory::Trajectory; @@ -64,6 +66,7 @@ class PathGenerator : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr path_publisher_; + rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -82,7 +85,7 @@ class PathGenerator : public rclcpp::Node bool is_data_ready(const InputData & input_data); - std::optional plan_path(const InputData & input_data); + std::optional plan_path(const InputData & input_data, const Params & params); std::optional generate_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const; diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index acc82f4300..b7a80385aa 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -14,10 +14,16 @@ #include "autoware/path_generator/utils.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include +#include #include +#include #include #include #include +#include #include @@ -386,6 +392,7 @@ std::vector get_path_bound( return path_bound; } +<<<<<<< HEAD // goal does not have z bool set_goal( const double search_radius_range, [[maybe_unused]] const double search_rad_range, @@ -614,6 +621,132 @@ PathWithLaneId modify_path_for_smooth_goal_connection( goal_search_radius -= range_reduce_by; } return refined_path; +======= +TurnIndicatorsCommand get_turn_signal( + const PathWithLaneId & path, const PlannerData & planner_data, + const geometry_msgs::msg::Pose & current_pose, const double current_vel, + const double search_distance, const double search_time, const double angle_threshold_deg, + const double base_link_to_front) +{ + TurnIndicatorsCommand turn_signal; + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + + const lanelet::BasicPoint2d current_point{current_pose.position.x, current_pose.position.y}; + const auto base_search_distance = search_distance + current_vel * search_time; + + std::vector searched_lanelet_ids = {}; + std::optional arc_length_from_vehicle_front_to_lanelet_start = std::nullopt; + + auto calc_arc_length = + [&](const lanelet::ConstLanelet & lanelet, const lanelet::BasicPoint2d & point) -> double { + return lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), point).length; + }; + + for (const auto & point : path.points) { + for (const auto & lane_id : point.lane_ids) { + if (exists(searched_lanelet_ids, lane_id)) { + continue; + } + searched_lanelet_ids.push_back(lane_id); + + const auto lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id); + if (!get_next_lanelet_within_route(lanelet, planner_data)) { + continue; + } + + if ( + !arc_length_from_vehicle_front_to_lanelet_start && + !lanelet::geometry::inside(lanelet, current_point)) { + continue; + } + + if (lanelet.hasAttribute("turn_direction")) { + turn_signal.command = + turn_signal_command_map.at(lanelet.attribute("turn_direction").value()); + + if (arc_length_from_vehicle_front_to_lanelet_start) { // ego is in front of lanelet + if ( + *arc_length_from_vehicle_front_to_lanelet_start > + lanelet.attributeOr("turn_signal_distance", base_search_distance)) { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + } + return turn_signal; + } + + // ego is inside lanelet + const auto required_end_point_opt = + get_turn_signal_required_end_point(lanelet, angle_threshold_deg); + if (!required_end_point_opt) continue; + if ( + calc_arc_length(lanelet, current_point) <= + calc_arc_length(lanelet, required_end_point_opt.value())) { + return turn_signal; + } + } + + const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + if (arc_length_from_vehicle_front_to_lanelet_start) { + *arc_length_from_vehicle_front_to_lanelet_start += lanelet_length; + } else { + arc_length_from_vehicle_front_to_lanelet_start = + lanelet_length - calc_arc_length(lanelet, current_point) - base_link_to_front; + } + break; + } + } + + return turn_signal; +} + +std::optional get_turn_signal_required_end_point( + const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg) +{ + std::vector centerline_poses(lanelet.centerline().size()); + std::transform( + lanelet.centerline().begin(), lanelet.centerline().end(), centerline_poses.begin(), + [](const auto & point) { + geometry_msgs::msg::Pose pose{}; + pose.position = lanelet::utils::conversion::toGeomMsgPt(point); + return pose; + }); + + // NOTE: Trajectory does not support less than 4 points, so resample if less than 4. + // This implementation should be replaced in the future + if (centerline_poses.size() < 4) { + const auto lanelet_length = autoware::motion_utils::calcArcLength(centerline_poses); + const auto resampling_interval = lanelet_length / 4.0; + std::vector resampled_arclength; + for (double s = 0.0; s < lanelet_length; s += resampling_interval) { + resampled_arclength.push_back(s); + } + if (lanelet_length - resampled_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampled_arclength.back() = lanelet_length; + } else { + resampled_arclength.push_back(lanelet_length); + } + centerline_poses = + autoware::motion_utils::resamplePoseVector(centerline_poses, resampled_arclength); + if (centerline_poses.size() < 4) return std::nullopt; + } + + auto centerline = + autoware::trajectory::Trajectory::Builder{}.build(centerline_poses); + if (!centerline) return std::nullopt; + centerline->align_orientation_with_trajectory_direction(); + + const auto terminal_yaw = tf2::getYaw(centerline->compute(centerline->length()).orientation); + const auto intervals = autoware::trajectory::find_intervals( + centerline.value(), + [terminal_yaw, angle_threshold_deg](const geometry_msgs::msg::Pose & point) { + const auto yaw = tf2::getYaw(point.orientation); + return std::fabs(autoware_utils::normalize_radian(yaw - terminal_yaw)) < + autoware_utils::deg2rad(angle_threshold_deg); + }); + if (intervals.empty()) return std::nullopt; + + return lanelet::utils::conversion::toLaneletPoint( + centerline->compute(intervals.front().start).position); +>>>>>>> main } } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/test/test_lanelet.cpp b/planning/autoware_path_generator/test/test_lanelet.cpp new file mode 100644 index 0000000000..3fead2b95c --- /dev/null +++ b/planning/autoware_path_generator/test/test_lanelet.cpp @@ -0,0 +1,234 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils_test.hpp" + +#include + +#include + +namespace autoware::path_generator +{ +TEST_F(UtilsTest, getPreviousLaneletWithinRoute) +{ + { // Normal case + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50); + + const auto prev_lanelet = utils::get_previous_lanelet_within_route(lanelet, planner_data_); + + ASSERT_TRUE(prev_lanelet.has_value()); + ASSERT_EQ(prev_lanelet->id(), 125); + } + + { // The given lanelet is at the start of the route + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10323); + + const auto prev_lanelet = utils::get_previous_lanelet_within_route(lanelet, planner_data_); + + ASSERT_FALSE(prev_lanelet.has_value()); + } +} + +TEST_F(UtilsTest, getNextLaneletWithinRoute) +{ + { // Normal case + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50); + + const auto next_lanelet = utils::get_next_lanelet_within_route(lanelet, planner_data_); + + ASSERT_TRUE(next_lanelet.has_value()); + ASSERT_EQ(next_lanelet->id(), 122); + } + + { // The given lanelet is at the end of the route + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(122); + + const auto next_lanelet = utils::get_next_lanelet_within_route(lanelet, planner_data_); + + ASSERT_FALSE(next_lanelet.has_value()); + } +} + +TEST_F(UtilsTest, getLaneletsWithinRouteUpTo) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + { // Normal case + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50); + const auto distance = 30.0; + + const auto prev_lanelets = + utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance); + + ASSERT_TRUE(prev_lanelets.has_value()); + ASSERT_EQ(prev_lanelets->size(), 1); + ASSERT_EQ(prev_lanelets->at(0).id(), 125); + } + + { // The given distance exceeds the route section + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50); + const auto distance = 100.0; + + const auto prev_lanelets = + utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance); + + ASSERT_TRUE(prev_lanelets.has_value()); + ASSERT_EQ(prev_lanelets->size(), 2); + ASSERT_EQ(prev_lanelets->at(0).id(), 10323); + ASSERT_EQ(prev_lanelets->at(1).id(), 125); + } + + { // The given distance is negative + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(50); + const auto distance = -30.0; + + const auto prev_lanelets = + utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance); + + ASSERT_TRUE(prev_lanelets.has_value()); + ASSERT_TRUE(prev_lanelets->empty()); + } + + { // The given lanelet is not within the route + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257); + const auto distance = 30.0; + + const auto prev_lanelets = + utils::get_lanelets_within_route_up_to(lanelet, planner_data_, distance); + + ASSERT_FALSE(prev_lanelets.has_value()); + } +} + +TEST_F(UtilsTest, getLaneletsWithinRouteAfter) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + { // Normal case + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125); + const auto distance = 30.0; + + const auto next_lanelets = + utils::get_lanelets_within_route_after(lanelet, planner_data_, distance); + + ASSERT_TRUE(next_lanelets.has_value()); + ASSERT_EQ(next_lanelets->size(), 2); + ASSERT_EQ(next_lanelets->at(0).id(), 50); + ASSERT_EQ(next_lanelets->at(1).id(), 122); + } + + { // The given distance exceeds the route section + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125); + const auto distance = 100.0; + + const auto next_lanelets = + utils::get_lanelets_within_route_after(lanelet, planner_data_, distance); + + ASSERT_TRUE(next_lanelets.has_value()); + ASSERT_EQ(next_lanelets->size(), 2); + ASSERT_EQ(next_lanelets->at(0).id(), 50); + ASSERT_EQ(next_lanelets->at(1).id(), 122); + } + + { // The given distance is negative + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(125); + const auto distance = -30.0; + + const auto next_lanelets = + utils::get_lanelets_within_route_after(lanelet, planner_data_, distance); + + ASSERT_TRUE(next_lanelets.has_value()); + ASSERT_TRUE(next_lanelets->empty()); + } + + { // The given lanelet is not within the route + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257); + const auto distance = 30.0; + + const auto next_lanelets = + utils::get_lanelets_within_route_after(lanelet, planner_data_, distance); + + ASSERT_FALSE(next_lanelets.has_value()); + } +} + +TEST_F(UtilsTest, getLaneletsWithinRoute) +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + { // Normal case + const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0); + const auto lanelet = get_lanelet_closest_to_pose(pose); + const auto backward_distance = 40.0; + const auto forward_distance = 40.0; + + const auto lanelets = utils::get_lanelets_within_route( + lanelet, planner_data_, pose, backward_distance, forward_distance); + + ASSERT_TRUE(lanelets.has_value()); + ASSERT_EQ(lanelets->size(), 3); + ASSERT_EQ(lanelets->at(0).id(), 125); + ASSERT_EQ(lanelets->at(1).id(), 50); + ASSERT_EQ(lanelets->at(2).id(), 122); + } + + { // The given backward distance is too small to search for the previous lanelets + const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0); + const auto lanelet = get_lanelet_closest_to_pose(pose); + const auto backward_distance = 5.0; + const auto forward_distance = 40.0; + + const auto lanelets = utils::get_lanelets_within_route( + lanelet, planner_data_, pose, backward_distance, forward_distance); + + ASSERT_TRUE(lanelets.has_value()); + ASSERT_EQ(lanelets->size(), 2); + ASSERT_EQ(lanelets->at(0).id(), 50); + ASSERT_EQ(lanelets->at(1).id(), 122); + } + + { // The given forward distance is too small to search for the next lanelets + const auto pose = autoware::test_utils::createPose(3765.606, 73746.328, 0.0, 0.0, 0.0, 0.0); + const auto lanelet = get_lanelet_closest_to_pose(pose); + const auto backward_distance = 40.0; + const auto forward_distance = 5.0; + + const auto lanelets = utils::get_lanelets_within_route( + lanelet, planner_data_, pose, backward_distance, forward_distance); + + ASSERT_TRUE(lanelets.has_value()); + ASSERT_EQ(lanelets->size(), 2); + ASSERT_EQ(lanelets->at(0).id(), 125); + ASSERT_EQ(lanelets->at(1).id(), 50); + } + + { // The given lanelet is not within the route + const auto pose = autoware::test_utils::createPose(3746.81, 73775.84, 0.0, 0.0, 0.0, 0.0); + const auto lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(10257); + const auto backward_distance = 40.0; + const auto forward_distance = 40.0; + + const auto lanelets = utils::get_lanelets_within_route( + lanelet, planner_data_, pose, backward_distance, forward_distance); + + ASSERT_FALSE(lanelets.has_value()); + } +} +} // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/test/test_path_generator_node_interface.cpp b/planning/autoware_path_generator/test/test_path_generator_node_interface.cpp new file mode 100644 index 0000000000..a4599b617b --- /dev/null +++ b/planning/autoware_path_generator/test/test_path_generator_node_interface.cpp @@ -0,0 +1,72 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/node.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + + auto test_manager = + std::make_shared(); + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto path_generator_dir = + ament_index_cpp::get_package_share_directory("autoware_path_generator"); + + const auto node_options = rclcpp::NodeOptions{}.arguments( + {"--ros-args", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", + path_generator_dir + "/config/path_generator.param.yaml"}); + + auto test_target_node = std::make_shared(node_options); + + // publish necessary topics from test_manager + test_manager->publishInput( + test_target_node, "path_generator/input/vector_map", autoware::test_utils::makeMapBinMsg()); + test_manager->publishInput( + test_target_node, "path_generator/input/odometry", autoware::test_utils::makeOdometry()); + + // create subscriber in test_manager + test_manager->subscribeOutput( + "path_generator/output/path"); + + const std::string route_topic_name = "path_generator/input/route"; + + // test with normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithBehaviorNormalRoute(test_target_node, route_topic_name)); + + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with trajectory with empty/one point/overlapping point + ASSERT_NO_THROW_WITH_ERROR_MSG( + test_manager->testWithAbnormalRoute(test_target_node, route_topic_name)); + + rclcpp::shutdown(); +} diff --git a/planning/autoware_path_generator/test/utils_test.hpp b/planning/autoware_path_generator/test/utils_test.hpp new file mode 100644 index 0000000000..b06e9efc51 --- /dev/null +++ b/planning/autoware_path_generator/test/utils_test.hpp @@ -0,0 +1,120 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_TEST_HPP_ +#define UTILS_TEST_HPP_ + +#include "autoware/path_generator/utils.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" + +#include +#include + +#include + +#include + +namespace autoware::path_generator +{ +class UtilsTest : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto lanelet_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet_map_path); + + planner_data_.lanelet_map_ptr = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + map_bin_msg, planner_data_.lanelet_map_ptr, &planner_data_.traffic_rules_ptr, + &planner_data_.routing_graph_ptr); + + const auto route_path = autoware::test_utils::get_absolute_path_to_route( + "autoware_path_generator", "route_data.yaml"); + const auto route = + autoware::test_utils::parse>( + route_path); + if (!route) { + throw std::runtime_error( + "Failed to parse YAML file: " + route_path + ". The file might be corrupted."); + } + + const auto path_path = + autoware::test_utils::get_absolute_path_to_route("autoware_path_generator", "path_data.yaml"); + try { + path_ = autoware::test_utils::parse< + std::optional>(path_path) + .value(); + } catch (const std::exception &) { + throw std::runtime_error( + "Failed to parse YAML file: " + path_path + ". The file might be corrupted."); + } + + planner_data_.route_frame_id = route->header.frame_id; + planner_data_.goal_pose = route->goal_pose; + + planner_data_.route_lanelets.clear(); + planner_data_.preferred_lanelets.clear(); + planner_data_.start_lanelets.clear(); + planner_data_.goal_lanelets.clear(); + + size_t primitives_num = 0; + for (const auto & route_section : route->segments) { + primitives_num += route_section.primitives.size(); + } + planner_data_.route_lanelets.reserve(primitives_num); + + for (const auto & route_section : route->segments) { + for (const auto & primitive : route_section.primitives) { + const auto id = primitive.id; + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(id); + planner_data_.route_lanelets.push_back(lanelet); + if (id == route_section.preferred_primitive.id) { + planner_data_.preferred_lanelets.push_back(lanelet); + } + } + } + + const auto set_lanelets_from_segment = + [&]( + const autoware_planning_msgs::msg::LaneletSegment & segment, + lanelet::ConstLanelets & lanelets) { + lanelets.reserve(segment.primitives.size()); + for (const auto & primitive : segment.primitives) { + const auto & lanelet = planner_data_.lanelet_map_ptr->laneletLayer.get(primitive.id); + lanelets.push_back(lanelet); + } + }; + set_lanelets_from_segment(route->segments.front(), planner_data_.start_lanelets); + set_lanelets_from_segment(route->segments.back(), planner_data_.goal_lanelets); + } + + lanelet::ConstLanelet get_lanelet_closest_to_pose(const geometry_msgs::msg::Pose & pose) const + { + lanelet::ConstLanelet lanelet; + if (!lanelet::utils::query::getClosestLanelet( + planner_data_.preferred_lanelets, pose, &lanelet)) { + throw std::runtime_error("Failed to get the closest lanelet to the given point."); + } + return lanelet; + } + + PlannerData planner_data_; + autoware_internal_planning_msgs::msg::PathWithLaneId path_; +}; +} // namespace autoware::path_generator + +#endif // UTILS_TEST_HPP_ diff --git a/planning/autoware_path_generator/test_route/path_data.yaml b/planning/autoware_path_generator/test_route/path_data.yaml new file mode 100644 index 0000000000..ca07fad977 --- /dev/null +++ b/planning/autoware_path_generator/test_route/path_data.yaml @@ -0,0 +1,606 @@ +header: + stamp: + sec: 0 + nanosec: 0 + frame_id: map +points: + - point: + pose: + position: + x: 3826.1124000007694 + y: 73774.42880000081 + z: 19.3582 + orientation: + x: 0.0 + y: -0.010408059262119533 + z: -0.9705764902524712 + w: 0.24056797141679348 + longitudinal_velocity_mps: 2.777777671813965 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 10323 + - point: + pose: + position: + x: 3821.7312000004167 + y: 73772.11999999965 + z: 19.383 + orientation: + x: 0.0 + y: 0.013220820296605745 + z: -0.9708576649235394 + w: 0.2392918811198939 + longitudinal_velocity_mps: 2.777777671813965 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 10323 + - point: + pose: + position: + x: 3817.4533801890275 + y: 73769.88479730961 + z: 19.35246031495346 + orientation: + x: 0.0 + y: -0.011417973196519817 + z: -0.9711978971196675 + w: 0.23800057671866956 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3813.1738896739644 + y: 73767.6526986561 + z: 19.37869325375783 + orientation: + x: 0.0 + y: -0.012964752075534441 + z: -0.9711053932648867 + w: 0.23829861597472615 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3808.8959197570566 + y: 73765.41774366194 + z: 19.408517204373425 + orientation: + x: 0.0 + y: -0.0028131605706304096 + z: -0.9711862957220262 + w: 0.23830498343369405 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3804.616495037253 + y: 73763.18537492011 + z: 19.414988742734643 + orientation: + x: 0.0 + y: -0.008752301402569804 + z: -0.9712241231755983 + w: 0.23800651205786155 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3800.3370998388054 + y: 73760.95302939299 + z: 19.435097769226346 + orientation: + x: 0.0 + y: -0.0025045187615443075 + z: -0.9711880257059231 + w: 0.23830137664563472 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3796.058848781846 + y: 73758.71840804795 + z: 19.440859206210327 + orientation: + x: 0.0 + y: 0.008151300988484274 + z: -0.9711890389822359 + w: 0.2381709613973889 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3791.7790837229213 + y: 73756.48676238871 + z: 19.422118084879205 + orientation: + x: 0.0 + y: 0.009526648685350046 + z: -0.9711744516038346 + w: 0.23817940195746778 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3787.5009553579766 + y: 73754.25200634624 + z: 19.400214040289335 + orientation: + x: 0.0 + y: 0.008602750316763037 + z: -0.9711426644634529 + w: 0.23834411665869387 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3783.2217949859596 + y: 73752.01920735127 + z: 19.38042058025824 + orientation: + x: 0.0 + y: 0.008368562779794529 + z: -0.9712131576557372 + w: 0.23806505319633295 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3778.94268644388 + y: 73749.78630385295 + z: 19.361188489427974 + orientation: + x: 0.0 + y: 0.010076619174316634 + z: -0.9711186270900394 + w: 0.2383842986959771 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3774.663650000206 + y: 73747.55330000003 + z: 19.338 + orientation: + x: 0.0 + y: 0.01655751946729637 + z: -0.9712477031686881 + w: 0.23749473181238823 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 125 + - point: + pose: + position: + x: 3771.20351875539 + y: 73746.08023090652 + z: 19.308422949253657 + orientation: + x: 0.0 + y: -0.028808527617570495 + z: -0.991906962701512 + w: 0.1236553520102108 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 50 + - point: + pose: + position: + x: 3767.44841229364 + y: 73745.92067275506 + z: 19.335201616300694 + orientation: + x: 0.0 + y: 0.03154166631745605 + z: 0.997073304181149 + w: 0.06964157792012048 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 50 + - point: + pose: + position: + x: 3763.8634553512893 + y: 73747.05541147766 + z: 19.318681800920974 + orientation: + x: 0.0 + y: 0.04661402436669824 + z: 0.9705625243959328 + w: 0.23629540615622546 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 50 + - point: + pose: + position: + x: 3760.893597598764 + y: 73749.35920021501 + z: 19.235860909622797 + orientation: + x: 0.0 + y: -0.004516897284480318 + z: 0.9091724303104097 + w: 0.41639535252255716 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 50 + - point: + pose: + position: + x: 3758.8846000006015 + y: 73752.53600000008 + z: 19.25 + orientation: + x: 0.0 + y: 0.0030186047630470095 + z: 0.8566827901328178 + w: 0.5158347459366567 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 50 + - point: + pose: + position: + x: 3756.6310610663327 + y: 73756.85820173749 + z: 19.23482002918408 + orientation: + x: 0.0 + y: 0.005226477477746975 + z: 0.8545896301522615 + w: 0.519277621286914 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 122 + - point: + pose: + position: + x: 3754.3786774318205 + y: 73761.18094836178 + z: 19.20836169194407 + orientation: + x: 0.0 + y: 0.004393567218089739 + z: 0.8551220058846712 + w: 0.518408190154126 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 122 + - point: + pose: + position: + x: 3752.125864407952 + y: 73765.50349484754 + z: 19.1861570915516 + orientation: + x: 0.0 + y: 0.005524581143586082 + z: 0.8549750178603273 + w: 0.5186397572862315 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 122 + - point: + pose: + position: + x: 3749.8730927121874 + y: 73769.82602858973 + z: 19.15822400604602 + orientation: + x: 0.0 + y: 0.00588070419830711 + z: 0.8550753447178805 + w: 0.5184704159098473 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 122 + - point: + pose: + position: + x: 3747.9598131512926 + y: 73773.49314026142 + z: 19.133001148147248 + orientation: + x: 0.0 + y: 0.005882087359149526 + z: 0.8551492429650769 + w: 0.5183485056451451 + longitudinal_velocity_mps: 8.333333015441895 + lateral_velocity_mps: 0.0 + heading_rate_rps: 0.0 + is_final: false + lane_ids: + - 122 +left_bound: + - x: 3826.8116950865324 + y: 73773.10179741136 + z: 0.0 + - x: 3822.4259000012535 + y: 73770.79059999948 + z: 0.0 + - x: 3818.3929999988177 + y: 73768.68310000002 + z: 0.0 + - x: 3814.358700000972 + y: 73766.5793999997 + z: 0.0 + - x: 3809.9741999990074 + y: 73764.28840000089 + z: 0.0 + - x: 3805.5181000005687 + y: 73761.96389999986 + z: 0.0 + - x: 3801.103800001554 + y: 73759.66119999858 + z: 0.0 + - x: 3796.6483000007574 + y: 73757.3339999998 + z: 0.0 + - x: 3792.225900001009 + y: 73755.0280999993 + z: 0.0 + - x: 3787.7658000008087 + y: 73752.6979999994 + z: 0.0 + - x: 3783.3856000007945 + y: 73750.41280000051 + z: 0.0 + - x: 3779.371600000537 + y: 73748.3182000001 + z: 0.0 + - x: 3775.357600000745 + y: 73746.22349999985 + z: 0.0 + - x: 3773.3275000007707 + y: 73745.1707999995 + z: 0.0 + - x: 3772.2302000016207 + y: 73744.79149999982 + z: 0.0 + - x: 3771.104200001457 + y: 73744.52949999878 + z: 0.0 + - x: 3769.931700001005 + y: 73744.37279999908 + z: 0.0 + - x: 3768.7504000009503 + y: 73744.32720000017 + z: 0.0 + - x: 3767.5679000001983 + y: 73744.3955000001 + z: 0.0 + - x: 3766.3990999996313 + y: 73744.57779999916 + z: 0.0 + - x: 3765.2516000010073 + y: 73744.87380000064 + z: 0.0 + - x: 3764.1403000026476 + y: 73745.27729999926 + z: 0.0 + - x: 3763.070399999735 + y: 73745.78510000044 + z: 0.0 + - x: 3762.054099999252 + y: 73746.39400000032 + z: 0.0 + - x: 3761.104099999764 + y: 73747.09769999981 + z: 0.0 + - x: 3760.2228000007453 + y: 73747.8900000006 + z: 0.0 + - x: 3759.462200001348 + y: 73748.71789999958 + z: 0.0 + - x: 3758.602099998854 + y: 73749.8490000004 + z: 0.0 + - x: 3757.6837000006926 + y: 73751.61229999876 + z: 0.0 + - x: 3757.560900000506 + y: 73751.8479000004 + z: 0.0 + - x: 3754.9260000021313 + y: 73756.89629999967 + z: 0.0 + - x: 3752.170700001123 + y: 73762.17720000027 + z: 0.0 + - x: 3751.868700002553 + y: 73762.75670000119 + z: 0.0 + - x: 3748.796900000423 + y: 73768.6604000004 + z: 0.0 + - x: 3746.297002201589 + y: 73773.45917678656 + z: 0.0 +right_bound: + - x: 3825.4130934879936 + y: 73775.75579656845 + z: 0.0 + - x: 3821.03649999958 + y: 73773.44939999981 + z: 0.0 + - x: 3817.004700000456 + y: 73771.3424999998 + z: 0.0 + - x: 3812.970499999472 + y: 73769.23880000087 + z: 0.0 + - x: 3808.5858000002336 + y: 73766.9478000002 + z: 0.0 + - x: 3804.130599999102 + y: 73764.62370000035 + z: 0.0 + - x: 3799.715600000811 + y: 73762.32069999957 + z: 0.0 + - x: 3795.2602999992087 + y: 73759.99360000016 + z: 0.0 + - x: 3790.837800001609 + y: 73757.68759999936 + z: 0.0 + - x: 3786.377400000696 + y: 73755.3573999987 + z: 0.0 + - x: 3781.997800001933 + y: 73753.07249999931 + z: 0.0 + - x: 3777.9837000013795 + y: 73750.9777999986 + z: 0.0 + - x: 3773.969699999667 + y: 73748.88310000021 + z: 0.0 + - x: 3771.9323999999324 + y: 73747.8207000005 + z: 0.0 + - x: 3771.439299998223 + y: 73747.66279999912 + z: 0.0 + - x: 3770.5671000002185 + y: 73747.45970000047 + z: 0.0 + - x: 3769.678300000727 + y: 73747.33989999956 + z: 0.0 + - x: 3768.783000002615 + y: 73747.30659999978 + z: 0.0 + - x: 3767.886000001512 + y: 73747.35949999979 + z: 0.0 + - x: 3767.0000000017462 + y: 73747.49550000066 + z: 0.0 + - x: 3766.130000000994 + y: 73747.72070000041 + z: 0.0 + - x: 3765.28849999886 + y: 73748.02570000011 + z: 0.0 + - x: 3764.4779000012786 + y: 73748.41050000116 + z: 0.0 + - x: 3763.7083000000566 + y: 73748.87189999875 + z: 0.0 + - x: 3762.98700000142 + y: 73749.40360000124 + z: 0.0 + - x: 3762.3192000018316 + y: 73750.00559999887 + z: 0.0 + - x: 3761.714699999313 + y: 73750.66549999965 + z: 0.0 + - x: 3761.229799999972 + y: 73751.25929999957 + z: 0.0 + - x: 3760.208300000697 + y: 73753.22409999976 + z: 0.0 + - x: 3757.7870999999577 + y: 73757.87270000018 + z: 0.0 + - x: 3755.368500002194 + y: 73762.52129999967 + z: 0.0 + - x: 3753.070000002277 + y: 73766.92820000043 + z: 0.0 + - x: 3751.0121000027866 + y: 73770.87030000007 + z: 0.0 + - x: 3748.95180000033 + y: 73774.81240000064 + z: 0.0 diff --git a/planning/autoware_path_generator/test_route/route_data.yaml b/planning/autoware_path_generator/test_route/route_data.yaml new file mode 100644 index 0000000000..08f08e5233 --- /dev/null +++ b/planning/autoware_path_generator/test_route/route_data.yaml @@ -0,0 +1,69 @@ +header: + stamp: + sec: 0 + nanosec: 0 + frame_id: map +start_pose: + position: + x: 3823.326171875 + y: 73772.7890625 + z: 19.383 + orientation: + x: -0.002431494101355529 + y: -0.0005975458095886364 + z: -0.971102217561599 + w: 0.23865081986531142 +goal_pose: + position: + x: 3748.0029296875 + y: 73773.515625 + z: 19.13228968940651 + orientation: + x: 0.0 + y: 0.0 + z: 0.8622851268650221 + w: 0.506423103725899 +segments: + - preferred_primitive: + id: 10323 + primitive_type: "" + primitives: + - id: 10323 + primitive_type: lane + - preferred_primitive: + id: 125 + primitive_type: "" + primitives: + - id: 125 + primitive_type: lane + - preferred_primitive: + id: 50 + primitive_type: "" + primitives: + - id: 50 + primitive_type: lane + - preferred_primitive: + id: 122 + primitive_type: "" + primitives: + - id: 122 + primitive_type: lane +uuid: + uuid: + - 74 + - 19 + - 38 + - 82 + - 251 + - 255 + - 124 + - 82 + - 104 + - 137 + - 14 + - 163 + - 29 + - 94 + - 56 + - 132 +allow_modification: false diff --git a/planning/autoware_path_generator/test_route/straight_test_route.yaml b/planning/autoware_path_generator/test_route/straight_test_route.yaml new file mode 100644 index 0000000000..6b919c8a53 --- /dev/null +++ b/planning/autoware_path_generator/test_route/straight_test_route.yaml @@ -0,0 +1,85 @@ +header: + stamp: + sec: 0 + nanosec: 0 + frame_id: map +start_pose: + position: + x: 5.0 + y: 1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +goal_pose: + position: + x: 105.0 + y: 1.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +segments: + - preferred_primitive: + id: 4424 + primitive_type: "" + primitives: + - id: 4424 + primitive_type: lane + - id: 9598 + primitive_type: lane + - preferred_primitive: + id: 4780 + primitive_type: "" + primitives: + - id: 4780 + primitive_type: lane + - id: 5084 + primitive_type: lane + - preferred_primitive: + id: 4785 + primitive_type: "" + primitives: + - id: 4785 + primitive_type: lane + - id: 5088 + primitive_type: lane + - preferred_primitive: + id: 4790 + primitive_type: "" + primitives: + - id: 4790 + primitive_type: lane + - id: 5092 + primitive_type: lane + - preferred_primitive: + id: 4795 + primitive_type: "" + primitives: + - id: 4795 + primitive_type: lane + - id: 5096 + primitive_type: lane +uuid: + uuid: + - 231 + - 254 + - 143 + - 227 + - 194 + - 8 + - 220 + - 88 + - 30 + - 194 + - 172 + - 147 + - 127 + - 143 + - 176 + - 133 +allow_modification: false diff --git a/testing/autoware_test_utils/README.md b/testing/autoware_test_utils/README.md index e9f61f191f..4cb92e53de 100644 --- a/testing/autoware_test_utils/README.md +++ b/testing/autoware_test_utils/README.md @@ -71,7 +71,7 @@ ros2 launch autoware_test_utils psim_intersection.launch.xml vehicle_model:=<> s ### Autoware Planning Test Manager -The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.core/main/testing/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.core/blob/main/testing/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. ### Generate test data for unit testing