refactor(planning_test_utils): remove route_handler dependencies (#7005)

* refactor(planning_test_utils): remove route_handler dependencies

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* style(pre-commit): autofix

* Fix precommit

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: zulfaqar-azmi-t4

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 <planning_test_utils/planning_test_utils.hpp>
+#include <route_handler/route_handler.hpp>
+
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <memory>
+#include <vector>
+
+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<autoware_planning_msgs::msg::LaneletSegment>;
+
+Pose createPoseFromLaneID(const lanelet::Id & lane_id)
+{
+  auto map_bin_msg = test_utils::makeMapBinMsg();
+  // create route_handler
+  auto route_handler = std::make_shared<RouteHandler>();
+  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<RouteHandler>();
+  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_

planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp

@@ -15,7 +15,9 @@
 #include "motion_utils/trajectory/conversion.hpp"
 
 #include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
+#include <autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp>
 #include <planning_test_utils/planning_test_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 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_,

planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp

@@ -23,7 +23,6 @@
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <route_handler/route_handler.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
@@ -37,7 +36,9 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
@@ -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<RouteHandler>();
-  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<RouteHandler>();
-  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()

planning/planning_test_utils/package.xml

@@ -25,10 +25,8 @@
   <depend>component_interface_utils</depend>
   <depend>lanelet2_extension</depend>
   <depend>lanelet2_io</depend>
-  <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
-  <depend>route_handler</depend>
   <depend>tf2_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_api_msgs</depend>