diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp new file mode 100644 index 0000000000000..63474c5e458cd --- /dev/null +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -0,0 +1,124 @@ +// 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. +// 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 AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ +#define AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware_planning_test_manager::utils +{ +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using route_handler::RouteHandler; +using RouteSections = std::vector; + +Pose createPoseFromLaneID(const lanelet::Id & lane_id) +{ + auto map_bin_msg = test_utils::makeMapBinMsg(); + // create route_handler + auto route_handler = std::make_shared(); + route_handler->setMap(map_bin_msg); + + // get middle idx of the lanelet + const auto lanelet = route_handler->getLaneletsFromId(lane_id); + const auto center_line = lanelet.centerline(); + const size_t middle_point_idx = std::floor(center_line.size() / 2.0); + + // get middle position of the lanelet + geometry_msgs::msg::Point middle_pos; + middle_pos.x = center_line[middle_point_idx].x(); + middle_pos.y = center_line[middle_point_idx].y(); + + // get next middle position of the lanelet + geometry_msgs::msg::Point next_middle_pos; + next_middle_pos.x = center_line[middle_point_idx + 1].x(); + next_middle_pos.y = center_line[middle_point_idx + 1].y(); + + // calculate middle pose + geometry_msgs::msg::Pose middle_pose; + middle_pose.position = middle_pos; + const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + + return middle_pose; +} + +// Function to create a route from given start and goal lanelet ids +// start pose and goal pose are set to the middle of the lanelet +LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) +{ + LaneletRoute route; + route.header.frame_id = "map"; + auto start_pose = createPoseFromLaneID(start_lane_id); + auto goal_pose = createPoseFromLaneID(goal_lane_id); + route.start_pose = start_pose; + route.goal_pose = goal_pose; + + auto map_bin_msg = test_utils::makeMapBinMsg(); + // create route_handler + auto route_handler = std::make_shared(); + route_handler->setMap(map_bin_msg); + + LaneletRoute route_msg; + RouteSections route_sections; + lanelet::ConstLanelets all_route_lanelets; + + // Plan the path between checkpoints (start and goal poses) + lanelet::ConstLanelets path_lanelets; + if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) { + return route_msg; + } + + // Add all path_lanelets to all_route_lanelets + for (const auto & lane : path_lanelets) { + all_route_lanelets.push_back(lane); + } + // create local route sections + route_handler->setRouteLanelets(path_lanelets); + const auto local_route_sections = route_handler->createMapSegments(path_lanelets); + route_sections = + test_utils::combineConsecutiveRouteSections(route_sections, local_route_sections); + for (const auto & route_section : route_sections) { + for (const auto & primitive : route_section.primitives) { + std::cerr << "primitive: " << primitive.id << std::endl; + } + std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl; + } + route_handler->setRouteLanelets(all_route_lanelets); + route.segments = route_sections; + + route.allow_modification = false; + return route; +} + +Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) +{ + Odometry current_odometry; + current_odometry.pose.pose = createPoseFromLaneID(lane_id); + current_odometry.header.frame_id = "map"; + + return current_odometry; +} + +} // namespace autoware_planning_test_manager::utils +#endif // AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 3aee408a76306..9ee162ec02d1f 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -15,7 +15,9 @@ #include "motion_utils/trajectory/conversion.hpp" #include +#include #include +#include namespace planning_test_utils { @@ -115,7 +117,7 @@ void PlanningInterfaceTestManager::publishInitialPose( if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, initial_pose_pub_, - test_utils::makeInitialPoseFromLaneId(10291)); + autoware_planning_test_manager::utils::makeInitialPoseFromLaneId(10291)); } else { test_utils::publishToTargetNode( test_node_, target_node, topic_name, initial_pose_pub_, test_utils::makeInitialPose(shift)); @@ -256,7 +258,7 @@ void PlanningInterfaceTestManager::publishBehaviorNominalRoute( if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, behavior_normal_route_pub_, - test_utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); + autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); } else { test_utils::publishToTargetNode( test_node_, target_node, topic_name, behavior_normal_route_pub_, diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 7b52f3860592e..c47404b45bb5d 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,7 +36,9 @@ #include #include #include +#include #include +#include #include #include @@ -71,7 +72,6 @@ using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using route_handler::RouteHandler; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; using tier4_autoware_utils::createPoint; @@ -267,46 +267,6 @@ Scenario makeScenarioMsg(const std::string scenario) return scenario_msg; } -Pose createPoseFromLaneID(const lanelet::Id & lane_id) -{ - auto map_bin_msg = makeMapBinMsg(); - // create route_handler - auto route_handler = std::make_shared(); - route_handler->setMap(map_bin_msg); - - // get middle idx of the lanelet - const auto lanelet = route_handler->getLaneletsFromId(lane_id); - const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); - - // get middle position of the lanelet - geometry_msgs::msg::Point middle_pos; - middle_pos.x = center_line[middle_point_idx].x(); - middle_pos.y = center_line[middle_point_idx].y(); - - // get next middle position of the lanelet - geometry_msgs::msg::Point next_middle_pos; - next_middle_pos.x = center_line[middle_point_idx + 1].x(); - next_middle_pos.y = center_line[middle_point_idx + 1].y(); - - // calculate middle pose - geometry_msgs::msg::Pose middle_pose; - middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - - return middle_pose; -} - -Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) -{ - Odometry current_odometry; - current_odometry.pose.pose = createPoseFromLaneID(lane_id); - current_odometry.header.frame_id = "map"; - - return current_odometry; -} - RouteSections combineConsecutiveRouteSections( const RouteSections & route_sections1, const RouteSections & route_sections2) { @@ -322,53 +282,6 @@ RouteSections combineConsecutiveRouteSections( return route_sections; } -// Function to create a route from given start and goal lanelet ids -// start pose and goal pose are set to the middle of the lanelet -LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) -{ - LaneletRoute route; - route.header.frame_id = "map"; - auto start_pose = createPoseFromLaneID(start_lane_id); - auto goal_pose = createPoseFromLaneID(goal_lane_id); - route.start_pose = start_pose; - route.goal_pose = goal_pose; - - auto map_bin_msg = makeMapBinMsg(); - // create route_handler - auto route_handler = std::make_shared(); - route_handler->setMap(map_bin_msg); - - LaneletRoute route_msg; - RouteSections route_sections; - lanelet::ConstLanelets all_route_lanelets; - - // Plan the path between checkpoints (start and goal poses) - lanelet::ConstLanelets path_lanelets; - if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) { - return route_msg; - } - - // Add all path_lanelets to all_route_lanelets - for (const auto & lane : path_lanelets) { - all_route_lanelets.push_back(lane); - } - // create local route sections - route_handler->setRouteLanelets(path_lanelets); - const auto local_route_sections = route_handler->createMapSegments(path_lanelets); - route_sections = combineConsecutiveRouteSections(route_sections, local_route_sections); - for (const auto & route_section : route_sections) { - for (const auto & primitive : route_section.primitives) { - std::cerr << "primitive: " << primitive.id << std::endl; - } - std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl; - } - route_handler->setRouteLanelets(all_route_lanelets); - route.segments = route_sections; - - route.allow_modification = false; - return route; -} - // this is for the test lanelet2_map.osm // file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 LaneletRoute makeBehaviorNormalRoute() diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 47540d6d3751f..b236a64f48da0 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -25,10 +25,8 @@ component_interface_utils lanelet2_extension lanelet2_io - motion_utils nav_msgs rclcpp - route_handler tf2_msgs tf2_ros tier4_api_msgs