scenario_simulator_v2 C++ API
entity_manager.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TRAFFIC_SIMULATOR__ENTITY__ENTITY_MANAGER_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_MANAGER_HPP_
17 
18 #include <tf2_ros/static_transform_broadcaster.h>
19 #include <tf2_ros/transform_broadcaster.h>
20 
21 #include <get_parameter/get_parameter.hpp>
28 #include <traffic_simulator_msgs/msg/entity_status_with_trajectory_array.hpp>
29 
30 namespace traffic_simulator
31 {
32 namespace entity
33 {
34 class LaneletMarkerQoS : public rclcpp::QoS
35 {
36 public:
37  explicit LaneletMarkerQoS(std::size_t depth = 1) : rclcpp::QoS(depth) { transient_local(); }
38 };
39 
40 class EntityMarkerQoS : public rclcpp::QoS
41 {
42 public:
43  explicit EntityMarkerQoS(std::size_t depth = 100) : rclcpp::QoS(depth) {}
44 };
45 
47 {
48  using EntityStatusWithTrajectoryArray =
49  traffic_simulator_msgs::msg::EntityStatusWithTrajectoryArray;
50  using MarkerArray = visualization_msgs::msg::MarkerArray;
51 
52 public:
53  template <class NodeT, class AllocatorT = std::allocator<void>>
54  explicit EntityManager(
55  NodeT && node, const Configuration & configuration,
56  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters)
57  : configuration_(configuration),
58  clock_ptr_(node->get_clock()),
59  node_parameters_(node_parameters),
60  broadcaster_(node),
61  base_link_broadcaster_(node),
62  entity_status_array_pub_ptr_(rclcpp::create_publisher<EntityStatusWithTrajectoryArray>(
63  node, "entity/status", EntityMarkerQoS(),
64  rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
65  lanelet_marker_pub_ptr_(rclcpp::create_publisher<MarkerArray>(
66  node, "lanelet/marker", LaneletMarkerQoS(),
67  rclcpp::PublisherOptionsWithAllocator<AllocatorT>())),
68  entities_(128),
69  hdmap_utils_ptr_(std::make_shared<hdmap_utils::HdMapUtils>(
70  configuration_.lanelet2_map_path(), getOrigin(*node))),
71  markers_raw_(hdmap_utils_ptr_->generateMarker())
72  {
74  }
75 
76  ~EntityManager() = default;
77 
78  // global
84  auto setTrafficLights(const std::shared_ptr<TrafficLights> & traffic_lights_ptr) -> void;
85 
86  auto setVerbose(const bool verbose) -> void;
87 
88  auto startNpcLogic(const double current_time) -> void;
89 
90  auto isNpcLogicStarted() const -> bool;
91 
92  auto makeDebugMarker() const -> visualization_msgs::msg::MarkerArray;
93 
94  // update
95  auto update(const double current_time, const double step_time) -> void;
96 
97  auto updateNpcLogic(
98  const std::string & name, const double current_time, const double step_time,
99  const std::shared_ptr<EuclideanDistancesMap> & distances) -> const CanonicalizedEntityStatus &;
100 
101  auto updateHdmapMarker() const -> void;
102 
103  auto broadcastEntityTransform() -> void;
104 
105  auto broadcastTransform(
106  const geometry_msgs::msg::PoseStamped & pose, const bool static_transform = true) -> void;
107 
108  // entities, ego - spawn
109  template <typename EntityType, typename PoseType, typename ParametersType, typename... Ts>
111  const std::string & name, const PoseType & pose, const ParametersType & parameters,
112  const double current_time, Ts &&... xs) -> entity::EntityBase &
113  {
114  static_assert(
115  std::disjunction<
116  std::is_same<PoseType, CanonicalizedLaneletPose>,
117  std::is_same<PoseType, geometry_msgs::msg::Pose>>::value,
118  "Pose must be of type CanonicalizedLaneletPose or geometry_msgs::msg::Pose");
119 
121  EntityStatus entity_status;
122 
123  if constexpr (std::is_same_v<std::decay_t<EntityType>, EgoEntity>) {
124  if (isAnyEgoSpawned()) {
125  THROW_SEMANTIC_ERROR("Multiple egos in the simulation are unsupported yet.");
126  } else {
127  entity_status.type.type = traffic_simulator_msgs::msg::EntityType::EGO;
128  }
129  } else if constexpr (std::is_same_v<std::decay_t<EntityType>, VehicleEntity>) {
130  entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE;
131  } else if constexpr (std::is_same_v<std::decay_t<EntityType>, PedestrianEntity>) {
132  entity_status.type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN;
133  } else {
134  entity_status.type.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT;
135  }
136 
137  entity_status.subtype = parameters.subtype;
138  entity_status.time = current_time;
139  entity_status.name = name;
140  entity_status.bounding_box = parameters.bounding_box;
141  entity_status.action_status = traffic_simulator_msgs::msg::ActionStatus();
142  entity_status.action_status.current_action = "waiting for initialize";
143 
144  const auto include_crosswalk = [](const auto & entity_type) {
145  return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) ||
146  (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type);
147  }(entity_status.type);
148 
149  const auto matching_distance = [](const auto & local_parameters) {
150  if constexpr (std::is_same_v<
151  std::decay_t<ParametersType>,
152  traffic_simulator_msgs::msg::VehicleParameters>) {
153  return std::max(
154  local_parameters.axles.front_axle.track_width,
155  local_parameters.axles.rear_axle.track_width) *
156  0.5 +
157  1.0;
158  } else {
159  return local_parameters.bounding_box.dimensions.y * 0.5 + 1.0;
160  }
161  }(parameters);
162 
163  if constexpr (std::is_same_v<std::decay_t<PoseType>, LaneletPose>) {
165  "LaneletPose is not supported type as pose argument. Only CanonicalizedLaneletPose and "
166  "msg::Pose are supported as pose argument of EntityManager::spawnEntity().");
167  } else if constexpr (std::is_same_v<std::decay_t<PoseType>, CanonicalizedLaneletPose>) {
168  entity_status.pose = toMapPose(pose);
169  return CanonicalizedEntityStatus(entity_status, pose);
170  } else if constexpr (std::is_same_v<std::decay_t<PoseType>, geometry_msgs::msg::Pose>) {
171  entity_status.pose = pose;
172  const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
173  pose, parameters.bounding_box, include_crosswalk, matching_distance);
174  return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose);
175  }
176  };
177 
178  if (const auto [iter, success] = entities_.emplace(
179  name, std::make_unique<EntityType>(
180  name, makeEntityStatus(), hdmap_utils_ptr_, parameters,
181  std::forward<decltype(xs)>(xs)...));
182  success) {
183  // FIXME: this ignores V2I traffic lights
184  iter->second->setTrafficLights(traffic_lights_ptr_->getConventionalTrafficLights());
185  return *(iter->second);
186  } else {
187  THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists.");
188  }
189  }
190 
191  // ego - checks, getters
192  auto getNumberOfEgo() const -> std::size_t;
193 
194  auto isAnyEgoSpawned() const -> bool;
195 
196  auto getFirstEgoName() const -> std::optional<std::string>;
197 
198  auto getEgoEntity(const std::string & name) -> entity::EgoEntity &;
199 
200  auto getEgoEntity(const std::string & name) const -> const entity::EgoEntity &;
201 
202  // entities - checks, getters
203  auto isEntityExist(const std::string & name) const -> bool;
204 
205  auto getEntityNames() const -> const std::vector<std::string>;
206 
207  auto getEntity(const std::string & name) -> entity::EntityBase &;
208 
209  auto getEntity(const std::string & name) const -> const entity::EntityBase &;
210 
211  auto getEntityPointer(const std::string & name) const -> std::shared_ptr<entity::EntityBase>;
212 
213  // entities - respawn, despawn, reset
222  auto resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name)
223  -> void;
224 
225  auto despawnEntity(const std::string & name) -> bool;
226 
227  // traffics, lanelet
228  auto getHdmapUtils() -> const std::shared_ptr<hdmap_utils::HdMapUtils> &;
229 
230  template <typename Node>
231  auto getOrigin(Node & node) const
232  {
233  geographic_msgs::msg::GeoPoint origin;
234  origin.latitude = common::getParameter<decltype(origin.latitude)>(
235  node.get_node_parameters_interface(), "origin_latitude");
236  origin.longitude = common::getParameter<decltype(origin.longitude)>(
237  node.get_node_parameters_interface(), "origin_longitude");
238  return origin;
239  }
240 
241  auto calculateEuclideanDistances() -> std::shared_ptr<EuclideanDistancesMap>;
242 
243 private:
244  /* */ Configuration configuration_;
245 
246  const rclcpp::Clock::SharedPtr clock_ptr_;
247 
248  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
249 
250  /* */ tf2_ros::StaticTransformBroadcaster broadcaster_;
251 
252  /* */ tf2_ros::TransformBroadcaster base_link_broadcaster_;
253 
254  const rclcpp::Publisher<EntityStatusWithTrajectoryArray>::SharedPtr entity_status_array_pub_ptr_;
255 
256  const rclcpp::Publisher<MarkerArray>::SharedPtr lanelet_marker_pub_ptr_;
257 
258  /* */ std::unordered_map<std::string, std::shared_ptr<entity::EntityBase>> entities_;
259 
260  /* */ bool npc_logic_started_{false};
261 
262  /* */ std::shared_ptr<TrafficLights> traffic_lights_ptr_{nullptr};
263 
264  const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
265 
266  /* */ MarkerArray markers_raw_;
267 };
268 } // namespace entity
269 } // namespace traffic_simulator
270 #endif // TRAFFIC_SIMULATOR__ENTITY__ENTITY_MANAGER_HPP_
Definition: ego_entity.hpp:35
Definition: entity_base.hpp:53
Definition: entity_manager.hpp:47
auto update(const double current_time, const double step_time) -> void
Definition: entity_manager.cpp:61
auto calculateEuclideanDistances() -> std::shared_ptr< EuclideanDistancesMap >
Definition: entity_manager.cpp:367
auto isAnyEgoSpawned() const -> bool
Definition: entity_manager.cpp:231
auto despawnEntity(const std::string &name) -> bool
Definition: entity_manager.cpp:356
auto getEntityNames() const -> const std::vector< std::string >
Definition: entity_manager.cpp:277
auto getEgoEntity(const std::string &name) -> entity::EgoEntity &
Definition: entity_manager.cpp:248
auto resetBehaviorPlugin(const std::string &name, const std::string &behavior_plugin_name) -> void
Reset behavior plugin of the target entity. The internal behavior is to take over the various paramet...
Definition: entity_manager.cpp:322
auto updateHdmapMarker() const -> void
Definition: entity_manager.cpp:136
auto spawnEntity(const std::string &name, const PoseType &pose, const ParametersType &parameters, const double current_time, Ts &&... xs) -> entity::EntityBase &
Definition: entity_manager.hpp:110
auto getFirstEgoName() const -> std::optional< std::string >
Definition: entity_manager.cpp:238
auto isNpcLogicStarted() const -> bool
Definition: entity_manager.cpp:49
auto broadcastTransform(const geometry_msgs::msg::PoseStamped &pose, const bool static_transform=true) -> void
Definition: entity_manager.cpp:202
EntityManager(NodeT &&node, const Configuration &configuration, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &node_parameters)
Definition: entity_manager.hpp:54
auto isEntityExist(const std::string &name) const -> bool
Definition: entity_manager.cpp:272
auto getHdmapUtils() -> const std::shared_ptr< hdmap_utils::HdMapUtils > &
Definition: entity_manager.cpp:362
auto updateNpcLogic(const std::string &name, const double current_time, const double step_time, const std::shared_ptr< EuclideanDistancesMap > &distances) -> const CanonicalizedEntityStatus &
Definition: entity_manager.cpp:118
auto getOrigin(Node &node) const
Definition: entity_manager.hpp:231
auto broadcastEntityTransform() -> void
Definition: entity_manager.cpp:148
auto setTrafficLights(const std::shared_ptr< TrafficLights > &traffic_lights_ptr) -> void
Definition: entity_manager.cpp:26
auto getEntity(const std::string &name) -> entity::EntityBase &
Definition: entity_manager.cpp:286
auto makeDebugMarker() const -> visualization_msgs::msg::MarkerArray
Definition: entity_manager.cpp:51
auto setVerbose(const bool verbose) -> void
Definition: entity_manager.cpp:32
auto startNpcLogic(const double current_time) -> void
Definition: entity_manager.cpp:40
auto getNumberOfEgo() const -> std::size_t
Definition: entity_manager.cpp:224
auto getEntityPointer(const std::string &name) const -> std::shared_ptr< entity::EntityBase >
Definition: entity_manager.cpp:304
Definition: entity_manager.hpp:41
EntityMarkerQoS(std::size_t depth=100)
Definition: entity_manager.hpp:43
Definition: entity_manager.hpp:35
LaneletMarkerQoS(std::size_t depth=1)
Definition: entity_manager.hpp:37
Definition: pedestrian_entity.hpp:34
Definition: vehicle_entity.hpp:37
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:28
#define THROW_SYNTAX_ERROR(...)
Definition: exception.hpp:62
#define THROW_SEMANTIC_ERROR(...)
Definition: exception.hpp:59
Definition: cache.hpp:28
Definition: lanelet_wrapper.hpp:40
std::unordered_map< std::pair< std::string, std::string >, double > EuclideanDistancesMap
Definition: entity_base.hpp:51
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
geometry_msgs::msg::PoseStamped PoseStamped
Definition: lanelet_wrapper.hpp:67
auto toCanonicalizedLaneletPose(const LaneletPose &lanelet_pose) -> std::optional< CanonicalizedLaneletPose >
Definition: pose.cpp:104
auto toMapPose(const CanonicalizedLaneletPose &lanelet_pose) -> geometry_msgs::msg::Pose
Definition: pose.cpp:87
Definition: api.hpp:32
traffic_simulator_msgs::msg::EntityType EntityType
Definition: entity_status.hpp:26
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:25
Definition: junit5.hpp:25
Definition: configuration.hpp:31
auto makeEntityStatus(traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed=0.0, const std::string name="default_entity_name", const uint8_t type=traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus
Definition: helper_functions.hpp:115