diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index a8c21bb..fad73fc 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -81,7 +81,8 @@ class RosActionNode : public BT::ActionNodeBase enum FailureCause{ MISSING_SERVER = 0, ABORTED_BY_SERVER = 1, - REJECTED_BY_SERVER = 2 + REJECTED_BY_SERVER = 2, + OTHER }; /// Called when a service call failed. Can be overriden by the user. @@ -98,8 +99,15 @@ class RosActionNode : public BT::ActionNodeBase { if( status() == NodeStatus::RUNNING ) { - action_client_->cancelGoal(); + ros::spinOnce(); + auto action_state = action_client_->getState(); + + if( !action_state.isDone() ) + { + action_client_->cancelGoal(); + } } + setStatus(NodeStatus::IDLE); } @@ -134,12 +142,12 @@ class RosActionNode : public BT::ActionNodeBase } // RUNNING + ros::spinOnce(); auto action_state = action_client_->getState(); // Please refer to these states - if( action_state == actionlib::SimpleClientGoalState::PENDING || - action_state == actionlib::SimpleClientGoalState::ACTIVE ) + if( !action_state.isDone() ) { return NodeStatus::RUNNING; } @@ -158,8 +166,11 @@ class RosActionNode : public BT::ActionNodeBase else { // FIXME: is there any other valid state we should consider? - throw std::logic_error("Unexpected state in RosActionNode::tick()"); + ROS_ERROR("Unexpected state in RosActionNode::tick(): %s", action_state.toString().c_str()); + return onFailedRequest( OTHER ); } + // this code is unreachable. Adding this just to avoid warnings + return status(); } }; diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 960f68b..7b5070d 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -166,12 +166,12 @@ RosActionNode(handle, name, conf) {} sum = "{add_two_result}" /> - + - + diff --git a/test/test_server.cpp b/test/test_server.cpp index 2a0d5af..226971a 100644 --- a/test/test_server.cpp +++ b/test/test_server.cpp @@ -2,6 +2,7 @@ #include #include #include +#include bool Add(behaviortree_ros::AddTwoInts::Request &req, behaviortree_ros::AddTwoInts::Response &res)