scenario_simulator_v2 C++ API
entity_base.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_BASE_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_
17 
18 #include <autoware_control_msgs/msg/control.hpp>
19 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
21 #include <memory>
22 #include <optional>
23 #include <queue>
24 #include <string>
35 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
36 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
37 #include <traffic_simulator_msgs/msg/entity_status.hpp>
38 #include <traffic_simulator_msgs/msg/entity_type.hpp>
39 #include <traffic_simulator_msgs/msg/obstacle.hpp>
40 #include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>
41 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
42 #include <unordered_map>
43 #include <vector>
44 #include <visualization_msgs/msg/marker_array.hpp>
45 
46 namespace traffic_simulator
47 {
48 namespace entity
49 {
50 class EntityBase : public std::enable_shared_from_this<EntityBase>
51 {
52 public:
53  explicit EntityBase(
54  const std::string & name, const CanonicalizedEntityStatus & entity_status,
55  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr);
56 
57  EntityBase(const EntityBase &) = delete;
58 
59  EntityBase & operator=(const EntityBase &) = delete;
60 
61  EntityBase(EntityBase &&) noexcept = delete;
62 
63  EntityBase & operator=(EntityBase &&) noexcept = delete;
64 
65  virtual ~EntityBase() = default;
66 
67  template <typename EntityType>
68  /* */ auto is() const -> bool
69  {
70  return dynamic_cast<EntityType const *>(this) != nullptr;
71  }
72 
73  template <typename EntityType>
74  /* */ auto as() -> EntityType &
75  {
76  if (const auto derived_ptr = dynamic_cast<EntityType *>(this); !derived_ptr) {
78  "Entity ", std::quoted(name), " is not ", std::quoted(typeid(EntityType).name()), "type");
79  } else {
80  return *derived_ptr;
81  }
82  }
83 
84  template <typename EntityType>
85  /* */ auto as() const -> const EntityType &
86  {
87  if (const auto derived_ptr = dynamic_cast<EntityType const *>(this); !derived_ptr) {
89  "Entity ", std::quoted(name), " is not ", std::quoted(typeid(EntityType).name()), "type");
90  } else {
91  return *derived_ptr;
92  }
93  }
94 
95  virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/);
96 
97  virtual void cancelRequest();
98 
99  // clang-format off
100 #define DEFINE_GETTER(NAME, TYPE, RETURN_VARIABLE) \
101  \
106  /* */ auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; }
107 
109  DEFINE_GETTER(CanonicalizedStatus, const CanonicalizedEntityStatus &, *status_)
110  DEFINE_GETTER(CanonicalizedStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_)
111  DEFINE_GETTER(CurrentAccel, const geometry_msgs::msg::Accel &, status_->getAccel())
112  DEFINE_GETTER(CurrentTwist, const geometry_msgs::msg::Twist &, status_->getTwist())
113  DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints)
114  DEFINE_GETTER(EntitySubtype, const traffic_simulator_msgs::msg::EntitySubtype &, status_->getSubtype())
115  DEFINE_GETTER(EntityType, const traffic_simulator_msgs::msg::EntityType &, status_->getType())
116  DEFINE_GETTER(LinearJerk, double, status_->getLinearJerk())
117  DEFINE_GETTER(MapPose, const geometry_msgs::msg::Pose &, status_->getMapPose())
118  DEFINE_GETTER(StandStillDuration, double, stand_still_duration_)
119  DEFINE_GETTER(TraveledDistance, double, traveled_distance_)
120  DEFINE_GETTER(Name, const std::string &, status_->getName())
121  // clang-format on
122 #undef DEFINE_GETTER
123 
124  /* */ auto get2DPolygon() const -> std::vector<geometry_msgs::msg::Point>;
125 
126  virtual auto getCurrentAction() const -> std::string = 0;
127 
128  virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0;
129 
130  virtual auto getDefaultDynamicConstraints() const
131  -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0;
132 
133  virtual auto getEntityTypename() const -> const std::string & = 0;
134 
135  virtual auto getGoalPoses() -> std::vector<CanonicalizedLaneletPose> = 0;
136 
137  /* */ auto isStopped() const -> bool;
138 
139  /* */ auto isNearbyPosition(const geometry_msgs::msg::Pose & pose, const double tolerance) const
140  -> bool;
141 
142  /* */ auto isNearbyPosition(
143  const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool;
144 
145  /* */ auto isInLanelet() const -> bool { return status_->isInLanelet(); };
146 
147  /* */ auto isInLanelet(
148  const lanelet::Id lanelet_id, std::optional<double> tolerance = std::nullopt) const -> bool;
149 
150  /* */ auto getCanonicalizedLaneletPose() const -> std::optional<CanonicalizedLaneletPose>;
151 
152  /* */ auto getCanonicalizedLaneletPose(const double matching_distance) const
154 
155  virtual auto getMaxAcceleration() const -> double = 0;
156 
157  virtual auto getMaxDeceleration() const -> double = 0;
158 
160 
161  virtual auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> = 0;
162 
163  virtual auto getRouteLanelets(const double horizon = 100.0) -> lanelet::Ids = 0;
164 
165  virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
166 
167  virtual auto onUpdate(const double current_time, const double step_time) -> void;
168 
169  virtual auto onPostUpdate(const double current_time, const double step_time) -> void;
170 
172 
174 
175  virtual void requestAcquirePosition(const geometry_msgs::msg::Pose &) = 0;
176 
177  virtual void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) = 0;
178 
179  virtual void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) = 0;
180 
181  virtual auto requestLaneChange(const lanelet::Id) -> void
182  {
187  }
188 
189  virtual auto requestLaneChange(const lane_change::Parameter &) -> void
190  {
195  }
196 
197  /* */ auto requestLaneChange(const lane_change::Direction & direction) -> void;
198 
199  /* */ auto requestLaneChange(
200  const lane_change::AbsoluteTarget & target, const lane_change::TrajectoryShape trajectory_shape,
201  const lane_change::Constraint & constraint) -> void;
202 
203  /* */ auto requestLaneChange(
204  const lane_change::RelativeTarget & target, const lane_change::TrajectoryShape trajectory_shape,
205  const lane_change::Constraint & constraint) -> void;
206 
207  virtual auto requestSpeedChange(
208  const double target_speed, const speed_change::Transition,
209  const speed_change::Constraint constraint, const bool continuous) -> void;
210 
211  virtual void requestSpeedChange(
212  const speed_change::RelativeTargetSpeed & target_speed,
213  const speed_change::Transition transition, const speed_change::Constraint constraint,
214  const bool continuous);
215 
216  virtual void requestSpeedChange(const double target_speed, const bool continuous);
217 
218  virtual void requestSpeedChange(
219  const speed_change::RelativeTargetSpeed & target_speed, const bool continuous);
220 
221  virtual auto isControlledBySimulator() const -> bool;
222 
223  virtual auto setControlledBySimulator(const bool /*unused*/) -> void;
224 
225  virtual auto requestFollowTrajectory(
226  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & /*unused*/) -> void;
227 
228  virtual void requestWalkStraight();
229 
230  virtual void setAccelerationLimit(const double acceleration) = 0;
231 
232  virtual void setAccelerationRateLimit(const double acceleration_rate) = 0;
233 
234  virtual void setDecelerationLimit(const double deceleration) = 0;
235 
236  virtual void setDecelerationRateLimit(const double deceleration_rate) = 0;
237 
238  /* */ void setDynamicConstraints(
239  const traffic_simulator_msgs::msg::DynamicConstraints & constraints);
240 
241  virtual void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) = 0;
242 
243  /* */ void setOtherStatus(
244  const std::unordered_map<std::string, CanonicalizedEntityStatus> & status);
245 
246  /* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void;
247 
248  virtual auto setStatus(const EntityStatus & status) -> void;
249 
250  virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void;
251 
252  virtual auto setStatus(
253  const geometry_msgs::msg::Pose & map_pose,
254  const traffic_simulator_msgs::msg::ActionStatus & action_status =
255  helper::constructActionStatus()) -> void;
256 
257  virtual auto setStatus(
258  const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & relative_pose,
259  const traffic_simulator_msgs::msg::ActionStatus & action_status =
260  helper::constructActionStatus()) -> void;
261 
262  virtual auto setStatus(
263  const geometry_msgs::msg::Pose & reference_pose,
264  const geometry_msgs::msg::Point & relative_position,
265  const geometry_msgs::msg::Vector3 & relative_rpy,
266  const traffic_simulator_msgs::msg::ActionStatus & action_status =
267  helper::constructActionStatus()) -> void;
268 
269  virtual auto setStatus(
270  const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
271  const traffic_simulator_msgs::msg::ActionStatus & action_status =
272  helper::constructActionStatus()) -> void;
273 
274  virtual auto setStatus(
275  const LaneletPose & lanelet_pose,
276  const traffic_simulator_msgs::msg::ActionStatus & action_status =
277  helper::constructActionStatus()) -> void;
278 
279  virtual auto setLinearAcceleration(const double linear_acceleration) -> void;
280 
281  virtual auto setLinearVelocity(const double linear_velocity) -> void;
282 
283  virtual void setTrafficLights(
284  const std::shared_ptr<traffic_simulator::TrafficLightsBase> & traffic_lights);
285 
286  virtual auto activateOutOfRangeJob(
287  const double min_velocity, const double max_velocity, const double min_acceleration,
288  const double max_acceleration, const double min_jerk, const double max_jerk) -> void;
289 
290  virtual auto setVelocityLimit(const double) -> void = 0;
291 
292  virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;
293 
294  /* */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void;
295 
296  /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void;
297 
298  /* */ auto setAction(const std::string & action) -> void;
299 
300  /* */ auto setLinearJerk(const double liner_jerk) -> void;
301 
302  /* */ void stopAtCurrentPosition();
303 
304  /* */ void updateEntityStatusTimestamp(const double current_time);
305 
306  /* */ auto requestSynchronize(
307  const std::string & target_name, const CanonicalizedLaneletPose & target_sync_pose,
308  const CanonicalizedLaneletPose & entity_target, const double target_speed,
309  const double tolerance) -> bool;
310 
311  const std::string name;
312 
313  bool verbose;
314 
315 protected:
317 
319 
320  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
322 
323  double stand_still_duration_ = 0.0;
324  double traveled_distance_ = 0.0;
325  double prev_job_duration_ = 0.0;
326  double step_time_ = 0.0;
327 
328  std::unordered_map<std::string, CanonicalizedEntityStatus> other_status_;
329 
330  std::optional<double> target_speed_;
331  traffic_simulator::job::JobList job_list_;
332 
333  std::unique_ptr<traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner>
335 
336 private:
337  virtual auto requestSpeedChangeWithConstantAcceleration(
338  const double target_speed, const speed_change::Transition, const double acceleration,
339  const bool continuous) -> void;
340  virtual auto requestSpeedChangeWithConstantAcceleration(
341  const speed_change::RelativeTargetSpeed & target_speed,
342  const speed_change::Transition transition, const double acceleration, const bool continuous)
343  -> void;
344  virtual auto requestSpeedChangeWithTimeConstraint(
345  const double target_speed, const speed_change::Transition, const double acceleration_time)
346  -> void;
347  virtual auto requestSpeedChangeWithTimeConstraint(
348  const speed_change::RelativeTargetSpeed & target_speed,
349  const speed_change::Transition transition, const double time) -> void;
350  /* */ auto isTargetSpeedReached(const double target_speed) const -> bool;
351  /* */ auto isTargetSpeedReached(const speed_change::RelativeTargetSpeed & target_speed) const
352  -> bool;
353 };
354 } // namespace entity
355 } // namespace traffic_simulator
356 
357 #endif // TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_
Definition: traffic_lights_base.hpp:39
Definition: entity_base.hpp:51
auto setTwist(const geometry_msgs::msg::Twist &twist) -> void
Definition: entity_base.cpp:666
virtual auto setLinearAcceleration(const double linear_acceleration) -> void
Definition: entity_base.cpp:655
virtual void setDecelerationLimit(const double deceleration)=0
CanonicalizedEntityStatus status_before_update_
Definition: entity_base.hpp:314
auto setLinearJerk(const double liner_jerk) -> void
Definition: entity_base.cpp:676
virtual void setDecelerationRateLimit(const double deceleration_rate)=0
virtual auto requestFollowTrajectory(const std::shared_ptr< traffic_simulator_msgs::msg::PolylineTrajectory > &) -> void
Definition: entity_base.cpp:551
auto as() const -> const EntityType &
Definition: entity_base.hpp:85
virtual auto requestSpeedChange(const double target_speed, const speed_change::Transition, const speed_change::Constraint constraint, const bool continuous) -> void
Definition: entity_base.cpp:325
auto is() const -> bool
Definition: entity_base.hpp:68
virtual void setAccelerationLimit(const double acceleration)=0
double stand_still_duration_
Definition: entity_base.hpp:319
virtual auto onUpdate(const double current_time, const double step_time) -> void
Definition: entity_base.cpp:143
virtual void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)=0
void stopAtCurrentPosition()
Definition: entity_base.cpp:731
auto isNearbyPosition(const geometry_msgs::msg::Pose &pose, const double tolerance) const -> bool
Definition: entity_base.cpp:98
traffic_simulator::job::JobList job_list_
Definition: entity_base.hpp:327
void setOtherStatus(const std::unordered_map< std::string, CanonicalizedEntityStatus > &status)
Definition: entity_base.cpp:571
virtual auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints &=0
virtual void requestAcquirePosition(const CanonicalizedLaneletPose &)=0
auto requestSynchronize(const std::string &target_name, const CanonicalizedLaneletPose &target_sync_pose, const CanonicalizedLaneletPose &entity_target, const double target_speed, const double tolerance) -> bool
Definition: entity_base.cpp:752
const std::string name
Definition: entity_base.hpp:307
virtual auto getGoalPoses() -> std::vector< CanonicalizedLaneletPose >=0
void resetDynamicConstraints()
Definition: entity_base.cpp:158
EntityBase(const EntityBase &)=delete
std::unordered_map< std::string, CanonicalizedEntityStatus > other_status_
Definition: entity_base.hpp:324
virtual auto getCurrentAction() const -> std::string=0
virtual auto isControlledBySimulator() const -> bool
Definition: entity_base.cpp:543
virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter=0
double step_time_
Definition: entity_base.hpp:322
auto isStopped() const -> bool
Definition: entity_base.cpp:126
std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils_ptr_
Definition: entity_base.hpp:316
auto getLinearJerk() const noexcept -> double
Get LinearJerk of the entity.
Definition: entity_base.hpp:112
virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray &)
Definition: entity_base.cpp:70
virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray=0
virtual void setAccelerationRateLimit(const double acceleration_rate)=0
virtual void cancelRequest()
Definition: entity_base.cpp:72
std::unique_ptr< traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner > speed_planner_
Definition: entity_base.hpp:330
virtual void requestWalkStraight()
Definition: entity_base.cpp:558
virtual auto onPostUpdate(const double current_time, const double step_time) -> void
Definition: entity_base.cpp:153
virtual auto requestLaneChange(const lanelet::Id) -> void
Definition: entity_base.hpp:177
EntityBase(const std::string &name, const CanonicalizedEntityStatus &entity_status, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils_ptr)
Definition: entity_base.cpp:34
auto setAcceleration(const geometry_msgs::msg::Accel &accel) -> void
Definition: entity_base.cpp:671
auto getCanonicalizedLaneletPose() const -> std::optional< CanonicalizedLaneletPose >
Definition: entity_base.cpp:79
auto getName() const noexcept -> const std::string &
Get Name of the entity.
Definition: entity_base.hpp:116
void setDynamicConstraints(const traffic_simulator_msgs::msg::DynamicConstraints &constraints)
Definition: entity_base.cpp:563
virtual auto getEntityTypename() const -> const std::string &=0
auto setAction(const std::string &action) -> void
Definition: entity_base.cpp:681
auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &
Get MapPose of the entity.
Definition: entity_base.hpp:113
virtual auto setControlledBySimulator(const bool) -> void
Definition: entity_base.cpp:545
double prev_job_duration_
Definition: entity_base.hpp:321
bool verbose
Definition: entity_base.hpp:309
virtual void requestAssignRoute(const std::vector< CanonicalizedLaneletPose > &)=0
virtual auto activateOutOfRangeJob(const double min_velocity, const double max_velocity, const double min_acceleration, const double max_acceleration, const double min_jerk, const double max_jerk) -> void
Definition: entity_base.cpp:689
double traveled_distance_
Definition: entity_base.hpp:320
void updateEntityStatusTimestamp(const double current_time)
Definition: entity_base.cpp:738
std::shared_ptr< CanonicalizedEntityStatus > status_
Definition: entity_base.hpp:312
std::shared_ptr< traffic_simulator::TrafficLightsBase > traffic_lights_
Definition: entity_base.hpp:317
auto isInLanelet() const -> bool
Definition: entity_base.hpp:141
virtual auto getMaxAcceleration() const -> double=0
auto setCanonicalizedStatus(const CanonicalizedEntityStatus &status) -> void
Definition: entity_base.cpp:578
EntityBase(EntityBase &&) noexcept=delete
virtual auto setMapPose(const geometry_msgs::msg::Pose &map_pose) -> void
Definition: entity_base.cpp:683
virtual auto setVelocityLimit(const double) -> void=0
auto as() -> EntityType &
Definition: entity_base.hpp:74
virtual void setTrafficLights(const std::shared_ptr< traffic_simulator::TrafficLightsBase > &traffic_lights)
Definition: entity_base.cpp:660
EntityBase & operator=(const EntityBase &)=delete
virtual auto getRouteLanelets(const double horizon=100.0) -> lanelet::Ids=0
std::optional< double > target_speed_
Definition: entity_base.hpp:326
virtual auto setLinearVelocity(const double linear_velocity) -> void
Definition: entity_base.cpp:650
auto get2DPolygon() const -> std::vector< geometry_msgs::msg::Point >
Definition: entity_base.cpp:74
virtual auto getMaxDeceleration() const -> double=0
virtual auto setStatus(const EntityStatus &status) -> void
Definition: entity_base.cpp:588
virtual auto getObstacle() -> std::optional< traffic_simulator_msgs::msg::Obstacle >=0
virtual auto getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double
Definition: entity_base.cpp:121
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:28
#define THROW_SEMANTIC_ERROR(...)
Definition: exception.hpp:59
#define DEFINE_GETTER(NAME, TYPE, RETURN_VARIABLE)
Definition: entity_base.hpp:100
Definition: cache.hpp:28
Definition: lanelet_wrapper.hpp:40
traffic_simulator_msgs::msg::ActionStatus constructActionStatus(double linear_vel=0, double angular_vel=0, double linear_accel=0, double angular_accel=0)
helper function for constructing action status
Definition: helper.cpp:25
TrajectoryShape
Definition: lane_change.hpp:31
Direction
Definition: lane_change.hpp:29
geometry_msgs::msg::Pose Pose
Definition: lanelet_map.hpp:30
geometry_msgs::msg::Point Point
Definition: lanelet_map.hpp:29
traffic_simulator_msgs::msg::BoundingBox BoundingBox
Definition: lanelet_wrapper.hpp:62
geometry_msgs::msg::Vector3 Vector3
Definition: lanelet_wrapper.hpp:69
Transition
Definition: speed_change.hpp:26
Definition: api.hpp:32
traffic_simulator_msgs::msg::EntityType EntityType
Definition: entity_status.hpp:26
traffic_simulator_msgs::msg::EntitySubtype EntitySubtype
Definition: entity_status.hpp:27
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:25
std::string string
Definition: junit5.hpp:26
Definition: lane_change.hpp:44
parameters for behavior plugin
Definition: lane_change.hpp:75
Definition: speed_change.hpp:35