HdMapUtils refactor (PR 5/6) - extend lanelet_wrapper: add ::traffic_lights, use wide in traffic_simulator, remove hdmap_utils

Doxyfile

@@ -99,7 +99,6 @@ EXPAND_AS_DEFINED += EXPECT_VECTOR3_EQ
 EXPAND_AS_DEFINED += FORWARD_GETTER_TO_TRAFFIC_LIGHT_MANAGER
 EXPAND_AS_DEFINED += FORWARD_TO_ENTITY
 EXPAND_AS_DEFINED += FORWARD_TO_ENTITY_MANAGER
-EXPAND_AS_DEFINED += FORWARD_TO_HDMAP_UTILS
 EXPAND_AS_DEFINED += TEST
 EXPAND_AS_DEFINED += THROW_ERROR
 EXPAND_AS_DEFINED += THROW_SEMANTIC_ERROR

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -26,7 +26,6 @@
 #include <traffic_simulator/data_type/behavior.hpp>
 #include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/entity/entity_base.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/helper/stop_watch.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights.hpp>
 #include <traffic_simulator_msgs/msg/obstacle.hpp>
@@ -72,7 +71,6 @@ class ActionNode : public BT::ActionNodeBase
       BT::InputPort<EntityStatusDict>("other_entity_status"),
       BT::InputPort<lanelet::Ids>("route_lanelets"),
       BT::InputPort<std::optional<double>>("target_speed"),
-      BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
       BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("canonicalized_entity_status"),
       BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightsBase>>("traffic_lights"),
       BT::InputPort<traffic_simulator::behavior::Request>("request"),
@@ -97,7 +95,6 @@ class ActionNode : public BT::ActionNodeBase
 
 protected:
   traffic_simulator::behavior::Request request;
-  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
   std::shared_ptr<traffic_simulator::TrafficLightsBase> traffic_lights;
   std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> canonicalized_entity_status;
   double current_time;

simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp

@@ -28,7 +28,6 @@
 #include <optional>
 #include <string>
 #include <traffic_simulator/behavior/behavior_plugin_base.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
 #include <vector>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -56,7 +55,6 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(DebugMarker,                                      std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double)
   DEFINE_GETTER_SETTER(GoalPoses,                                        std::vector<geometry_msgs::msg::Pose>)
-  DEFINE_GETTER_SETTER(HdMapUtils,                                       std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters,                             traffic_simulator::lane_change::Parameter)
   DEFINE_GETTER_SETTER(Obstacle,                                         std::optional<traffic_simulator_msgs::msg::Obstacle>)
   DEFINE_GETTER_SETTER(OtherEntityStatus,                                EntityStatusDict)

simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp

@@ -26,7 +26,6 @@
 #include <optional>
 #include <string>
 #include <traffic_simulator/behavior/behavior_plugin_base.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
 #include <traffic_simulator_msgs/msg/obstacle.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
@@ -60,7 +59,6 @@ class VehicleBehaviorTree : public BehaviorPluginBase
   DEFINE_GETTER_SETTER(DebugMarker,                                      std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double)
   DEFINE_GETTER_SETTER(GoalPoses,                                        std::vector<geometry_msgs::msg::Pose>)
-  DEFINE_GETTER_SETTER(HdMapUtils,                                       std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters,                             traffic_simulator::lane_change::Parameter)
   DEFINE_GETTER_SETTER(Obstacle,                                         std::optional<traffic_simulator_msgs::msg::Obstacle>)
   DEFINE_GETTER_SETTER(OtherEntityStatus,                                EntityStatusDict)

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -60,9 +60,6 @@ auto ActionNode::getBlackBoardValues() -> void
   if (!getInput<double>("current_time", current_time)) {
     THROW_SIMULATION_ERROR("failed to get input current_time in ActionNode");
   }
-  if (!getInput<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils", hdmap_utils)) {
-    THROW_SIMULATION_ERROR("failed to get input hdmap_utils in ActionNode");
-  }
   if (!getInput<std::shared_ptr<traffic_simulator::TrafficLightsBase>>(
         "traffic_lights", traffic_lights)) {
     THROW_SIMULATION_ERROR("failed to get input traffic_lights in ActionNode");

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

@@ -18,6 +18,7 @@
 #include <optional>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <vector>
 

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp

@@ -18,6 +18,7 @@
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
 #include <traffic_simulator/helper/stop_watch.hpp>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <vector>
 

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp

@@ -18,6 +18,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <utility>
 #include <vector>

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp

@@ -17,6 +17,7 @@
 #include <optional>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <utility>
 #include <vector>

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp

@@ -17,6 +17,7 @@
 #include <optional>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <utility>
 #include <vector>

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp

@@ -19,6 +19,7 @@
 #include <optional>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <vector>
 

simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp

@@ -21,7 +21,6 @@
 #include <optional>
 #include <scenario_simulator_exception/exception.hpp>
 #include <string>
-#include <traffic_simulator/utils/pose.hpp>
 #include <traffic_simulator/utils/route.hpp>
 #include <vector>
 

simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp

@@ -73,7 +73,6 @@ private:                                                               \
   DEFINE_GETTER_SETTER(BehaviorParameter,         traffic_simulator_msgs::msg::BehaviorParameter,                   behavior_parameter_)
   DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>,    canonicalized_entity_status_)
   DEFINE_GETTER_SETTER(CurrentTime,               double,                                                           current_time_)
-  DEFINE_GETTER_SETTER(HdMapUtils,                std::shared_ptr<hdmap_utils::HdMapUtils>,                         hdmap_utils_)
   DEFINE_GETTER_SETTER(PolylineTrajectory,        std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>, polyline_trajectory)
   DEFINE_GETTER_SETTER(Request,                   traffic_simulator::behavior::Request,                             request)
   DEFINE_GETTER_SETTER(StepTime,                  double,                                                           step_time_)

simulation/simulation_interface/include/simulation_interface/conversions.hpp

@@ -168,6 +168,7 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time)
 void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto);
 void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header);
 
+// clang-format off
 #define DEFINE_CONVERSION(PACKAGE, TYPENAME)                               \
   auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \
   auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void
@@ -254,6 +255,7 @@ auto toMsg(
   traffic_light_bulb_state.color = convert_color(proto.color());
   traffic_light_bulb_state.confidence = proto.confidence();
 }
+// clang-format on
 
 auto toProtobufMessage(const traffic_simulator_msgs::msg::Vertex &)
   -> traffic_simulator_msgs::Vertex;

simulation/traffic_simulator/CMakeLists.txt

@@ -44,7 +44,6 @@ ament_auto_add_library(traffic_simulator SHARED
   src/entity/misc_object_entity.cpp
   src/entity/pedestrian_entity.cpp
   src/entity/vehicle_entity.cpp
-  src/hdmap_utils/hdmap_utils.cpp
   src/helper/helper.cpp
   src/job/job.cpp
   src/job/job_list.cpp
@@ -71,6 +70,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/utils/lanelet_map.cpp
   src/utils/pose.cpp
   src/utils/route.cpp
+  src/utils/traffic_lights.cpp
 )
 
 ament_auto_add_library(visualization_component SHARED

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -34,7 +34,6 @@
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 #include <traffic_simulator/entity/entity_base.hpp>
 #include <traffic_simulator/entity/entity_manager.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/simulation_clock/simulation_clock.hpp>
 #include <traffic_simulator/traffic/traffic_controller.hpp>
@@ -70,8 +69,7 @@ class API
     entity_manager_ptr_(
       std::make_shared<entity::EntityManager>(node, configuration, node_parameters_)),
     traffic_lights_ptr_(std::make_shared<TrafficLights>(
-      node, entity_manager_ptr_->getHdmapUtils(),
-      getROS2Parameter<std::string>("architecture_type", "awf/universe/20240605"))),
+      node, getROS2Parameter<std::string>("architecture_type", "awf/universe/20240605"))),
     traffic_controller_ptr_(std::make_shared<traffic::TrafficController>(
       [this](const std::string & name) { despawn(name); }, entity_manager_ptr_,
       configuration.auto_sink_entity_types)),
@@ -371,7 +369,6 @@ class API
   FORWARD_TO_ENTITY_MANAGER(getEntityNames);
   FORWARD_TO_ENTITY_MANAGER(getEntityStatus);
   FORWARD_TO_ENTITY_MANAGER(getCanonicalizedStatusBeforeUpdate);
-  FORWARD_TO_ENTITY_MANAGER(getHdmapUtils);
   FORWARD_TO_ENTITY_MANAGER(getLinearJerk);
   FORWARD_TO_ENTITY_MANAGER(getStandStillDuration);
   FORWARD_TO_ENTITY_MANAGER(getTraveledDistance);

simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp

@@ -15,12 +15,13 @@
 #ifndef TRAFFIC_SIMULATOR__BEHAVIOR__BEHAVIOR_PLUGIN_BASE_HPP_
 #define TRAFFIC_SIMULATOR__BEHAVIOR__BEHAVIOR_PLUGIN_BASE_HPP_
 
+#include <geometry/spline/catmull_rom_subspline.hpp>
 #include <optional>
 #include <string>
 #include <traffic_simulator/behavior/follow_trajectory.hpp>
 #include <traffic_simulator/data_type/behavior.hpp>
 #include <traffic_simulator/data_type/entity_status.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
+#include <traffic_simulator/data_type/lane_change.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights.hpp>
 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 #include <traffic_simulator_msgs/msg/entity_type.hpp>
@@ -44,6 +45,7 @@ class BehaviorPluginBase
   virtual auto update(const double current_time, const double step_time) -> void = 0;
   virtual const std::string & getCurrentAction() const = 0;
 
+  // clang-format off
 #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE)      \
   virtual TYPE get##NAME() = 0;                    \
   virtual void set##NAME(const TYPE & value) = 0;  \
@@ -53,14 +55,12 @@ class BehaviorPluginBase
     return key;                                    \
   }
 
-  // clang-format off
   DEFINE_GETTER_SETTER(BehaviorParameter,                                "behavior_parameter",                             traffic_simulator_msgs::msg::BehaviorParameter)
   DEFINE_GETTER_SETTER(CanonicalizedEntityStatus,                        "canonicalized_entity_status",                    std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>)
   DEFINE_GETTER_SETTER(CurrentTime,                                      "current_time",                                   double)
   DEFINE_GETTER_SETTER(DebugMarker,                                      "debug_marker",                                   std::vector<visualization_msgs::msg::Marker>)
   DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, "matching_distance_for_lanelet_pose_calculation", double)
   DEFINE_GETTER_SETTER(GoalPoses,                                        "goal_poses",                                     std::vector<geometry_msgs::msg::Pose>)
-  DEFINE_GETTER_SETTER(HdMapUtils,                                       "hdmap_utils",                                    std::shared_ptr<hdmap_utils::HdMapUtils>)
   DEFINE_GETTER_SETTER(LaneChangeParameters,                             "lane_change_parameters",                         traffic_simulator::lane_change::Parameter)
   DEFINE_GETTER_SETTER(Obstacle,                                         "obstacle",                                       std::optional<traffic_simulator_msgs::msg::Obstacle>)
   DEFINE_GETTER_SETTER(OtherEntityStatus,                                "other_entity_status",                            EntityStatusDict)

simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

@@ -43,7 +43,6 @@ class EgoEntity : public VehicleEntity, private concealer::FieldOperatorApplicat
 
   explicit EgoEntity(
     const std::string & name, const CanonicalizedEntityStatus &,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &,
     const traffic_simulator_msgs::msg::VehicleParameters &, const Configuration &,
     const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &);
 

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -27,7 +27,6 @@
 #include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/data_type/lane_change.hpp>
 #include <traffic_simulator/data_type/speed_change.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/job/job_list.hpp>
 #include <traffic_simulator/traffic_lights/traffic_lights.hpp>
@@ -50,9 +49,7 @@ namespace entity
 class EntityBase
 {
 public:
-  explicit EntityBase(
-    const std::string & name, const CanonicalizedEntityStatus & entity_status,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr);
+  explicit EntityBase(const std::string & name, const CanonicalizedEntityStatus & entity_status);
 
   virtual ~EntityBase() = default;
 
@@ -267,7 +264,6 @@ class EntityBase
 
   CanonicalizedEntityStatus status_before_update_;
 
-  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
   std::shared_ptr<traffic_simulator::TrafficLightsBase> traffic_lights_;
 
   double stand_still_duration_ = 0.0;

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -36,7 +36,6 @@
 #include <traffic_simulator/entity/misc_object_entity.hpp>
 #include <traffic_simulator/entity/pedestrian_entity.hpp>
 #include <traffic_simulator/entity/vehicle_entity.hpp>
-#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
 #include <traffic_simulator/traffic_lights/configurable_rate_updater.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_marker_publisher.hpp>
 #include <traffic_simulator/traffic_lights/traffic_light_publisher.hpp>
@@ -92,8 +91,6 @@ class EntityManager
   using MarkerArray = visualization_msgs::msg::MarkerArray;
   const rclcpp::Publisher<MarkerArray>::SharedPtr lanelet_marker_pub_ptr_;
 
-  const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
-
   MarkerArray markers_raw_;
 
   std::shared_ptr<traffic_simulator::TrafficLights> traffic_lights_ptr_;
@@ -110,24 +107,6 @@ class EntityManager
     traffic_lights_ptr_ = traffic_lights_ptr;
   }
 
-  template <typename Node>
-  auto getOrigin(Node & node) const
-  {
-    geographic_msgs::msg::GeoPoint origin;
-    {
-      if (!node.has_parameter("origin_latitude")) {
-        node.declare_parameter("origin_latitude", 0.0);
-      }
-      if (!node.has_parameter("origin_longitude")) {
-        node.declare_parameter("origin_longitude", 0.0);
-      }
-      node.get_parameter("origin_latitude", origin.latitude);
-      node.get_parameter("origin_longitude", origin.longitude);
-    }
-
-    return origin;
-  }
-
   template <class NodeT, class AllocatorT = std::allocator<void>>
   explicit EntityManager(
     NodeT && node, const Configuration & configuration,
@@ -145,11 +124,9 @@ class EntityManager
     lanelet_marker_pub_ptr_(rclcpp::create_publisher<MarkerArray>(
       node, "lanelet/marker", LaneletMarkerQoS(),
       rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
-    hdmap_utils_ptr_(std::make_shared<hdmap_utils::HdMapUtils>(
-      configuration.lanelet2_map_path(), getOrigin(*node))),
-    markers_raw_(hdmap_utils_ptr_->generateMarker())
+    markers_raw_(lanelet_map::visualizationMarker())
   {
-    updateHdmapMarker();
+    updateLaneletMarker();
   }
 
   ~EntityManager() = default;
@@ -240,8 +217,6 @@ class EntityManager
 
   auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &;
 
-  auto getHdmapUtils() -> const std::shared_ptr<hdmap_utils::HdMapUtils> &;
-
   auto getNumberOfEgo() const -> std::size_t;
 
   auto getObstacle(const std::string & name)
@@ -379,8 +354,7 @@ class EntityManager
 
     if (const auto [iter, success] = entities_.emplace(
           name, std::make_unique<Entity>(
-                  name, makeEntityStatus(), hdmap_utils_ptr_, parameters,
-                  std::forward<decltype(xs)>(xs)...));
+                  name, makeEntityStatus(), parameters, std::forward<decltype(xs)>(xs)...));
         success) {
       // FIXME: this ignores V2I traffic lights
       iter->second->setTrafficLights(traffic_lights_ptr_->getConventionalTrafficLights());
@@ -405,7 +379,7 @@ class EntityManager
 
   void update(const double current_time, const double step_time);
 
-  void updateHdmapMarker();
+  void updateLaneletMarker();
 
   auto startNpcLogic(const double current_time) -> void;
 

simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp

@@ -28,7 +28,6 @@ class MiscObjectEntity : public EntityBase
 public:
   explicit MiscObjectEntity(
     const std::string & name, const CanonicalizedEntityStatus &,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &,
     const traffic_simulator_msgs::msg::MiscObjectParameters &);
 
   void onUpdate(double, double) override;

simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp

@@ -58,7 +58,6 @@ class PedestrianEntity : public EntityBase
 
   explicit PedestrianEntity(
     const std::string & name, const CanonicalizedEntityStatus &,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &,
     const traffic_simulator_msgs::msg::PedestrianParameters &,
     const std::string & plugin_name = BuiltinBehavior::defaultBehavior());
 

simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp

@@ -61,7 +61,6 @@ class VehicleEntity : public EntityBase
 
   explicit VehicleEntity(
     const std::string & name, const CanonicalizedEntityStatus &,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &,
     const traffic_simulator_msgs::msg::VehicleParameters &,
     const std::string & plugin_name = BuiltinBehavior::defaultBehavior());