diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 05c664d..068544a 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include "behaviortree_cpp/action_node.h" @@ -152,8 +153,15 @@ class RosActionNode : public BT::ActionNodeBase } /** Callback invoked when something goes wrong. + * The result is provided if it is available. * It must return either SUCCESS or FAILURE. */ + virtual BT::NodeStatus onFailure(ActionNodeErrorCode error, + const std::optional&) + { + return onFailure(error); + } + virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) { return NodeStatus::FAILURE; @@ -390,7 +398,7 @@ inline NodeStatus RosActionNode::tick() if(!setGoal(goal)) { - return CheckStatus(onFailure(INVALID_GOAL)); + return CheckStatus(onFailure(INVALID_GOAL, {})); } typename ActionClient::SendGoalOptions goal_options; @@ -432,7 +440,7 @@ inline NodeStatus RosActionNode::tick() // Check if server is ready if(!action_client->action_server_is_ready()) { - return onFailure(SERVER_UNREACHABLE); + return onFailure(SERVER_UNREACHABLE, {}); } future_goal_handle_ = action_client->async_send_goal(goal, goal_options); @@ -459,7 +467,7 @@ inline NodeStatus RosActionNode::tick() { if((now() - time_goal_sent_) > timeout) { - return CheckStatus(onFailure(SEND_GOAL_TIMEOUT)); + return CheckStatus(onFailure(SEND_GOAL_TIMEOUT, {})); } else { @@ -490,11 +498,11 @@ inline NodeStatus RosActionNode::tick() { if(result_.code == rclcpp_action::ResultCode::ABORTED) { - return CheckStatus(onFailure(ACTION_ABORTED)); + return CheckStatus(onFailure(ACTION_ABORTED, result_)); } else if(result_.code == rclcpp_action::ResultCode::CANCELED) { - return CheckStatus(onFailure(ACTION_CANCELLED)); + return CheckStatus(onFailure(ACTION_CANCELLED, result_)); } else {