entity_behavior::ActionNode Class Reference

#include <action_node.hpp>

Inheritance diagram for entity_behavior::ActionNode:

Collaboration diagram for entity_behavior::ActionNode:

Public Member Functions
	ActionNode (const std::string &name, const BT::NodeConfiguration &config)
	~ActionNode () override=default
auto	foundConflictingEntity (const lanelet::Ids &following_lanelets) const -> bool
auto	getDistanceToConflictingEntity (const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
auto	getFrontEntityName (const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< std::string >
auto	calculateStopDistance (const traffic_simulator_msgs::msg::DynamicConstraints &) const -> double
auto	getDistanceToFrontEntity (const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
auto	getDistanceToTrafficLightStopLine (const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
auto	getRightOfWayEntities () const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
auto	getRightOfWayEntities (const lanelet::Ids &following_lanelets) const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
auto	getYieldStopDistance (const lanelet::Ids &following_lanelets) const -> std::optional< double >
auto	getOtherEntityStatus (lanelet::Id lanelet_id) const -> std::vector< traffic_simulator::CanonicalizedEntityStatus >
auto	stopEntity () const -> void
auto	getHorizon () const -> double
auto	executeTick () -> BT::NodeStatus override
	throws if the derived class return RUNNING. More...
void	halt () override final
virtual auto	getBlackBoardValues () -> void
auto	getEntityStatus (const std::string &target_name) const -> const traffic_simulator::CanonicalizedEntityStatus &
auto	setCanonicalizedEntityStatus (const traffic_simulator::EntityStatus &entity_status) -> void
auto	calculateUpdatedEntityStatus (const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus
auto	calculateUpdatedEntityStatusInWorldFrame (const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus

Static Public Member Functions
static BT::PortsList	providedPorts ()

Protected Member Functions
auto	getDistanceToTargetEntity (const math::geometry::CatmullRomSplineInterface &spline, const traffic_simulator::CanonicalizedEntityStatus &status) const -> std::optional< double >

Protected Attributes
traffic_simulator::behavior::Request	request
std::shared_ptr< hdmap_utils::HdMapUtils >	hdmap_utils
std::shared_ptr< traffic_simulator::TrafficLightsBase >	traffic_lights
std::shared_ptr< traffic_simulator::CanonicalizedEntityStatus >	canonicalized_entity_status
double	current_time
double	step_time
double	default_matching_distance_for_lanelet_pose_calculation
std::optional< double >	target_speed
EntityStatusDict	other_entity_status
lanelet::Ids	route_lanelets
std::shared_ptr< EuclideanDistancesMap >	euclidean_distances_map
traffic_simulator_msgs::msg::BehaviorParameter	behavior_parameter

Constructor & Destructor Documentation

ActionNode()

entity_behavior::ActionNode::ActionNode	(	const std::string &	name,
		const BT::NodeConfiguration &	config
	)

~ActionNode()

entity_behavior::ActionNode::~ActionNode ( )

overridedefault

Member Function Documentation

calculateStopDistance()

auto entity_behavior::ActionNode::calculateStopDistance

(

const traffic_simulator_msgs::msg::DynamicConstraints &

constraints

)

const -> double

calculateUpdatedEntityStatus()

auto entity_behavior::ActionNode::calculateUpdatedEntityStatus	(	const double	local_target_speed,
		const traffic_simulator_msgs::msg::DynamicConstraints &	constraints
	)		const -> traffic_simulator::EntityStatus

Todo:

it will be moved to route::moveAlongLaneletPose(...)

calculateUpdatedEntityStatusInWorldFrame()

auto entity_behavior::ActionNode::calculateUpdatedEntityStatusInWorldFrame	(	const double	local_target_speed,
		const traffic_simulator_msgs::msg::DynamicConstraints &	constraints
	)		const -> traffic_simulator::EntityStatus

Note

Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)

Apply position change

Todo:

first determine global desired_velocity, calculate position change using it then pass the same global desired_velocity to updatePositionForLaneletTransition()

Note

If it is the transition between lanelets: overwrite position to improve precision

Handle lanelet transition

executeTick()

auto entity_behavior::ActionNode::executeTick ( ) -> BT::NodeStatus

override

throws if the derived class return RUNNING.

foundConflictingEntity()

auto entity_behavior::ActionNode::foundConflictingEntity

(

const lanelet::Ids &

following_lanelets

)

const -> bool

getBlackBoardValues()

auto entity_behavior::ActionNode::getBlackBoardValues ( ) -> void

virtual

Reimplemented in entity_behavior::VehicleActionNode, entity_behavior::vehicle::LaneChangeAction, entity_behavior::vehicle::follow_lane_sequence::MoveBackwardAction, entity_behavior::vehicle::follow_lane_sequence::FollowLaneAction, entity_behavior::pedestrian::WalkStraightAction, entity_behavior::PedestrianActionNode, and entity_behavior::pedestrian::FollowLaneAction.

getDistanceToConflictingEntity()

auto entity_behavior::ActionNode::getDistanceToConflictingEntity	(	const lanelet::Ids &	route_lanelets,
		const math::geometry::CatmullRomSplineInterface &	spline
	)		const -> std::optional<double>

getDistanceToFrontEntity()

auto entity_behavior::ActionNode::getDistanceToFrontEntity

(

const math::geometry::CatmullRomSplineInterface &

spline

)

const -> std::optional<double>

getDistanceToTargetEntity()

auto entity_behavior::ActionNode::getDistanceToTargetEntity ( const math::geometry::CatmullRomSplineInterface &  spline, const traffic_simulator::CanonicalizedEntityStatus &  status  ) const -> std::optional<double>

protected

Note

getDistanceToTargetEntity working schematics

1. Check if route to target entity from reference entity exists, if not try to transform pose to other routable lanelet, within matching distance (findRoutableAlternativeLaneletPoseFrom).
2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance.
3. Calculate longitudinal distance between entities poses -> longitudinal_distance.
4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners) -> target_to_spline_distance.
5. If target_to_spline_distance is less than half width of reference entity target entity is conflicting.
6. Check corner case where target entity width is bigger than width of entity and target entity is exactly on the spline -> spline.getCollisionPointIn2D
7. If target entity is conflicting return bounding_box_distance enlarged by half of the entity length.

boundingBoxLaneLongitudinalDistance requires routing_configuration, 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets

Todo:

rotation of NPC is not taken into account, same as in boundingBoxLaneLongitudinalDistance this should be considered to be changed in separate task in the future

Note

if the distance of the target entity to the spline is smaller than the width of the reference entity

if the distance of the target entity to the spline cannot be calculated because a collision occurs

getDistanceToTrafficLightStopLine()

auto entity_behavior::ActionNode::getDistanceToTrafficLightStopLine	(	const lanelet::Ids &	route_lanelets,
		const math::geometry::CatmullRomSplineInterface &	spline
	)		const -> std::optional<double>

getEntityStatus()

auto entity_behavior::ActionNode::getEntityStatus

(

const std::string &

target_name

)

const -> const traffic_simulator::CanonicalizedEntityStatus &

getFrontEntityName()

auto entity_behavior::ActionNode::getFrontEntityName

(

const math::geometry::CatmullRomSplineInterface &

spline

)

const -> std::optional<std::string>

Note

Euclidean distance is here used as a "rough" distance to filter only NPCs which possibly are in range of current horizon. Because euclidean distance is the shortest possible distance comparing it with horizon will never omit NPCs for which actual lane distance is in range of horizon.

hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.

getHorizon()

auto entity_behavior::ActionNode::getHorizon

(

)

const -> double

getOtherEntityStatus()

auto entity_behavior::ActionNode::getOtherEntityStatus

(

lanelet::Id

lanelet_id

)

const -> std::vector<traffic_simulator::CanonicalizedEntityStatus>

getRightOfWayEntities() [1/2]

auto entity_behavior::ActionNode::getRightOfWayEntities

(

)

const -> std::vector<traffic_simulator::CanonicalizedEntityStatus>

getRightOfWayEntities() [2/2]

auto entity_behavior::ActionNode::getRightOfWayEntities

(

const lanelet::Ids &

following_lanelets

)

const -> std::vector<traffic_simulator::CanonicalizedEntityStatus>

getYieldStopDistance()

auto entity_behavior::ActionNode::getYieldStopDistance

(

const lanelet::Ids &

following_lanelets

)

const -> std::optional<double>

halt()

void entity_behavior::ActionNode::halt ( )

inlinefinaloverride

providedPorts()

static BT::PortsList entity_behavior::ActionNode::providedPorts ( )

inlinestatic

setCanonicalizedEntityStatus()

auto entity_behavior::ActionNode::setCanonicalizedEntityStatus

(

const traffic_simulator::EntityStatus &

entity_status

)

-> void

stopEntity()

auto entity_behavior::ActionNode::stopEntity

(

)

const -> void

Member Data Documentation

behavior_parameter

traffic_simulator_msgs::msg::BehaviorParameter entity_behavior::ActionNode::behavior_parameter

protected

canonicalized_entity_status

std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus> entity_behavior::ActionNode::canonicalized_entity_status

protected

current_time

double entity_behavior::ActionNode::current_time

protected

default_matching_distance_for_lanelet_pose_calculation

double entity_behavior::ActionNode::default_matching_distance_for_lanelet_pose_calculation

protected

euclidean_distances_map

std::shared_ptr<EuclideanDistancesMap> entity_behavior::ActionNode::euclidean_distances_map

protected

hdmap_utils

std::shared_ptr<hdmap_utils::HdMapUtils> entity_behavior::ActionNode::hdmap_utils

protected

other_entity_status

EntityStatusDict entity_behavior::ActionNode::other_entity_status

protected

request

traffic_simulator::behavior::Request entity_behavior::ActionNode::request

protected

route_lanelets

lanelet::Ids entity_behavior::ActionNode::route_lanelets

protected

step_time

double entity_behavior::ActionNode::step_time

protected

target_speed

std::optional<double> entity_behavior::ActionNode::target_speed

protected

traffic_lights

std::shared_ptr<traffic_simulator::TrafficLightsBase> entity_behavior::ActionNode::traffic_lights

protected

---

The documentation for this class was generated from the following files:

• simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp
• simulation/behavior_tree_plugin/src/action_node.cpp