Merge pull request #1472 from tier4/ref/RJD-1387-hdmap-utils-to-lanel…

…et-wrapper-pose HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose
tier4 · Jan 24, 2025 · 0cc9b07 · 0cc9b07
2 parents fa9ca06 + 1649df0
commit 0cc9b07
Show file tree

Hide file tree

Showing 112 changed files with 2,754 additions and 1,828 deletions.
diff --git a/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp
@@ -0,0 +1,46 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// 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 GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_
+#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_
+
+#include <cmath>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/vector3/is_like_vector3.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+
+namespace math
+{
+namespace geometry
+{
+auto convertDirectionToQuaternion(const double dx, const double dy, const double dz)
+{
+  const auto euler_angles = geometry_msgs::build<geometry_msgs::msg::Vector3>()
+                              .x(0.0)
+                              .y(std::atan2(-dz, std::hypot(dx, dy)))
+                              .z(std::atan2(dy, dx));
+  return math::geometry::convertEulerAngleToQuaternion(euler_angles);
+}
+
+template <
+  typename T, std::enable_if_t<std::conjunction_v<IsLikeVector3<T>>, std::nullptr_t> = nullptr>
+auto convertDirectionToQuaternion(const T & v)
+{
+  return convertDirectionToQuaternion(v.x, v.y, v.z);
+}
+}  // namespace geometry
+}  // namespace math
+
+#endif  // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_
diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp
@@ -75,14 +75,9 @@ class CppScenarioNode : public rclcpp::Node
     const std::string & scenario_filename, const bool verbose,
     const std::set<std::uint8_t> & auto_sink_entity_types = {}) -> traffic_simulator::Configuration
   {
-    auto configuration = traffic_simulator::Configuration(map_path);
-    {
-      configuration.lanelet2_map_file = lanelet2_map_file;
-      // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm";
-      configuration.scenario_path = scenario_filename;
-      configuration.verbose = verbose;
-      configuration.auto_sink_entity_types = auto_sink_entity_types;
-    }
+    auto configuration = traffic_simulator::Configuration(
+      map_path, lanelet2_map_file, scenario_filename, auto_sink_entity_types);
+    configuration.verbose = verbose;
     checkConfiguration(configuration);
     return configuration;
   }

diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp
@@ -67,27 +67,21 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0),
       getVehicleParameters(),
       traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
     api_.spawn(
-      "pedestrian",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 3.0, 0.0, api_.getHdmapUtils()),
+      "pedestrian", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0),
       getPedestrianParameters(),
       traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing());
     api_.spawn(
       "vehicle_spawn_with_behavior_tree",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 2.0, 0.0, api_.getHdmapUtils()),
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 2.0, 0.0),
       getVehicleParameters(),
       traffic_simulator::entity::VehicleEntity::BuiltinBehavior::behaviorTree());
     api_.spawn(
       "pedestrian_spawn_with_behavior_tree",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 3.0, 0.0, api_.getHdmapUtils()),
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0),
       getPedestrianParameters(),
       traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::behaviorTree());
   }

diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp
@@ -52,9 +52,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 0.0);
     api_.requestSpeedChange("ego", 15, true);
@@ -63,9 +61,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode
     api_.setBehaviorParameter("ego", behavior_parameter);
 
     api_.spawn(
-      "npc",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 10.0, 0.0, api_.getHdmapUtils()),
+      "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("npc", 0.0);
     api_.requestSpeedChange("npc", 5, true);

diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp
@@ -50,17 +50,13 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.2, 1.3, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.2, 1.3),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 0);
     api_.requestSpeedChange("ego", 0, true);
 
     api_.spawn(
-      "bob",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.0, -0.874, api_.getHdmapUtils()),
+      "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, -0.874),
       getPedestrianParameters());
     api_.setLinearVelocity("bob", 0);
     api_.requestSpeedChange("bob", 0, true);

diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp
@@ -74,23 +74,17 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        120545, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 8, true);
     api_.requestAssignRoute(
       "ego", std::vector<traffic_simulator::CanonicalizedLaneletPose>{
-               traffic_simulator::helper::constructCanonicalizedLaneletPose(
-                 34675, 0.0, 0.0, api_.getHdmapUtils()),
-               traffic_simulator::helper::constructCanonicalizedLaneletPose(
-                 34690, 0.0, 0.0, api_.getHdmapUtils())});
+               traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0),
+               traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)});
 
     api_.spawn(
-      "bob",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34378, 0.0, 0.0, api_.getHdmapUtils()),
+      "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0),
       getPedestrianParameters());
     api_.setLinearVelocity("bob", 0);
     api_.requestSpeedChange(

diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp
@@ -59,16 +59,12 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 3);
 
     api_.spawn(
-      "npc",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 10.0, 0.0, api_.getHdmapUtils()),
+      "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("npc", 10);
     api_.requestSpeedChange("npc", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp
@@ -59,16 +59,12 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 15);
 
     api_.spawn(
-      "npc",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 15.0, 0.0, api_.getHdmapUtils()),
+      "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 15.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("npc", 10);
     api_.requestSpeedChange("npc", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp
@@ -47,19 +47,14 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34741, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0),
       getVehicleParameters());
     api_.setEntityStatus(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0),
       traffic_simulator::helper::constructActionStatus(10));
     api_.requestSpeedChange("ego", 10, true);
     const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34408, 1.0, 0.0, api_.getHdmapUtils()));
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0));
     api_.requestAcquirePosition("ego", goal_pose);
   }
 };

diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp
@@ -47,19 +47,15 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);
     std::vector<geometry_msgs::msg::Pose> goal_poses;
     goal_poses.emplace_back(traffic_simulator::pose::toMapPose(
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34408, 1.0, 0.0, api_.getHdmapUtils())));
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0)));
     goal_poses.emplace_back(traffic_simulator::pose::toMapPose(
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34408, 10, 0.0, api_.getHdmapUtils())));
+      traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 10, 0.0)));
     api_.requestAssignRoute("ego", goal_poses);
   }
 };

diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp
@@ -40,10 +40,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
   void onUpdate() override
   {
     if (api_.reachPosition(
-          "ego",
-          traffic_simulator::helper::constructCanonicalizedLaneletPose(
-            34513, 30, 0, api_.getHdmapUtils()),
-          3.0)) {
+          "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 30, 0), 3.0)) {
       api_.cancelRequest("ego");
       canceled = true;
     }
@@ -54,14 +51,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 0.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 7);
     api_.requestSpeedChange("ego", 7, true);
     const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
-      traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils());
+      traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0));
     api_.requestAcquirePosition("ego", goal_pose);
   }
 };

diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp
@@ -55,9 +55,7 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 0.0, 3.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 3.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp
@@ -51,9 +51,7 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp
@@ -51,9 +51,7 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp
@@ -55,9 +55,7 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp
@@ -59,9 +59,7 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp
@@ -60,9 +60,7 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp
@@ -47,9 +47,7 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 1);
     api_.requestSpeedChange("ego", 1, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp
@@ -51,9 +51,7 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp
@@ -51,9 +51,7 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34513, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);

diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp
@@ -60,9 +60,7 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode
   void onInitialize() override
   {
     api_.spawn(
-      "ego",
-      traffic_simulator::helper::constructCanonicalizedLaneletPose(
-        34462, 10.0, 0.0, api_.getHdmapUtils()),
+      "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0),
       getVehicleParameters());
     api_.setLinearVelocity("ego", 10);
     api_.requestSpeedChange("ego", 10, true);