15 #ifndef BEHAVIOR_TREE_PLUGIN__ACTION_NODE_HPP_ 
   16 #define BEHAVIOR_TREE_PLUGIN__ACTION_NODE_HPP_ 
   18 #include <behaviortree_cpp_v3/action_node.h> 
   22 #include <geometry_msgs/msg/point.hpp> 
   35 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp> 
   36 #include <traffic_simulator_msgs/msg/obstacle.hpp> 
   37 #include <traffic_simulator_msgs/msg/waypoints_array.hpp> 
   38 #include <unordered_map> 
   44 BT::PortsList 
operator+(
const BT::PortsList & ports_0, 
const BT::PortsList & ports_1);
 
   52     const lanelet::Ids & route_lanelets,
 
   55     -> std::optional<std::string>;
 
   59     -> std::optional<double>;
 
   61     const std::vector<geometry_msgs::msg::Point> & waypoints, 
const double width,
 
   62     const std::size_t num_segments) 
const -> std::optional<std::pair<std::string, double>>;
 
   64     const lanelet::Ids & route_lanelets,
 
   76   void halt() override final { setStatus(BT::NodeStatus::IDLE); }
 
   82       BT::InputPort<double>(
"current_time"),
 
   83       BT::InputPort<double>(
"matching_distance_for_lanelet_pose_calculation"),
 
   84       BT::InputPort<double>(
"step_time"),
 
   85       BT::InputPort<EntityStatusDict>(
"other_entity_status"),
 
   86       BT::InputPort<lanelet::Ids>(
"route_lanelets"),
 
   87       BT::InputPort<std::optional<double>>(
"target_speed"),
 
   88       BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>(
"hdmap_utils"),
 
   89       BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>(
"canonicalized_entity_status"),
 
   90       BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightsBase>>(
"traffic_lights"),
 
   91       BT::InputPort<traffic_simulator::behavior::Request>(
"request"),
 
   92       BT::InputPort<std::shared_ptr<EuclideanDistancesMap>>(
"euclidean_distances_map"),
 
   93       BT::InputPort<traffic_simulator_msgs::msg::BehaviorParameter>(
"behavior_parameter"),
 
   94       BT::InputPort<std::optional<double>>(
"lateral_collision_threshold"),
 
   95       BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>(
"obstacle"),
 
   96       BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>(
"waypoints"),
 
   97       BT::OutputPort<traffic_simulator::behavior::Request>(
"request"),
 
  108     const double local_target_speed, 
const traffic_simulator_msgs::msg::DynamicConstraints &) 
const 
  111     const double local_target_speed, 
const traffic_simulator_msgs::msg::DynamicConstraints &) 
const 
  135   auto foundConflictingEntity(
const lanelet::Ids & following_lanelets) 
const -> bool;
 
  136   auto getDistanceToTargetEntityOnCrosswalk(
 
  139   auto getConflictingEntityStatusOnCrossWalk(
const lanelet::Ids & route_lanelets) 
const 
  140     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
 
  141   auto getConflictingEntityStatusOnLane(
const lanelet::Ids & route_lanelets) 
const 
  142     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
 
  143   auto getOtherEntityStatus(lanelet::Id lanelet_id) 
const 
  144     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
 
  145   auto isOtherEntityAtConsideredAltitude(
 
  147   auto tick() -> BT::NodeStatus 
override;
 
  149   std::shared_ptr<EuclideanDistancesMap> euclidean_distances_map_;
 
Definition: action_node.hpp:47
auto getFrontEntityNameAndDistanceByTrajectory(const std::vector< geometry_msgs::msg::Point > &waypoints, const double width, const std::size_t num_segments) const -> std::optional< std::pair< std::string, double >>
Definition: action_node.cpp:452
std::optional< double > target_speed_
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:229
void halt() override final
Definition: action_node.hpp:76
~ActionNode() override=default
auto stopEntity() const -> void
Definition: action_node.cpp:124
auto calculateStopDistance(const traffic_simulator_msgs::msg::DynamicConstraints &) const -> double
Definition: action_node.cpp:772
std::optional< double > lateral_collision_threshold_
Definition: action_node.hpp:126
lanelet::Ids route_lanelets_
Definition: action_node.hpp:124
auto calculateUpdatedEntityStatus(const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus
Definition: action_node.cpp:646
auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus &entity_status) -> void
Definition: action_node.cpp:131
EntityStatusDict other_entity_status_
Definition: action_node.hpp:123
auto getHorizon() const -> double
Definition: action_node.cpp:119
double default_matching_distance_for_lanelet_pose_calculation_
Definition: action_node.hpp:121
virtual BT::NodeStatus doAction()=0
auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: action_node.cpp:252
std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils_
Definition: action_node.hpp:116
double step_time_
Definition: action_node.hpp:120
virtual bool checkPreconditions()
Definition: action_node.hpp:128
auto getEntityStatus(const std::string &target_name) const -> const traffic_simulator::CanonicalizedEntityStatus &
Definition: action_node.cpp:324
traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_
Definition: action_node.hpp:125
auto getDistanceToConflictingEntity(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: action_node.cpp:571
auto getYieldStopDistance(const lanelet::Ids &following_lanelets) const -> std::optional< double >
Definition: action_node.cpp:150
auto calculateUpdatedEntityStatusInWorldFrame(const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus
Definition: action_node.cpp:687
auto getFrontEntityName(const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< std::string >
Definition: action_node.cpp:262
auto getRightOfWayEntities() const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
Definition: action_node.cpp:207
std::shared_ptr< traffic_simulator::TrafficLightsBase > traffic_lights_
Definition: action_node.hpp:117
std::shared_ptr< traffic_simulator::CanonicalizedEntityStatus > canonicalized_entity_status_
Definition: action_node.hpp:118
static BT::PortsList providedPorts()
Definition: action_node.hpp:78
ActionNode(const std::string &name, const BT::NodeConfiguration &config)
Definition: action_node.cpp:49
auto executeTick() -> BT::NodeStatus override
throws if the derived class return RUNNING.
Definition: action_node.cpp:54
auto getDistanceToTargetEntity(const math::geometry::CatmullRomSplineInterface &spline, const traffic_simulator::CanonicalizedEntityStatus &status) const -> std::optional< double >
Definition: action_node.cpp:485
double current_time_
Definition: action_node.hpp:119
traffic_simulator::behavior::Request request_
Definition: action_node.hpp:115
virtual auto getBlackBoardValues() -> void
Definition: action_node.cpp:65
Definition: catmull_rom_spline_interface.hpp:30
Definition: entity_status.hpp:32
Definition: action_node.hpp:43
std::unordered_map< std::string, traffic_simulator::CanonicalizedEntityStatus > EntityStatusDict
Definition: behavior_plugin_base.hpp:37
BT::PortsList operator+(const BT::PortsList &ports_0, const BT::PortsList &ports_1)
Definition: action_node.cpp:42
Definition: lanelet_wrapper.hpp:40
Request
Definition: behavior.hpp:25
traffic_simulator_msgs::msg::EntityStatus EntityStatus
Definition: entity_status.hpp:25
std::string string
Definition: junit5.hpp:26