15 #ifndef TRAFFIC_SIMULATOR__ENTITY__VEHICLE_ENTITY_HPP_
16 #define TRAFFIC_SIMULATOR__ENTITY__VEHICLE_ENTITY_HPP_
20 #include <pluginlib/class_loader.hpp>
21 #include <pugixml.hpp>
22 #include <rclcpp/rclcpp.hpp>
27 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
28 #include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>
29 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
43 static const std::string name =
"behavior_tree_plugin/VehicleBehaviorTree";
64 const std::shared_ptr<hdmap_utils::HdMapUtils> &,
65 const traffic_simulator_msgs::msg::VehicleParameters &,
70 void appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array)
override;
77 -> const traffic_simulator_msgs::msg::DynamicConstraints & override;
87 auto
getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override;
93 auto
getObstacle() ->
std::optional<traffic_simulator_msgs::msg::Obstacle> override;
97 auto
getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
99 void onUpdate(
double current_time,
double step_time) override;
110 const
std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) ->
void override;
142 std::shared_ptr<
math::geometry::CatmullRomSpline> spline_;
144 lanelet::Ids previous_route_lanelets_;
Definition: route_planner.hpp:27
Definition: traffic_light_manager.hpp:34
Definition: entity_base.hpp:51
const std::string name
Definition: entity_base.hpp:242
Definition: vehicle_entity.hpp:37
void setDecelerationLimit(double deceleration) override
Definition: vehicle_entity.cpp:312
auto getEntityTypename() const -> const std::string &override
Definition: vehicle_entity.cpp:96
auto getGoalPoses() -> std::vector< CanonicalizedLaneletPose > override
Definition: vehicle_entity.cpp:102
auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override
Definition: vehicle_entity.cpp:122
void setVelocityLimit(double linear_velocity) override
Definition: vehicle_entity.cpp:282
void onUpdate(double current_time, double step_time) override
Definition: vehicle_entity.cpp:140
void setAccelerationLimit(double acceleration) override
Definition: vehicle_entity.cpp:292
void requestAssignRoute(const std::vector< geometry_msgs::msg::Pose > &) override
Definition: vehicle_entity.cpp:218
void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override
Definition: vehicle_entity.cpp:332
auto getCurrentAction() const -> std::string override
Definition: vehicle_entity.cpp:59
auto getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double override
Definition: vehicle_entity.cpp:75
auto requestFollowTrajectory(const std::shared_ptr< traffic_simulator_msgs::msg::PolylineTrajectory > &) -> void override
Definition: vehicle_entity.cpp:233
auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints &override
Definition: vehicle_entity.cpp:68
VehicleEntity(const std::string &name, const CanonicalizedEntityStatus &, const std::shared_ptr< hdmap_utils::HdMapUtils > &, const traffic_simulator_msgs::msg::VehicleParameters &, const std::string &plugin_name=BuiltinBehavior::defaultBehavior())
Definition: vehicle_entity.cpp:26
void appendDebugMarker(visualization_msgs::msg::MarkerArray &marker_array) override
Definition: vehicle_entity.cpp:47
auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override
Definition: vehicle_entity.cpp:84
void requestAcquirePosition(const CanonicalizedLaneletPose &)
Definition: vehicle_entity.cpp:184
void requestLaneChange(const lanelet::Id to_lanelet_id) override
Definition: vehicle_entity.cpp:253
~VehicleEntity() override=default
void setAccelerationRateLimit(double acceleration_rate) override
Definition: vehicle_entity.cpp:302
void setTrafficLightManager(const std::shared_ptr< traffic_simulator::TrafficLightManager > &) override
Definition: vehicle_entity.cpp:338
const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters
Definition: vehicle_entity.hpp:133
void setDecelerationRateLimit(double deceleration_rate) override
Definition: vehicle_entity.cpp:322
auto getMaxAcceleration() const -> double override
Definition: vehicle_entity.cpp:268
auto getMaxDeceleration() const -> double override
Definition: vehicle_entity.cpp:275
auto getRouteLanelets(double horizon=100) -> lanelet::Ids override
Definition: vehicle_entity.cpp:112
auto fillLaneletPose(CanonicalizedEntityStatus &status) -> void override
Definition: vehicle_entity.cpp:345
void cancelRequest() override
Definition: vehicle_entity.cpp:53
auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType &override
Definition: vehicle_entity.cpp:89
auto getObstacle() -> std::optional< traffic_simulator_msgs::msg::Obstacle > override
Definition: vehicle_entity.cpp:107
Definition: entity_status.hpp:29
Definition: lanelet_pose.hpp:27
Definition: action_node.hpp:38
Definition: bounding_box.hpp:32
std::string string
Definition: junit5.hpp:26
Definition: vehicle_entity.hpp:40
static auto defaultBehavior() -> const std::string &
Definition: vehicle_entity.hpp:59
static auto doNothing() noexcept -> const std::string &
Definition: vehicle_entity.hpp:53
static auto behaviorTree() -> const std::string &
Definition: vehicle_entity.hpp:41
static auto contextGamma() -> const std::string &
Definition: vehicle_entity.hpp:47