15 #ifndef TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_
19 #include <boost/filesystem.hpp>
27 #include <traffic_simulator_msgs/msg/entity_type.hpp>
36 const std::unique_ptr<concealer::FieldOperatorApplication> field_operator_application;
38 static auto makeFieldOperatorApplication(
39 const Configuration &,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &)
40 -> std::unique_ptr<concealer::FieldOperatorApplication>;
42 bool is_controlled_by_simulator_{
false};
43 std::optional<double> target_speed_;
44 traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_;
45 std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> polyline_trajectory_;
52 const std::shared_ptr<hdmap_utils::HdMapUtils> &,
53 const traffic_simulator_msgs::msg::VehicleParameters &,
const Configuration &,
54 const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &);
73 -> const traffic_simulator_msgs::msg::DynamicConstraints & override;
81 auto
getObstacle() ->
std::optional<traffic_simulator_msgs::msg::Obstacle> override;
85 auto
getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
89 void onUpdate(
double current_time,
double step_time) override;
100 const
std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) ->
void override;
107 const
double, const speed_change::
Transition, const speed_change::Constraint,
108 const
bool continuous) ->
void override;
111 const speed_change::RelativeTargetSpeed &, const speed_change::
Transition,
112 const speed_change::Constraint, const
bool continuous) ->
void override;
120 is_controlled_by_simulator_ = state;
133 auto setMapPose(
const geometry_msgs::msg::Pose & map_pose) ->
void override;
135 template <
typename... Ts>
140 "You cannot set entity status to the ego vehicle named ", std::quoted(
status_->getName()),
141 " after starting scenario.");
Definition: ego_entity.hpp:35
auto getObstacle() -> std::optional< traffic_simulator_msgs::msg::Obstacle > override
Definition: ego_entity.cpp:107
auto setMapPose(const geometry_msgs::msg::Pose &map_pose) -> void override
Definition: ego_entity.cpp:294
void requestAssignRoute(const std::vector< CanonicalizedLaneletPose > &) override
Definition: ego_entity.cpp:180
void requestLaneChange(const lanelet::Id) override
Definition: ego_entity.cpp:225
void requestAcquirePosition(const CanonicalizedLaneletPose &) override
Definition: ego_entity.cpp:170
auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override
Definition: ego_entity.cpp:132
auto getEntityStatus(const double, const double) const -> const CanonicalizedEntityStatus
auto getCurrentPose() const -> const geometry_msgs::msg::Pose &
Definition: ego_entity.cpp:127
auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints &override
Definition: ego_entity.cpp:259
~EgoEntity() override=default
auto setStatus(Ts &&... xs)
Definition: ego_entity.hpp:136
auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &override
Definition: ego_entity.cpp:81
void requestClearRoute() override
Definition: ego_entity.cpp:257
auto updateFieldOperatorApplication() const -> void
Definition: ego_entity.cpp:137
void onUpdate(double current_time, double step_time) override
Definition: ego_entity.cpp:143
auto setControlledBySimulator(bool state) -> void override
Definition: ego_entity.hpp:118
auto getEntityTypename() const -> const std::string &override
Definition: ego_entity.cpp:101
auto requestFollowTrajectory(const std::shared_ptr< traffic_simulator_msgs::msg::PolylineTrajectory > &) -> void override
Definition: ego_entity.cpp:217
auto setVelocityLimit(double) -> void override
Definition: ego_entity.cpp:288
auto operator=(EgoEntity &&) -> EgoEntity &=delete
auto getCurrentAction() const -> std::string override
Definition: ego_entity.cpp:87
auto operator=(const EgoEntity &) -> EgoEntity &=delete
auto getRouteLanelets(double horizon=100) -> lanelet::Ids override
Definition: ego_entity.cpp:112
EgoEntity(const EgoEntity &)=delete
auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override
Definition: ego_entity.cpp:265
EgoEntity(EgoEntity &&)=delete
auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override
Definition: ego_entity.cpp:93
auto requestSpeedChange(const double, const speed_change::Transition, const speed_change::Constraint, const bool continuous) -> void override
Definition: ego_entity.cpp:241
auto isControlledBySimulator() const -> bool override
Definition: ego_entity.cpp:215
virtual auto setStatus(const EntityStatus &status, const lanelet::Ids &lanelet_ids) -> void
Definition: entity_base.cpp:546
const std::string name
Definition: entity_base.hpp:255
std::shared_ptr< CanonicalizedEntityStatus > status_
Definition: entity_base.hpp:260
Definition: vehicle_entity.hpp:37
Definition: entity_status.hpp:32
Definition: lanelet_pose.hpp:27
#define THROW_SEMANTIC_ERROR(...)
Definition: exception.hpp:59
Definition: autoware_universe.hpp:40
Transition
Definition: speed_change.hpp:26
Definition: junit5.hpp:25
std::string string
Definition: junit5.hpp:26
Definition: configuration.hpp:30
Definition: speed_change.hpp:51