Change the way default port is passed, to allow namespaces

Author: facontidavide

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

@@ -108,8 +108,7 @@ class RosActionNode : public BT::ActionNodeBase
    */
   static PortsList providedBasicPorts(PortsList addition)
   {
-    PortsList basic = { InputPort<std::string>("action_name", "__default__placeholder__",
-                                               "Action server name") };
+    PortsList basic = { InputPort<std::string>("action_name", "", "Action server name") };
     basic.insert(addition.begin(), addition.end());
     return basic;
   }
@@ -164,9 +163,12 @@ class RosActionNode : public BT::ActionNodeBase
   void cancelGoal();
 
   /// The default halt() implementation will call cancelGoal if necessary.
-  void halt() override final;
+  void halt() override;
 
-  NodeStatus tick() override final;
+  NodeStatus tick() override;
+
+  /// Can be used to change the name of the action programmatically
+  void setActionName(const std::string& action_name);
 
 protected:
   struct ActionClientInstance
@@ -216,7 +218,7 @@ class RosActionNode : public BT::ActionNodeBase
   std::weak_ptr<rclcpp::Node> node_;
   std::shared_ptr<ActionClientInstance> client_instance_;
   std::string action_name_;
-  bool action_name_may_change_ = false;
+  bool action_name_should_be_checked_ = false;
   const std::chrono::milliseconds server_timeout_;
   const std::chrono::milliseconds wait_for_server_timeout_;
   std::string action_client_key_;
@@ -265,44 +267,23 @@ inline RosActionNode<T>::RosActionNode(const std::string& instance_name,
   auto portIt = config().input_ports.find("action_name");
   if(portIt != config().input_ports.end())
   {
-    const std::string& bb_action_name = portIt->second;
+    const std::string& bb_service_name = portIt->second;
 
-    if(bb_action_name.empty() || bb_action_name == "__default__placeholder__")
-    {
-      if(params.default_port_value.empty())
-      {
-        throw std::logic_error("Both [action_name] in the InputPort and the "
-                               "RosNodeParams are empty.");
-      }
-      else
-      {
-        createClient(params.default_port_value);
-      }
-    }
-    else if(!isBlackboardPointer(bb_action_name))
+    if(isBlackboardPointer(bb_service_name))
     {
-      // If the content of the port "action_name" is not
-      // a pointer to the blackboard, but a static string, we can
-      // create the client in the constructor.
-      createClient(bb_action_name);
+      // unknown value at construction time. Postpone to tick
+      action_name_should_be_checked_ = true;
     }
-    else
+    else if(!bb_service_name.empty())
     {
-      action_name_may_change_ = true;
-      // createClient will be invoked in the first tick().
+      // "hard-coded" name in the bb_service_name. Use it.
+      createClient(bb_service_name);
     }
   }
-  else
+  // no port value or it is empty. Use the default value
+  if(!client_instance_ && !params.default_port_value.empty())
   {
-    if(params.default_port_value.empty())
-    {
-      throw std::logic_error("Both [action_name] in the InputPort and the RosNodeParams "
-                             "are empty.");
-    }
-    else
-    {
-      createClient(params.default_port_value);
-    }
+    createClient(params.default_port_value);
   }
 }
 
@@ -347,6 +328,13 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
   return found;
 }
 
+template <class T>
+inline void RosActionNode<T>::setActionName(const std::string& action_name)
+{
+  action_name_ = action_name;
+  createClient(action_name);
+}
+
 template <class T>
 inline NodeStatus RosActionNode<T>::tick()
 {
@@ -359,7 +347,8 @@ inline NodeStatus RosActionNode<T>::tick()
   // First, check if the action_client_ is valid and that the name of the
   // action_name in the port didn't change.
   // otherwise, create a new client
-  if(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_))
+  if(!client_instance_ ||
+     (status() == NodeStatus::IDLE && action_name_should_be_checked_))
   {
     std::string action_name;
     getInput("action_name", action_name);
@@ -368,6 +357,13 @@ inline NodeStatus RosActionNode<T>::tick()
       createClient(action_name);
     }
   }
+
+  if(!client_instance_)
+  {
+    throw BT::RuntimeError("RosActionNode: no client was specified neither as default or "
+                           "in the ports");
+  }
+
   auto& action_client = client_instance_->action_client;
 
   //------------------------------------------

behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp

@@ -96,8 +96,7 @@ class RosServiceNode : public BT::ActionNodeBase
    */
   static PortsList providedBasicPorts(PortsList addition)
   {
-    PortsList basic = { InputPort<std::string>("service_name", "__default__placeholder__",
-                                               "Service name") };
+    PortsList basic = { InputPort<std::string>("service_name", "", "Service name") };
     basic.insert(addition.begin(), addition.end());
     return basic;
   }
@@ -111,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase
     return providedBasicPorts({});
   }
 
-  NodeStatus tick() override final;
+  NodeStatus tick() override;
 
   /// The default halt() implementation.
   void halt() override;
@@ -157,6 +156,9 @@ class RosServiceNode : public BT::ActionNodeBase
     return action_client_mutex;
   }
 
+  // method to set the service name programmatically
+  void setServiceName(const std::string& service_name);
+
   rclcpp::Logger logger()
   {
     if(auto node = node_.lock())
@@ -186,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase
 
   std::weak_ptr<rclcpp::Node> node_;
   std::string service_name_;
-  bool service_name_may_change_ = false;
+  bool service_name_should_be_checked_ = false;
   const std::chrono::milliseconds service_timeout_;
   const std::chrono::milliseconds wait_for_service_timeout_;
 
@@ -233,51 +235,30 @@ inline RosServiceNode<T>::RosServiceNode(const std::string& instance_name,
   {
     const std::string& bb_service_name = portIt->second;
 
-    if(bb_service_name.empty() || bb_service_name == "__default__placeholder__")
+    if(isBlackboardPointer(bb_service_name))
     {
-      if(params.default_port_value.empty())
-      {
-        throw std::logic_error("Both [service_name] in the InputPort and the "
-                               "RosNodeParams are empty.");
-      }
-      else
-      {
-        createClient(params.default_port_value);
-      }
+      // unknown value at construction time. postpone to tick
+      service_name_should_be_checked_ = true;
     }
-    else if(!isBlackboardPointer(bb_service_name))
+    else if(!bb_service_name.empty())
     {
-      // If the content of the port "service_name" is not
-      // a pointer to the blackboard, but a static string, we can
-      // create the client in the constructor.
+      // "hard-coded" name in the bb_service_name. Use it.
       createClient(bb_service_name);
     }
-    else
-    {
-      service_name_may_change_ = true;
-      // createClient will be invoked in the first tick().
-    }
   }
-  else
+  // no port value or it is empty. Use the default port value
+  if(!srv_instance_ && !params.default_port_value.empty())
   {
-    if(params.default_port_value.empty())
-    {
-      throw std::logic_error("Both [service_name] in the InputPort and the RosNodeParams "
-                             "are empty.");
-    }
-    else
-    {
-      createClient(params.default_port_value);
-    }
+    createClient(params.default_port_value);
   }
 }
 
 template <class T>
 inline bool RosServiceNode<T>::createClient(const std::string& service_name)
 {
-  if(service_name.empty())
+  if(service_name.empty() || service_name == "__default__placeholder__")
   {
-    throw RuntimeError("service_name is empty");
+    throw RuntimeError("service_name is empty or invalid");
   }
 
   std::unique_lock lk(getMutex());
@@ -314,6 +295,13 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
   return found;
 }
 
+template <class T>
+inline void RosServiceNode<T>::setServiceName(const std::string& service_name)
+{
+  service_name_ = service_name;
+  createClient(service_name);
+}
+
 template <class T>
 inline NodeStatus RosServiceNode<T>::tick()
 {
@@ -326,7 +314,7 @@ inline NodeStatus RosServiceNode<T>::tick()
   // First, check if the service_client is valid and that the name of the
   // service_name in the port didn't change.
   // otherwise, create a new client
-  if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_))
+  if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_should_be_checked_))
   {
     std::string service_name;
     getInput("service_name", service_name);
@@ -336,6 +324,12 @@ inline NodeStatus RosServiceNode<T>::tick()
     }
   }
 
+  if(!srv_instance_)
+  {
+    throw BT::RuntimeError("RosServiceNode: no service client was specified neither as "
+                           "default or in the ports");
+  }
+
   auto CheckStatus = [](NodeStatus status) {
     if(!isStatusCompleted(status))
     {

behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp

@@ -40,6 +40,13 @@ struct RosNodeParams
   std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000);
   // timeout used when detecting the server the first time
   std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500);
+
+  RosNodeParams() = default;
+  RosNodeParams(std::shared_ptr<rclcpp::Node> node) : nh(node)
+  {}
+  RosNodeParams(std::shared_ptr<rclcpp::Node> node, const std::string& port_name)
+    : nh(node), default_port_value(port_name)
+  {}
 };
 
 }  // namespace BT

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -76,6 +76,15 @@ class TreeExecutionServer
   BT::BehaviorTreeFactory& factory();
 
 protected:
+  /**
+   * @brief Callback invoked when a goal is received and before the tree is created.
+   * If it returns false, the goal will be rejected.
+  */
+  virtual bool onGoalReceived(const std::string& tree_name, const std::string& payload)
+  {
+    return true;
+  }
+
   /**
    * @brief Callback invoked after the tree is created.
    * It can be used, for instance, to initialize a logger or the global blackboard.

behaviortree_ros2/src/tree_execution_server.cpp

@@ -121,6 +121,11 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
 {
   RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s",
               goal->target_tree.c_str());
+
+  if(!onGoalReceived(goal->target_tree, goal->payload))
+  {
+    return rclcpp_action::GoalResponse::REJECT;
+  }
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 

btcpp_ros2_samples/CMakeLists.txt

@@ -9,10 +9,12 @@ find_package(ament_cmake REQUIRED)
 find_package(behaviortree_ros2 REQUIRED)
 find_package(btcpp_ros2_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 
 set(THIS_PACKAGE_DEPS
     behaviortree_ros2
     std_msgs
+    std_srvs
     btcpp_ros2_interfaces )
 
 ######################################################
@@ -50,6 +52,14 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS})
 add_executable(subscriber_test src/subscriber_test.cpp)
 ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS})
 
+######################################################
+# the SetBool test
+add_executable(bool_client src/bool_client.cpp src/set_bool_node.cpp)
+ament_target_dependencies(bool_client ${THIS_PACKAGE_DEPS})
+
+add_executable(bool_server src/bool_server.cpp )
+ament_target_dependencies(bool_server ${THIS_PACKAGE_DEPS})
+
 ######################################################
 # INSTALL
 
@@ -60,6 +70,8 @@ install(TARGETS
   sleep_plugin
   subscriber_test
   sample_bt_executor
+  bool_client
+  bool_server
   DESTINATION lib/${PROJECT_NAME}
   )
 

btcpp_ros2_samples/src/bool_client.cpp

@@ -0,0 +1,45 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+#include "set_bool_node.hpp"
+
+using namespace BT;
+
+static const char* xml_text = R"(
+ <root BTCPP_format="4">
+     <BehaviorTree>
+        <Sequence>
+
+            <SetBoolA value="true"/>
+            <SetBool  service_name="robotB/set_bool" value="true"/>
+            <SetRobotBool robot="robotA" value="true"/>
+            <SetRobotBool robot="robotB" value="true"/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+ )";
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  auto nh = std::make_shared<rclcpp::Node>("bool_client");
+
+  BehaviorTreeFactory factory;
+
+  // version with default port
+  factory.registerNodeType<SetBoolService>("SetBoolA", BT::RosNodeParams(nh, "robotA/"
+                                                                             "set_bool"));
+
+  // version without default port
+  factory.registerNodeType<SetBoolService>("SetBool", BT::RosNodeParams(nh));
+
+  // namespace version
+  factory.registerNodeType<SetRobotBoolService>("SetRobotBool", nh, "set_bool");
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  tree.tickWhileRunning();
+
+  return 0;
+}