fixes a bug in logger and enables linking to the logger

CMakeLists.txt

@@ -44,8 +44,8 @@ generate_messages(
 )
 
 catkin_package(
-    INCLUDE_DIRS include
-    LIBRARIES
+    INCLUDE_DIRS include include/behaviortree_ros
+    LIBRARIES behaviortree_ros
     CATKIN_DEPENDS ${ROS_DEPENDENCIES}
 )
 
@@ -56,8 +56,11 @@ include_directories( include ${catkin_INCLUDE_DIRS})
 
 #header only for the time being
 
-#add_library(behaviortree_ros ... )
-#target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+add_library(behaviortree_ros src/loggers/rosout_logger.cpp  src/loggers/ros_topic_logger.cpp src/recovery_node.cpp)
+target_link_libraries(behaviortree_ros ${catkin_LIBRARIES})
+target_compile_definitions(behaviortree_ros PRIVATE  BT_PLUGIN_EXPORT )
+
+add_dependencies(behaviortree_ros behaviortree_ros_gencpp)
 
 ######################################################
 # TESTS
@@ -73,8 +76,14 @@ target_link_libraries(test_server ${catkin_LIBRARIES} )
 ######################################################
 # INSTALL
 
-install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(TARGETS  behaviortree_ros
+ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install (
+        DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h")
 
 ######################################################
 # EXAMPLES and TOOLS

include/behaviortree_ros/bt_async_service_node.h

@@ -0,0 +1,179 @@
+// Copyright (c) 2019 Samsung Research America
+// Copyright (c) 2020 Davide Faconti
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_
+#define BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <ros/ros.h>
+#include <ros/service_client.h>
+
+namespace BT
+{
+
+/**
+ * Base Action to implement a ROS Service
+ */
+template<class ServiceT>
+class RosAsyncServiceNode : public BT::ActionNodeBase
+{
+protected:
+
+  RosAsyncServiceNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf):
+   BT::ActionNodeBase(name, conf), node_(nh) { 
+   }
+
+  RosAsyncServiceNode(ros::NodeHandle& nh,const std::string& service_name, ros::Duration timeout, const std::string& name, const BT::NodeConfiguration & conf):
+   BT::ActionNodeBase(name, conf), node_(nh) ,service_name_(service_name), timeout_(timeout) { 
+      std::cout << "RosAsyncServiceNode constructor" << std::endl;
+      std::cout << "service_name: " << service_name << std::endl;
+      service_client_ = node_.serviceClient<ServiceT>( service_name_ );
+   }
+
+public:
+
+  using BaseClass    = RosAsyncServiceNode<ServiceT>;
+  using ServiceType  = ServiceT;
+  using RequestType  = typename ServiceT::Request;
+  using ResponseType = typename ServiceT::Response;
+
+  RosAsyncServiceNode() = delete;
+
+  virtual ~RosAsyncServiceNode() = default;
+
+  /// These ports will be added automatically if this Node is
+  /// registered using RegisterRosAction<DeriveClass>()
+  static PortsList providedPorts()
+  {
+    return  {
+      InputPort<std::string>("service_name", "name of the ROS service"),
+      InputPort<unsigned>("timeout", 100, "timeout to connect to server (milliseconds)")
+      };
+  }
+
+  /// User must implement this method.
+  virtual void sendRequest(RequestType& request) = 0;
+
+  /// Method (to be implemented by the user) to receive the reply.
+  /// User can decide which NodeStatus it will return (SUCCESS or FAILURE).
+  virtual NodeStatus onResponse( const ResponseType& rep) = 0;
+
+  enum FailureCause{
+    MISSING_SERVER = 0,
+    FAILED_CALL = 1
+  };
+
+  /// Called when a service call failed. Can be overriden by the user.
+  virtual NodeStatus onFailedRequest(FailureCause failure)
+  {
+    return NodeStatus::FAILURE;
+  }
+
+protected:
+
+  ros::ServiceClient service_client_;
+  std::string service_name_;
+
+  RequestType request_;
+  ResponseType reply_;
+
+
+
+  // The node that will be used for any ROS operations
+  ros::NodeHandle& node_;
+
+  ros::Duration timeout_;
+
+  std::future< std::pair<bool,bool> > future_;
+
+  std::pair<bool, bool> call_service(RequestType &request, ros::Duration timeout){
+    bool connected = service_client_.waitForExistence(timeout);
+    if( !connected ){
+      return std::make_pair(false, false);
+    }
+    bool received = service_client_.call( request, reply_ );
+    return std::make_pair(true, received);
+
+  }
+
+  BT::NodeStatus tick() override
+  {
+    if( !service_client_.isValid() ){
+
+      service_client_ = node_.serviceClient<ServiceT>( service_name_ );
+    }
+
+    // first step to be done only at the beginning of the Action
+    if (status() == BT::NodeStatus::IDLE) {
+      // setting the status to RUNNING to notify the BT Loggers (if any)
+      setStatus(BT::NodeStatus::RUNNING);
+
+      sendRequest(request_);
+
+      future_ = std::async(std::launch::async, &RosAsyncServiceNode::call_service, this, std::ref(request_), timeout_);
+
+    }
+
+    if( future_.valid() ){
+      if (future_.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
+        return BT::NodeStatus::RUNNING;
+      }
+      auto result = future_.get();
+      if( !result.first ){
+        return onFailedRequest(MISSING_SERVER);
+      }
+      if( !result.second ){
+        return onFailedRequest(FAILED_CALL);
+      }
+      return onResponse(reply_);
+    }
+    throw BT::LogicError("future is not valid, this should never happen");
+    return BT::NodeStatus::RUNNING;
+  }
+
+  //halt just resets the future so that result will be ignored, and next tick will start a new future
+  virtual void halt() override{
+    future_ = std::future< std::pair<bool,bool> >();
+  }
+
+};
+
+
+/// Method to register the service into a factory.
+/// It gives you the opportunity to set the ros::NodeHandle.
+template <class DerivedT> static
+  void RegisterRosAsyncService(BT::BehaviorTreeFactory& factory,
+                     const std::string& registration_ID,
+                     ros::NodeHandle& node_handle, std::string service_name, ros::Duration timeout)
+{
+  NodeBuilder builder = [&node_handle, service_name , timeout](const std::string& name, const NodeConfiguration& config) {
+    return std::make_unique<DerivedT>(node_handle,  service_name, timeout, name, config );
+  };
+
+  TreeNodeManifest manifest;
+  manifest.type = getType<DerivedT>();
+  manifest.ports = DerivedT::providedPorts();
+  manifest.registration_ID = registration_ID;
+  //const auto& basic_ports = RosAsyncServiceNode< typename DerivedT::ServiceType>::providedPorts();
+  //manifest.ports.insert( basic_ports.begin(), basic_ports.end() );
+
+  factory.registerBuilder( manifest, builder );
+}
+
+
+}  // namespace BT
+
+#endif  // BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_

include/behaviortree_ros/loggers/ros_topic_logger.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <behaviortree_cpp_v3/loggers/abstract_logger.h>
+#include <ros/ros.h>
+#include <behaviortree_ros/StatusChangeLog.h>
+#include <behaviortree_ros/StatusChange.h>
+
+
+namespace BT
+{
+
+class RosTopicLogger : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    RosTopicLogger(TreeNode* root_node, ros::NodeHandle nh, const std::string topic_name = "behavior_tree_log");
+
+
+    ~RosTopicLogger() override;
+
+    virtual void callback(Duration timestamp,
+                          const TreeNode& node,
+                          NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+private:
+    ros::NodeHandle nh_;
+    std::string topic_name_;
+    ros::Publisher status_change_pub_;
+    std::vector<behaviortree_ros::StatusChange> event_log_;
+};
+
+}   // end namespace

include/behaviortree_ros/recovery_node.h

@@ -0,0 +1,79 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace behaviortree_ros
+{
+/**
+ * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
+ * returns SUCCESS.
+ *
+ * - If the first child returns FAILURE, the second child will be executed.  After that the first
+ * child is executed again if the second child returns SUCCESS.
+ *
+ * - If the first or second child returns RUNNING, this node returns RUNNING.
+ *
+ * - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
+ *
+ */
+class RecoveryNode : public BT::ControlNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::RecoveryNode
+   * @param name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  RecoveryNode(
+    const std::string & name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief A destructor for nav2_behavior_tree::RecoveryNode
+   */
+  ~RecoveryNode() override = default;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<int>("number_of_retries", 1, "Number of retries")
+    };
+  }
+
+private:
+  unsigned int current_child_idx_;
+  unsigned int number_of_retries_;
+  unsigned int retry_count_;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief The other (optional) override required by a BT action to reset node state
+   */
+  void halt() override;
+};
+
+}  // namespace behaviortree_ros