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 {
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  virtual ~EntityBase() = default;
58 
59  virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/);
60 
61  virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &;
62 
63  virtual void cancelRequest();
64 
65  // clang-format off
66 #define DEFINE_GETTER(NAME, TYPE, RETURN_VARIABLE) \
67  \
72  /* */ auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; }
73 
74  DEFINE_GETTER(BoundingBox, const traffic_simulator_msgs::msg::BoundingBox &, status_->getBoundingBox())
75  DEFINE_GETTER(CanonicalizedStatus, const CanonicalizedEntityStatus &, *status_)
76  DEFINE_GETTER(CanonicalizedStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_)
77  DEFINE_GETTER(CurrentAccel, const geometry_msgs::msg::Accel &, status_->getAccel())
78  DEFINE_GETTER(CurrentTwist, const geometry_msgs::msg::Twist &, status_->getTwist())
79  DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints)
80  DEFINE_GETTER(EntitySubtype, const traffic_simulator_msgs::msg::EntitySubtype &, status_->getSubtype())
81  DEFINE_GETTER(EntityType, const traffic_simulator_msgs::msg::EntityType &, status_->getType())
82  DEFINE_GETTER(LinearJerk, double, status_->getLinearJerk())
83  DEFINE_GETTER(MapPose, const geometry_msgs::msg::Pose &, status_->getMapPose())
84  DEFINE_GETTER(StandStillDuration, double, stand_still_duration_)
85  DEFINE_GETTER(TraveledDistance, double, traveled_distance_)
86  // clang-format on
87 #undef DEFINE_GETTER
88 
89  // clang-format off
90 #define DEFINE_CHECK_FUNCTION(FUNCTION_NAME, BOOL_VARIABLE) \
91  \
94  /* */ auto FUNCTION_NAME() const->bool { return BOOL_VARIABLE; }
95 
97  // clang-format on
98 #undef DEFINE_CHECK_FUNCTION
99 
100  /* */ auto get2DPolygon() const -> std::vector<geometry_msgs::msg::Point>;
101 
102  virtual auto getCurrentAction() const -> std::string = 0;
103 
104  virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0;
105 
106  virtual auto getDefaultDynamicConstraints() const
107  -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0;
108 
109  virtual auto getEntityTypename() const -> const std::string & = 0;
110 
111  virtual auto getGoalPoses() -> std::vector<CanonicalizedLaneletPose> = 0;
112 
113  /* */ auto getCanonicalizedLaneletPose() const -> std::optional<CanonicalizedLaneletPose>;
114 
115  /* */ auto getCanonicalizedLaneletPose(const double matching_distance) const
116  -> std::optional<CanonicalizedLaneletPose>;
117 
118  virtual auto getMaxAcceleration() const -> double = 0;
119 
120  virtual auto getMaxDeceleration() const -> double = 0;
121 
123 
124  virtual auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> = 0;
125 
126  virtual auto getRouteLanelets(const double horizon = 100.0) -> lanelet::Ids = 0;
127 
128  virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
129 
130  virtual auto onUpdate(const double current_time, const double step_time) -> void;
131 
132  virtual auto onPostUpdate(const double current_time, const double step_time) -> void;
133 
135 
137 
138  virtual void requestAcquirePosition(const geometry_msgs::msg::Pose &) = 0;
139 
140  virtual void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) = 0;
141 
142  virtual void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) = 0;
143 
144  virtual void requestLaneChange(const lanelet::Id)
145  {
150  }
151 
153  {
158  }
159 
160  /* */ void requestLaneChange(
161  const lane_change::AbsoluteTarget & target, const lane_change::TrajectoryShape trajectory_shape,
162  const lane_change::Constraint & constraint);
163 
164  /* */ void requestLaneChange(
165  const lane_change::RelativeTarget & target, const lane_change::TrajectoryShape trajectory_shape,
166  const lane_change::Constraint & constraint);
167 
168  virtual void requestSpeedChange(
169  const double target_speed, const speed_change::Transition transition,
170  const speed_change::Constraint constraint, const bool continuous);
171 
172  virtual void requestSpeedChange(
173  const speed_change::RelativeTargetSpeed & target_speed,
174  const speed_change::Transition transition, const speed_change::Constraint constraint,
175  const bool continuous);
176 
177  virtual void requestSpeedChange(const double target_speed, const bool continuous);
178 
179  virtual void requestSpeedChange(
180  const speed_change::RelativeTargetSpeed & target_speed, const bool continuous);
181 
182  virtual void requestClearRoute()
183  {
188  }
189 
190  virtual auto isControlledBySimulator() const -> bool;
191 
192  virtual auto setControlledBySimulator(const bool /*unused*/) -> void;
193 
194  virtual auto requestFollowTrajectory(
195  const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & /*unused*/) -> void;
196 
197  virtual void requestWalkStraight();
198 
199  virtual void setAccelerationLimit(const double acceleration) = 0;
200 
201  virtual void setAccelerationRateLimit(const double acceleration_rate) = 0;
202 
203  virtual void setDecelerationLimit(const double deceleration) = 0;
204 
205  virtual void setDecelerationRateLimit(const double deceleration_rate) = 0;
206 
207  /* */ void setDynamicConstraints(
208  const traffic_simulator_msgs::msg::DynamicConstraints & constraints);
209 
210  virtual void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) = 0;
211 
212  /* */ void setOtherStatus(
213  const std::unordered_map<std::string, CanonicalizedEntityStatus> & status);
214 
215  virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void;
216 
217  virtual auto setStatus(const EntityStatus & status) -> void;
218 
219  /* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void;
220 
221  virtual auto setLinearAcceleration(const double linear_acceleration) -> void;
222 
223  virtual auto setLinearVelocity(const double linear_velocity) -> void;
224 
225  virtual void setTrafficLights(
226  const std::shared_ptr<traffic_simulator::TrafficLightsBase> & traffic_lights);
227 
228  virtual auto activateOutOfRangeJob(
229  const double min_velocity, const double max_velocity, const double min_acceleration,
230  const double max_acceleration, const double min_jerk, const double max_jerk) -> void;
231 
232  virtual auto setVelocityLimit(const double) -> void = 0;
233 
234  virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;
235 
236  /* */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void;
237 
238  /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void;
239 
240  /* */ auto setAction(const std::string & action) -> void;
241 
242  /* */ auto setLinearJerk(const double liner_jerk) -> void;
243 
244  /* */ void stopAtCurrentPosition();
245 
246  /* */ void updateEntityStatusTimestamp(const double current_time);
247 
248  /* */ bool reachPosition(
249  const geometry_msgs::msg::Pose & target_pose, const double tolerance) const;
250 
251  /* */ bool reachPosition(
252  const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const;
253 
254  /* */ bool reachPosition(const std::string & target_name, const double tolerance) const;
255 
256  /* */ auto requestSynchronize(
257  const std::string & target_name, const CanonicalizedLaneletPose & target_sync_pose,
258  const CanonicalizedLaneletPose & entity_target, const double target_speed,
259  const double tolerance) -> bool;
260 
261  const std::string name;
262 
263  bool verbose;
264 
265 protected:
267 
269 
270  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
272 
273  double stand_still_duration_ = 0.0;
274  double traveled_distance_ = 0.0;
275  double prev_job_duration_ = 0.0;
276  double step_time_ = 0.0;
277 
278  std::unordered_map<std::string, CanonicalizedEntityStatus> other_status_;
279 
280  std::optional<double> target_speed_;
281  traffic_simulator::job::JobList job_list_;
282 
283  std::unique_ptr<traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner>
285 
286 private:
287  virtual auto requestSpeedChangeWithConstantAcceleration(
288  const double target_speed, const speed_change::Transition, const double acceleration,
289  const bool continuous) -> void;
290  virtual auto requestSpeedChangeWithConstantAcceleration(
291  const speed_change::RelativeTargetSpeed & target_speed,
292  const speed_change::Transition transition, const double acceleration, const bool continuous)
293  -> void;
294  virtual auto requestSpeedChangeWithTimeConstraint(
295  const double target_speed, const speed_change::Transition, const double acceleration_time)
296  -> void;
297  virtual auto requestSpeedChangeWithTimeConstraint(
298  const speed_change::RelativeTargetSpeed & target_speed,
299  const speed_change::Transition transition, const double time) -> void;
300  /* */ auto isTargetSpeedReached(const double target_speed) const -> bool;
301  /* */ auto isTargetSpeedReached(const speed_change::RelativeTargetSpeed & target_speed) const
302  -> bool;
303 };
304 } // namespace entity
305 } // namespace traffic_simulator
306 
307 #endif // TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_
Definition: traffic_lights_base.hpp:39
Definition: entity_base.hpp:51
virtual auto setStatus(const EntityStatus &status, const lanelet::Ids &lanelet_ids) -> void
Definition: entity_base.cpp:546
auto setTwist(const geometry_msgs::msg::Twist &twist) -> void
Definition: entity_base.cpp:578
virtual auto setLinearAcceleration(const double linear_acceleration) -> void
Definition: entity_base.cpp:567
virtual void setDecelerationLimit(const double deceleration)=0
CanonicalizedEntityStatus status_before_update_
Definition: entity_base.hpp:262
auto setLinearJerk(const double liner_jerk) -> void
Definition: entity_base.cpp:588
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:519
virtual void setAccelerationLimit(const double acceleration)=0
double stand_still_duration_
Definition: entity_base.hpp:267
virtual auto onUpdate(const double current_time, const double step_time) -> void
Definition: entity_base.cpp:122
virtual void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)=0
void stopAtCurrentPosition()
Definition: entity_base.cpp:643
traffic_simulator::job::JobList job_list_
Definition: entity_base.hpp:275
void setOtherStatus(const std::unordered_map< std::string, CanonicalizedEntityStatus > &status)
Definition: entity_base.cpp:539
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:681
const std::string name
Definition: entity_base.hpp:255
virtual auto getGoalPoses() -> std::vector< CanonicalizedLaneletPose >=0
void resetDynamicConstraints()
Definition: entity_base.cpp:137
virtual void requestClearRoute()
Definition: entity_base.hpp:176
virtual void requestSpeedChange(const double target_speed, const speed_change::Transition transition, const speed_change::Constraint constraint, const bool continuous)
Definition: entity_base.cpp:293
std::unordered_map< std::string, CanonicalizedEntityStatus > other_status_
Definition: entity_base.hpp:272
virtual auto getCurrentAction() const -> std::string=0
virtual auto isControlledBySimulator() const -> bool
Definition: entity_base.cpp:511
virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter=0
double step_time_
Definition: entity_base.hpp:270
std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils_ptr_
Definition: entity_base.hpp:264
auto getLinearJerk() const noexcept -> double
Get LinearJerk of the entity.
Definition: entity_base.hpp:78
virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray &)
Definition: entity_base.cpp:69
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:79
std::unique_ptr< traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner > speed_planner_
Definition: entity_base.hpp:278
virtual void requestWalkStraight()
Definition: entity_base.cpp:526
virtual auto onPostUpdate(const double current_time, const double step_time) -> void
Definition: entity_base.cpp:132
EntityBase(const std::string &name, const CanonicalizedEntityStatus &entity_status, const std::shared_ptr< hdmap_utils::HdMapUtils > &hdmap_utils_ptr)
Definition: entity_base.cpp:33
auto setAcceleration(const geometry_msgs::msg::Accel &accel) -> void
Definition: entity_base.cpp:583
auto getCanonicalizedLaneletPose() const -> std::optional< CanonicalizedLaneletPose >
Definition: entity_base.cpp:86
bool reachPosition(const geometry_msgs::msg::Pose &target_pose, const double tolerance) const
Definition: entity_base.cpp:660
void setDynamicConstraints(const traffic_simulator_msgs::msg::DynamicConstraints &constraints)
Definition: entity_base.cpp:531
virtual auto getEntityTypename() const -> const std::string &=0
auto setAction(const std::string &action) -> void
Definition: entity_base.cpp:593
auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &
Get MapPose of the entity.
Definition: entity_base.hpp:79
virtual auto setControlledBySimulator(const bool) -> void
Definition: entity_base.cpp:513
auto laneMatchingSucceed() const -> bool
Definition: entity_base.hpp:90
double prev_job_duration_
Definition: entity_base.hpp:269
bool verbose
Definition: entity_base.hpp:257
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:601
double traveled_distance_
Definition: entity_base.hpp:268
void updateEntityStatusTimestamp(const double current_time)
Definition: entity_base.cpp:650
std::shared_ptr< CanonicalizedEntityStatus > status_
Definition: entity_base.hpp:260
std::shared_ptr< traffic_simulator::TrafficLightsBase > traffic_lights_
Definition: entity_base.hpp:265
virtual auto getMaxAcceleration() const -> double=0
auto setCanonicalizedStatus(const CanonicalizedEntityStatus &status) -> void
Definition: entity_base.cpp:557
virtual auto setMapPose(const geometry_msgs::msg::Pose &map_pose) -> void
Definition: entity_base.cpp:595
virtual auto setVelocityLimit(const double) -> void=0
virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &
Definition: entity_base.cpp:71
virtual void setTrafficLights(const std::shared_ptr< traffic_simulator::TrafficLightsBase > &traffic_lights)
Definition: entity_base.cpp:572
virtual void requestLaneChange(const lanelet::Id)
Definition: entity_base.hpp:138
virtual auto getRouteLanelets(const double horizon=100.0) -> lanelet::Ids=0
std::optional< double > target_speed_
Definition: entity_base.hpp:274
virtual auto setLinearVelocity(const double linear_velocity) -> void
Definition: entity_base.cpp:562
auto get2DPolygon() const -> std::vector< geometry_msgs::msg::Point >
Definition: entity_base.cpp:81
virtual auto getMaxDeceleration() const -> double=0
virtual auto getObstacle() -> std::optional< traffic_simulator_msgs::msg::Obstacle >=0
virtual auto getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double
Definition: entity_base.cpp:105
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:27
#define DEFINE_GETTER(NAME, TYPE, RETURN_VARIABLE)
Definition: entity_base.hpp:66
#define DEFINE_CHECK_FUNCTION(FUNCTION_NAME, BOOL_VARIABLE)
Definition: entity_base.hpp:86
Definition: autoware_universe.hpp:40
Definition: cache.hpp:46
Definition: cache.hpp:27
TrajectoryShape
Definition: lane_change.hpp:31
Transition
Definition: speed_change.hpp:26
Definition: api.hpp:48
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::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