Class ExecutorNode

Inheritance Relationships

Base Type

  • public rclcpp_lifecycle::LifecycleNode

Class Documentation

class ExecutorNode : public rclcpp_lifecycle::LifecycleNode

ROS2 Lifecycle node that manages plan execution.

This node receives plans to execute, transforms them into behavior trees, and monitors their execution. It also provides services to retrieve information about the current execution state and supports replanning when necessary.

Public Types

using ExecutePlan = plansys2_msgs::action::ExecutePlan
using GoalHandleExecutePlan = rclcpp_action::ServerGoalHandle<ExecutePlan>
using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Public Functions

explicit ExecutorNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Constructor for the ExecutorNode.

virtual ~ExecutorNode()

Destructor for the ExecutorNode.

CallbackReturnT on_configure(const rclcpp_lifecycle::State &state)

Configures the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if configuration is successful, FAILURE otherwise.

CallbackReturnT on_activate(const rclcpp_lifecycle::State &state)

Activates the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if activation is successful, FAILURE otherwise.

CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &state)

Deactivates the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if deactivation is successful, FAILURE otherwise.

CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &state)

Cleans up the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if cleanup is successful, FAILURE otherwise.

CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &state)

Shuts down the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if shutdown is successful, FAILURE otherwise.

CallbackReturnT on_error(const rclcpp_lifecycle::State &state)

Handles errors in the node.

Parameters:

state – [in] The current lifecycle state.

Returns:

SUCCESS if error handling is successful, FAILURE otherwise.

void get_ordered_sub_goals_service_callback(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<plansys2_msgs::srv::GetOrderedSubGoals::Request> request, const std::shared_ptr<plansys2_msgs::srv::GetOrderedSubGoals::Response> response)

Service callback to get ordered sub-goals derived from the plan.

Parameters:

  • request_header – [in] ROS service request header.

  • request – [in] Empty service request.

  • response – [out] Service response containing the ordered sub-goals and success status.

void get_plan_service_callback(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<plansys2_msgs::srv::GetPlan::Request> request, const std::shared_ptr<plansys2_msgs::srv::GetPlan::Response> response)

Service callback to get the complete plan being executed.

Parameters:

  • request_header – [in] ROS service request header.

  • request – [in] Empty service request.

  • response – [out] Service response containing the complete plan and success status.

void get_remaining_plan_service_callback(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<plansys2_msgs::srv::GetPlan::Request> request, const std::shared_ptr<plansys2_msgs::srv::GetPlan::Response> response)

Service callback to get the remaining plan yet to be executed.

Parameters:

  • request_header – [in] ROS service request header.

  • request – [in] Empty service request.

  • response – [out] Service response containing the remaining plan and success status.

Protected Functions

void get_ordered_subgoals(PlanRuntineInfo &runtime_info)

Extracts ordered sub-goals from a plan.

Parameters:

runtime_info – [inout] Runtime information to update with ordered sub-goals.

std::vector<plansys2_msgs::msg::ActionExecutionInfo> get_feedback_info(std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map)

Constructs feedback information about action execution.

Parameters:

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

Returns:

Vector of action execution info messages.

void print_execution_info(std::shared_ptr<std::map<std::string, ActionExecutionInfo>> exec_info)

Prints execution information to stderr for debugging.

Parameters:

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

rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const ExecutePlan::Goal> goal)

Callback for handling new goal requests.

Parameters:

  • uuid – [in] Goal UUID.

  • goal – [in] Goal containing the plan to execute.

Returns:

Goal response indicating acceptance.

rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleExecutePlan> goal_handle)

Callback for handling cancellation requests.

Parameters:

goal_handle – [in] Handle to the goal being canceled.

Returns:

Cancel response indicating acceptance or rejection.

void handle_accepted(const std::shared_ptr<GoalHandleExecutePlan> goal_handle)

Callback for handling accepted goals.

Parameters:

goal_handle – [in] Handle to the accepted goal.

void purge_handlers_list()

Removes completed goal handlers from the list.

void execution_cycle()

Main execution cycle that runs in a separate thread.

Manages the state machine for plan execution, including processing new plans, executing current plans, handling cancellations, and reporting results.

void update_plan(PlanRuntineInfo &runtime_info)

Updates the remaining plan based on action execution status.

Parameters:

runtime_info – [inout] Runtime information to update.

bool init_plan_for_execution(PlanRuntineInfo &runtime_info)

Initializes runtime information for a new plan.

Parameters:

runtime_info – [inout] Runtime information to initialize.

Returns:

True if initialization was successful, false otherwise.

bool replan_for_execution(PlanRuntineInfo &runtime_info)

Reinitializes runtime information for replanning.

Parameters:

runtime_info – [inout] Runtime information to reinitialize.

Returns:

True if reinitialization was successful, false otherwise.

void execute_plan()

Executes the current plan using the behavior tree.

bool get_tree_from_plan(PlanRuntineInfo &runtime_info)

Builds a behavior tree from the current plan.

Parameters:

runtime_info – [inout] Runtime information containing the plan.

Returns:

True if tree creation was successful, false otherwise.

void create_plan_runtime_info(PlanRuntineInfo &runtime_info)

Creates runtime information structures for a plan.

Parameters:

runtime_info – [out] Runtime information to create.

void cancel_all_running_actions(PlanRuntineInfo &runtime_info)

Cancels all currently running actions.

Parameters:

runtime_info – [inout] Runtime information containing the actions.

void add_groot_monitoring(BT::Tree *tree, uint16_t server_port)

Add Groot2 monitor to publish BT status changes.

Parameters:

  • tree – BT to monitor

  • server_port – Groot2 Server port, first of the pair (server_port, publisher_port)

void reset_groot_monitor()

Reset Groot2 monitor.

Protected Attributes

std::string action_bt_xml_
std::string start_action_bt_xml_
std::string end_action_bt_xml_
pluginlib::ClassLoader<plansys2::BTBuilder> bt_builder_loader_
std::shared_ptr<plansys2::DomainExpertClient> domain_client_
std::shared_ptr<plansys2::ProblemExpertClient> problem_client_
std::shared_ptr<plansys2::PlannerClient> planner_client_
rclcpp_lifecycle::LifecyclePublisher<plansys2_msgs::msg::ActionExecutionInfo>::SharedPtr execution_info_pub_
rclcpp_lifecycle::LifecyclePublisher<plansys2_msgs::msg::Plan>::SharedPtr executing_plan_pub_
rclcpp_lifecycle::LifecyclePublisher<plansys2_msgs::msg::Plan>::SharedPtr remaining_plan_pub_
rclcpp_action::Server<ExecutePlan>::SharedPtr execute_plan_action_server_
rclcpp::Service<plansys2_msgs::srv::GetOrderedSubGoals>::SharedPtr get_ordered_sub_goals_service_
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr dotgraph_pub_
rclcpp::Service<plansys2_msgs::srv::GetPlan>::SharedPtr get_plan_service_
rclcpp::Service<plansys2_msgs::srv::GetPlan>::SharedPtr get_remaining_plan_service_
rclcpp::TimerBase::SharedPtr execution_timer_
bool cancel_plan_requested_
bool replan_requested_
bool new_plan_received_ = {false}
bool cancel_requested_ = {false}
bool node_running_ = {true}
std::list<std::shared_ptr<GoalHandleExecutePlan>> goal_handlers_
std::shared_ptr<GoalHandleExecutePlan> current_goal_handle_
std::shared_ptr<GoalHandleExecutePlan> new_goal_handle_
std::shared_ptr<GoalHandleExecutePlan> cancel_goal_handle_
int executor_state_
PlanRuntineInfo runtime_info_
std::unique_ptr<BT::Groot2Publisher> groot_monitor_

Protected Static Attributes

static const int STATE_IDLE = 0
static const int STATE_EXECUTING = 1
static const int STATE_REPLANNING = 2
static const int STATE_ABORTING = 3
static const int STATE_CANCELLED = 4
static const int STATE_FAILED = 5
static const int STATE_SUCCEDED = 6
static const int STATE_ERROR = 7