Class STNBTBuilder

Inheritance Relationships

Base Type

Class Documentation

class STNBTBuilder : public plansys2::BTBuilder

Behavior tree builder that uses Simple Temporal Networks (STN) to manage temporal constraints.

This class implements the BTBuilder interface by constructing an STN from a plan, propagating temporal constraints, and then converting the resulting network into a behavior tree for execution. It handles durative actions with temporal constraints and supports concurrent execution of actions.

Public Functions

STNBTBuilder()

Constructor for STNBTBuilder.

virtual void initialize(const std::string &bt_action_1 = "", const std::string &bt_action_2 = "", int precision = 3)

Initializes the builder with behavior tree templates.

Parameters:

  • bt_action_1 – [in] XML template for action start phase, default empty.

  • bt_action_2 – [in] XML template for action end phase, default empty.

  • precision – [in] Precision for time calculations, default 3.

virtual std::string get_tree(const plansys2_msgs::msg::Plan &current_plan)

Generates a behavior tree XML from a plan.

Builds an STN from the plan, propagates temporal constraints, and converts the resulting network into a behavior tree XML representation.

Parameters:

current_plan – [in] The plan to transform into a behavior tree.

Returns:

std::string containing the behavior tree XML, empty if plan is invalid.

inline virtual Graph::Ptr get_graph()

Gets the internal graph representation.

Returns:

Shared pointer to the STN graph.

virtual bool propagate(const Graph::Ptr stn)

Propagates temporal constraints through the STN.

Uses the Floyd-Warshall algorithm to compute all-pairs shortest paths and updates the temporal constraints in the graph.

Parameters:

stn – [in] The graph to propagate constraints through.

Returns:

True if the STN is consistent (no negative cycles), false otherwise.

virtual std::string get_dotgraph(std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map, bool enable_legend = false, bool enable_print_graph = false)

Generates a DOT graph representation of the STN for visualization.

Parameters:

  • action_map – [in] Map of action IDs to execution information.

  • enable_legend – [in] Whether to include a legend in the graph, default false.

  • enable_print_graph – [in] Whether to print the graph to console, default false.

Returns:

std::string containing the DOT graph representation.

Protected Functions

Graph::Ptr build_stn(const plansys2_msgs::msg::Plan &plan) const

Builds an STN from a plan.

Creates a temporal network representing the plan actions and their constraints.

Parameters:

plan – [in] The plan to analyze.

Returns:

Shared pointer to the constructed STN.

std::string build_bt(const Graph::Ptr stn) const

Builds a behavior tree from an STN.

Traverses the STN and generates the corresponding behavior tree XML.

Parameters:

stn – [in] The STN to convert to a behavior tree.

Returns:

std::string containing the behavior tree XML.

Graph::Ptr init_graph(const plansys2_msgs::msg::Plan &plan) const

Initializes an empty graph with nodes for the initial state, actions, and goal.

Parameters:

plan – [in] The plan to extract actions from.

Returns:

Shared pointer to the initialized graph.

std::vector<ActionStamped> get_plan_actions(const plansys2_msgs::msg::Plan &plan) const

Extracts action information from a plan.

Parameters:

plan – [in] The plan to extract actions from.

Returns:

std::vector<ActionStamped> Vector of ActionStamped objects representing the plan actions.

std::set<int> get_happenings(const plansys2_msgs::msg::Plan &plan) const

Gets the set of happening times in a plan.

Happening times are timepoints where actions start or end.

Parameters:

plan – [in] The plan to analyze.

Returns:

Set of integer timepoints representing happenings.

std::set<int>::iterator get_happening(int time, const std::set<int> &happenings) const

Finds the happening time for a given time point.

Parameters:

  • time – [in] The time to find a happening for.

  • happenings – [in] The set of all happening times.

Returns:

Iterator to the happening time, or happenings.end() if not found.

std::set<int>::iterator get_previous(int time, const std::set<int> &happenings) const

Finds the previous happening time before a given time point.

Parameters:

  • time – [in] The time to find the previous happening for.

  • happenings – [in] The set of all happening times.

Returns:

Iterator to the previous happening time, or happenings.end() if not found.

std::multimap<int, ActionStamped> get_simple_plan(const plansys2_msgs::msg::Plan &plan) const

Creates a simplified representation of the plan with snap actions.

Transforms the plan into a multimap where each action is represented by its start, end, and overall points.

Parameters:

plan – [in] The plan to transform.

Returns:

Multimap from time points to action snapshots.

std::map<int, StateVec> get_states(const std::set<int> &happenings, const std::multimap<int, ActionStamped> &plan) const

Computes the state at each happening time in the plan.

Parameters:

  • happenings – [in] The set of happening times.

  • plan – [in] The simplified plan representation.

Returns:

std::map<int, StateVec> Map from happening times to state vectors.

plansys2_msgs::msg::Tree from_state(const std::vector<plansys2::Predicate> &preds, const std::vector<plansys2::Function> &funcs) const

Creates a tree representation from a state.

Parameters:

  • preds – [in] Predicates in the state.

  • funcs – [in] Functions in the state.

Returns:

Tree representing the conjunction of predicates and functions.

std::vector<Node::Ptr> get_nodes(const ActionStamped &action, const Graph::Ptr graph) const

Finds the graph nodes corresponding to an action.

Parameters:

  • action – [in] The action to find nodes for.

  • graph – [in] The graph to search in.

Returns:

std::vector<Node::Ptr> Vector of nodes matching the action.

bool is_match(const Node::Ptr node, const ActionStamped &action) const

Checks if a node matches an action.

Compares the time and expression of the node’s action with the given action.

Parameters:

  • node – [in] The node to check.

  • action – [in] The action to match against.

Returns:

True if the node matches the action, false otherwise.

std::vector<std::pair<int, ActionStamped>> get_parents(const std::pair<int, ActionStamped> &action, const std::multimap<int, ActionStamped> &plan, const std::set<int> &happenings, const std::map<int, StateVec> &states) const

Finds the parent actions that an action depends on.

Combines actions that satisfy the action’s requirements and actions that might threaten the action’s execution.

Parameters:

  • action – [in] The action to find parents for.

  • plan – [in] The simplified plan representation.

  • happenings – [in] The set of happening times.

  • states – [in] The states at each happening time.

Returns:

Vector of parent actions.

std::vector<std::pair<int, ActionStamped>> get_satisfy(const std::pair<int, ActionStamped> &action, const std::multimap<int, ActionStamped> &plan, const std::set<int> &happenings, const std::map<int, StateVec> &states) const

Finds actions that satisfy the requirements of an action.

Parameters:

  • action – [in] The action to find satisfiers for.

  • plan – [in] The simplified plan representation.

  • happenings – [in] The set of happening times.

  • states – [in] The states at each happening time.

Returns:

Vector of actions that satisfy requirements.

std::vector<std::pair<int, ActionStamped>> get_threat(const std::pair<int, ActionStamped> &action, const std::multimap<int, ActionStamped> &plan, const std::set<int> &happenings, const std::map<int, StateVec> &states) const

Finds actions that threaten the execution of an action.

An action threatens another if it could interfere with its requirements or has contradictory effects.

Parameters:

  • action – [in] The action to find threats for.

  • plan – [in] The simplified plan representation.

  • happenings – [in] The set of happening times.

  • states – [in] The states at each happening time.

Returns:

std::vector<std::pair<int, ActionStamped>> Vector of threatening actions.

bool can_apply(const std::pair<int, ActionStamped> &action, const std::multimap<int, ActionStamped> &plan, const int &time, StateVec &state) const

Checks if an action can be applied in a given state.

Determines if there’s a valid ordering of actions at the same time point that would allow the action to be executed.

Parameters:

  • action – [in] The action to check.

  • plan – [in] The simplified plan representation.

  • time – [in] The time point to check at.

  • state – [inout] The state to check in, updated if a valid ordering is found.

Returns:

True if the action can be applied, false otherwise.

StateVec get_diff(const StateVec &X_1, const StateVec &X_2) const

Computes the difference between two states.

Finds predicates and functions that differ between the states.

Parameters:

  • X_1 – [in] The first state.

  • X_2 – [in] The second state.

Returns:

StateVec containing the differences.

StateVec get_intersection(const StateVec &X_1, const StateVec &X_2) const

Computes the intersection of two states.

Finds predicates and functions that are common to both states.

Parameters:

  • X_1 – [in] The first state.

  • X_2 – [in] The second state.

Returns:

StateVec containing the intersection.

plansys2_msgs::msg::Tree get_conditions(const ActionStamped &action) const

Gets the conditions required by an action.

Returns different conditions based on the action type (start, end, overall).

Parameters:

action – [in] The action to get conditions for.

Returns:

Tree representing the conditions.

plansys2_msgs::msg::Tree get_effects(const ActionStamped &action) const

Gets the effects produced by an action.

Returns different effects based on the action type (start, end).

Parameters:

action – [in] The action to get effects for.

Returns:

Tree representing the effects.

void prune_paths(Node::Ptr current, Node::Ptr previous) const

Prunes redundant paths in the graph.

Removes unnecessary edges that are implied by other paths.

Parameters:

  • current – [in] The node to prune paths to.

  • previous – [in] The node to prune paths from.

bool check_paths(Node::Ptr current, Node::Ptr previous) const

Checks if there’s a path between two nodes.

Parameters:

  • current – [in] The destination node.

  • previous – [in] The source node.

Returns:

True if a path exists, false otherwise.

Eigen::MatrixXd get_distance_matrix(const Graph::Ptr stn) const

Computes the distance matrix for an STN.

Creates a matrix where each entry (i,j) represents the maximum allowed time difference between nodes i and j.

Parameters:

stn – [in] The STN to compute distances for.

Returns:

Matrix of distances between nodes.

void floyd_warshall(Eigen::MatrixXd &dist) const

Applies the Floyd-Warshall algorithm to a distance matrix.

Updates the matrix to contain shortest paths between all pairs of nodes.

Parameters:

dist – [inout] The distance matrix to update.

std::string get_flow(const Node::Ptr node, const Node::Ptr prev_node, std::set<Node::Ptr> &used, const int &level) const

Generates the behavior tree XML for a node and its descendants.

Recursively traverses the graph to generate the BT structure.

Parameters:

  • node – [in] The node to process.

  • prev_node – [in] The previous node in the traversal.

  • used – [inout] Set of nodes already processed.

  • level – [in] Indentation level.

Returns:

String containing the behavior tree XML.

std::string start_execution_block(const Node::Ptr node, const int &l) const

Generates the BT XML for the start execution block of an action.

Parameters:

  • node – [in] The action node.

  • l – [in] Indentation level.

Returns:

String containing the BT XML.

std::string end_execution_block(const Node::Ptr node, const int &l) const

Generates the BT XML for the end execution block of an action.

Parameters:

  • node – [in] The action node.

  • l – [in] Indentation level.

Returns:

String containing the BT XML.

void get_flow_dotgraph(Node::Ptr node, std::set<std::string> &edges)

Collects edge definitions for DOT graph visualization.

Parameters:

  • node – [in] The node to process.

  • edges – [inout] Set to collect edge definitions.

std::string get_node_dotgraph(Node::Ptr node, std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map)

Generates DOT graph node definition for a single action node.

Parameters:

  • node – [in] The node to generate a definition for.

  • action_map – [in] Map of action IDs to execution information.

Returns:

String containing the DOT node definition.

ActionExecutor::Status get_action_status(ActionStamped action, std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map)

Gets the execution status of an action.

Parameters:

  • action – [in] The action to check.

  • action_map – [in] Map of action IDs to execution information.

Returns:

Status of the action.

std::string add_dot_graph_legend(int level_counter, int node_counter)

Adds a legend to the DOT graph.

Parameters:

  • level_counter – [in] Counter for cluster IDs.

  • node_counter – [in] Counter for node IDs.

Returns:

String containing the legend definition.

void print_graph(const plansys2::Graph::Ptr graph) const

Prints the entire STN to stderr for debugging.

Parameters:

graph – [in] The graph to print.

void print_node(const Node::Ptr node, int level) const

Prints a node and its descendants to stderr for debugging.

Parameters:

  • node – [in] The node to print.

  • level – [in] Indentation level.

void print_arcs(const plansys2::Graph::Ptr graph) const

Prints all arcs in the STN to stderr for debugging.

Parameters:

graph – [in] The graph to print arcs for.

bool is_end(const std::tuple<Node::Ptr, double, double> &edge, const ActionStamped &action) const

Checks if an edge represents the end of a durative action.

Parameters:

  • edge – [in] The edge to check.

  • action – [in] The action to compare against.

Returns:

True if the edge connects to the end node of the action.

std::string t(const int &level) const

Helper function to generate indentation.

Parameters:

level – [in] Indentation level.

Returns:

std::string with the specified number of tabs.

Protected Attributes

std::shared_ptr<plansys2::DomainExpertClient> domain_client_
std::shared_ptr<plansys2::ProblemExpertClient> problem_client_
Graph::Ptr stn_
std::string bt_start_action_
std::string bt_end_action_
int action_time_precision_