15 #ifndef BEHAVIOR_TREE_PLUGIN__ACTION_NODE_HPP_
16 #define BEHAVIOR_TREE_PLUGIN__ACTION_NODE_HPP_
18 #include <behaviortree_cpp_v3/action_node.h>
32 #include <traffic_simulator_msgs/msg/obstacle.hpp>
33 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
34 #include <unordered_map>
49 -> std::optional<std::string>;
53 -> std::optional<double>;
56 const std::vector<geometry_msgs::msg::Point> & waypoints)
const -> std::optional<double>;
68 auto
getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus;
74 void halt() override final { setStatus(BT::NodeStatus::IDLE); }
80 BT::InputPort<double>(
"current_time"),
81 BT::InputPort<double>(
"matching_distance_for_lanelet_pose_calculation"),
82 BT::InputPort<double>(
"step_time"),
83 BT::InputPort<EntityStatusDict>(
"other_entity_status"),
84 BT::InputPort<lanelet::Ids>(
"route_lanelets"),
85 BT::InputPort<std::optional<double>>(
"target_speed"),
86 BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>(
"hdmap_utils"),
87 BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>(
"entity_status"),
88 BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>(
"traffic_light_manager"),
89 BT::InputPort<traffic_simulator::behavior::Request>(
"request"),
90 BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>(
"obstacle"),
91 BT::OutputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>(
"updated_status"),
92 BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>(
"waypoints"),
93 BT::OutputPort<traffic_simulator::behavior::Request>(
"request"),
102 double width_extension_right = 0.0,
double width_extension_left = 0.0,
103 double length_extension_front = 0.0,
double length_extension_rear = 0.0) const
104 ->
std::optional<
double>;
106 double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const
109 double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const
126 auto getDistanceToTargetEntityOnCrosswalk(
127 const
math::geometry::CatmullRomSplineInterface & spline,
130 const
math::geometry::CatmullRomSplineInterface & spline,
131 const
traffic_simulator::CanonicalizedEntityStatus & status,
double width_extension_right = 0.0,
132 double width_extension_left = 0.0,
double length_extension_front = 0.0,
133 double length_extension_rear = 0.0) const ->
std::optional<
double>;
134 auto getConflictingEntityStatus(const lanelet::Ids & following_lanelets) const
136 auto getConflictingEntityStatusOnCrossWalk(const lanelet::Ids &
route_lanelets) const
138 auto getConflictingEntityStatusOnLane(const lanelet::Ids &
route_lanelets) const
Definition: action_node.hpp:40
EntityStatusDict other_entity_status
Definition: action_node.hpp:122
auto getDistanceToTrafficLightStopLine(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: action_node.cpp:191
void halt() override final
Definition: action_node.hpp:74
~ActionNode() override=default
lanelet::Ids route_lanelets
Definition: action_node.hpp:123
auto stopEntity() const -> void
Definition: action_node.cpp:90
auto calculateStopDistance(const traffic_simulator_msgs::msg::DynamicConstraints &) const -> double
Definition: action_node.cpp:517
auto getDistanceToStopLine(const lanelet::Ids &route_lanelets, const std::vector< geometry_msgs::msg::Point > &waypoints) const -> std::optional< double >
Definition: action_node.cpp:219
auto getEntityStatus(const std::string &target_name) const -> traffic_simulator::CanonicalizedEntityStatus
Definition: action_node.cpp:280
std::shared_ptr< traffic_simulator::TrafficLightManager > traffic_light_manager
Definition: action_node.hpp:115
auto getHorizon() const -> double
Definition: action_node.cpp:85
double step_time
Definition: action_node.hpp:118
traffic_simulator::behavior::Request request
Definition: action_node.hpp:113
auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: action_node.cpp:226
auto getDistanceToTargetEntityPolygon(const math::geometry::CatmullRomSplineInterface &spline, const std::string target_name, double width_extension_right=0.0, double width_extension_left=0.0, double length_extension_front=0.0, double length_extension_rear=0.0) const -> std::optional< double >
Definition: action_node.cpp:289
auto getDistanceToConflictingEntity(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: action_node.cpp:319
double default_matching_distance_for_lanelet_pose_calculation
Definition: action_node.hpp:119
auto getYieldStopDistance(const lanelet::Ids &following_lanelets) const -> std::optional< double >
Definition: action_node.cpp:111
auto getFrontEntityName(const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< std::string >
Definition: action_node.cpp:236
std::shared_ptr< traffic_simulator::CanonicalizedEntityStatus > entity_status
Definition: action_node.hpp:116
auto getRightOfWayEntities() const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
Definition: action_node.cpp:167
auto getOtherEntityStatus(lanelet::Id lanelet_id) const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
Definition: action_node.cpp:97
auto calculateUpdatedEntityStatus(double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::CanonicalizedEntityStatus
Definition: action_node.cpp:399
double current_time
Definition: action_node.hpp:117
static BT::PortsList providedPorts()
Definition: action_node.hpp:76
auto getEntityName() const noexcept -> std::string
Definition: action_node.cpp:532
ActionNode(const std::string &name, const BT::NodeConfiguration &config)
Definition: action_node.cpp:36
auto executeTick() -> BT::NodeStatus override
throws if the derived class return RUNNING.
Definition: action_node.cpp:41
auto foundConflictingEntity(const lanelet::Ids &following_lanelets) const -> bool
Definition: action_node.cpp:377
auto getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus
Definition: action_node.cpp:527
std::shared_ptr< traffic_simulator::CanonicalizedEntityStatus > updated_status
Definition: action_node.hpp:121
std::optional< double > target_speed
Definition: action_node.hpp:120
auto calculateUpdatedEntityStatusInWorldFrame(double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::CanonicalizedEntityStatus
Definition: action_node.cpp:478
auto getBlackBoardValues() -> void
Definition: action_node.cpp:43
Definition: catmull_rom_spline_interface.hpp:30
Definition: entity_status.hpp:29
Definition: action_node.hpp:38
std::unordered_map< std::string, traffic_simulator::CanonicalizedEntityStatus > EntityStatusDict
Definition: behavior_plugin_base.hpp:37
Definition: bounding_box.hpp:32
Request
Definition: behavior.hpp:25
std::string string
Definition: junit5.hpp:26