onHalt added and bug fixed

Author: davide

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

@@ -38,6 +38,20 @@ enum ActionNodeErrorCode
   INVALID_GOAL
 };
 
+inline const char* toStr(const ActionNodeErrorCode& err)
+{
+  switch (err)
+  {
+    case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE";
+    case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT";
+    case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER";
+    case ACTION_ABORTED: return "ACTION_ABORTED";
+    case ACTION_CANCELLED: return "ACTION_CANCELLED";
+    case INVALID_GOAL: return "INVALID_GOAL";
+  }
+  return nullptr;
+}
+
 /**
  * @brief Abstract class to wrap rclcpp_action::Client<>
  *
@@ -106,10 +120,9 @@ class RosActionNode : public BT::ActionNodeBase
     return providedBasicPorts({});
   }
 
-  NodeStatus tick() override final;
-
-  /// The default halt() implementation will call cancelGoal if necessary.
-  void halt() override;
+  /// @brief  Callback executed when the node is halted. Note that cancelGoal()
+  /// is done automatically.
+  virtual void onHalt() {}
 
   /** setGoal s a callback that allows the user to set
    *  the goal message (ActionT::Goal).
@@ -146,6 +159,12 @@ class RosActionNode : public BT::ActionNodeBase
   /// Method used to send a request to the Action server to cancel the current goal
   void cancelGoal();
 
+
+  /// The default halt() implementation will call cancelGoal if necessary.
+  void halt() override final;
+
+  NodeStatus tick() override final;
+
 protected:
 
   std::shared_ptr<rclcpp::Node> node_;
@@ -311,9 +330,11 @@ template<class T> inline
     goal_options.result_callback =
       [this](const WrappedResult& result)
     {
-      RCLCPP_DEBUG( node_->get_logger(), "result_callback" );
-      result_ = result;
-      emitWakeUpSignal();
+      if (goal_handle_->get_goal_id() == result.goal_id) {
+        RCLCPP_DEBUG( node_->get_logger(), "result_callback" );
+        result_ = result;
+        emitWakeUpSignal();
+      }
     };
     //--------------------
     goal_options.goal_response_callback =
@@ -324,7 +345,7 @@ template<class T> inline
       {
         RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
       } else {
-        RCLCPP_INFO(node_->get_logger(), "Goal accepted by server, waiting for result");
+        RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result");
       }
     };
     //--------------------
@@ -396,9 +417,10 @@ template<class T> inline
 template<class T> inline
   void RosActionNode<T>::halt()
 {
-  if( status() == NodeStatus::RUNNING )
+  if(status() == BT::NodeStatus::RUNNING)
   {
     cancelGoal();
+    onHalt();
   }
 }
 

behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp

@@ -34,6 +34,18 @@ enum ServiceNodeErrorCode
   SERVICE_ABORTED
 };
 
+inline const char* toStr(const ServiceNodeErrorCode& err)
+{
+  switch (err)
+  {
+    case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE";
+    case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT";
+    case INVALID_REQUEST: return "INVALID_REQUEST";
+    case SERVICE_ABORTED: return "SERVICE_ABORTED";
+  }
+  return nullptr;
+}
+
 /**
  * @brief Abstract class use to wrap rclcpp::Client<>
  *

btcpp_ros2_samples/src/sleep_action.cpp

@@ -10,16 +10,23 @@ bool SleepAction::setGoal(RosActionNode::Goal &goal)
 
 NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult &wr)
 {
-  RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived %d", name().c_str(), wr.result->done );
+  RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), 
+               wr.result->done ? "true" : "false" );
+
   return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
 }
 
 NodeStatus SleepAction::onFailure(ActionNodeErrorCode error)
 {
-  RCLCPP_ERROR( node_->get_logger(), "%s: onFailure %d", name().c_str(), error );
+  RCLCPP_ERROR( node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error) );
   return NodeStatus::FAILURE;
 }
 
+void SleepAction::onHalt()
+{
+  RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() );
+}
+
 // Plugin registration.
 // The class SleepAction will self register with name  "Sleep".
 CreateRosNodePlugin(SleepAction, "Sleep");

btcpp_ros2_samples/src/sleep_action.hpp

@@ -19,6 +19,8 @@ class SleepAction: public RosActionNode<btcpp_ros2_interfaces::action::Sleep>
 
   bool setGoal(Goal& goal) override;
 
+  void onHalt() override;
+
   BT::NodeStatus onResultReceived(const WrappedResult& wr) override;
 
   virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override;

btcpp_ros2_samples/src/sleep_client.cpp

@@ -79,7 +79,9 @@ int main(int argc, char **argv)
 
   auto tree = factory.createTreeFromText(xml_text);
 
-  tree.tickWhileRunning();
+  for(int i=0; i<5; i++){
+    tree.tickWhileRunning();
+  }
 
   return 0;
 }