15 #ifndef TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_
18 #include <autoware_control_msgs/msg/control.hpp>
19 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
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>
44 #include <visualization_msgs/msg/marker_array.hpp>
50 class EntityBase :
public std::enable_shared_from_this<EntityBase>
55 const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr);
68 auto
is() const ->
bool
70 return dynamic_cast<EntityType const *
>(
this) !=
nullptr;
73 template <
typename EntityType>
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");
84 template <
typename EntityType>
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");
100 #define DEFINE_GETTER(NAME, TYPE, RETURN_VARIABLE) \
106 auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; }
128 virtual auto
getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0;
131 -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0;
148 const lanelet::Id lanelet_id, std::optional<double> tolerance = std::nullopt)
const -> bool;
161 virtual auto
getObstacle() ->
std::optional<traffic_simulator_msgs::msg::Obstacle> = 0;
163 virtual auto
getRouteLanelets(const
double horizon = 100.0) -> lanelet::Ids = 0;
165 virtual auto
getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
167 virtual auto
onUpdate(const
double current_time, const
double step_time) ->
void;
169 virtual auto
onPostUpdate(const
double current_time, const
double step_time) ->
void;
214 const bool continuous);
226 const
std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & ) ->
void;
239 const traffic_simulator_msgs::msg::DynamicConstraints & constraints);
253 const geometry_msgs::msg::
Pose & map_pose,
254 const traffic_simulator_msgs::msg::ActionStatus & action_status =
258 const geometry_msgs::msg::
Pose & reference_pose, const geometry_msgs::msg::
Pose & relative_pose,
259 const traffic_simulator_msgs::msg::ActionStatus & action_status =
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 =
271 const traffic_simulator_msgs::msg::ActionStatus & action_status =
276 const traffic_simulator_msgs::msg::ActionStatus & action_status =
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;
292 virtual auto
setMapPose(const geometry_msgs::msg::
Pose & map_pose) ->
void;
294 auto
setTwist(const geometry_msgs::msg::Twist & twist) ->
void;
309 const
double tolerance) ->
bool;
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)
344 virtual auto requestSpeedChangeWithTimeConstraint(
345 const
double target_speed, const speed_change::
Transition, const
double acceleration_time)
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
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: 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
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:34
Definition: lane_change.hpp:44
parameters for behavior plugin
Definition: lane_change.hpp:75
Definition: lane_change.hpp:58
Definition: speed_change.hpp:35
Definition: speed_change.hpp:51