make it easier to change action/service name programmatically

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

btcpp_ros2_samples/src/bool_client.cpp

@@ -10,8 +10,11 @@ static const char* xml_text = R"(
  <root BTCPP_format="4">
      <BehaviorTree>
         <Sequence>
-            <RobotSetBool robot="robotA" command="true"/>
-            <RobotSetBool robot="robotB" command="true"/>
+
+            <SetBoolA value="true"/>
+            <SetBool  service_name="robotB/set_bool" value="true"/>
+            <SetRobotBool robot="robotA" value="true"/>
+            <SetRobotBool robot="robotB" value="true"/>
         </Sequence>
      </BehaviorTree>
  </root>
@@ -23,7 +26,16 @@ int main(int argc, char** argv)
   auto nh = std::make_shared<rclcpp::Node>("bool_client");
 
   BehaviorTreeFactory factory;
-  factory.registerNodeType<NamespacedSetBool>("RobotSetBool", "set_bool", nh);
+
+  // 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);
 

btcpp_ros2_samples/src/set_bool_node.cpp

@@ -12,12 +12,12 @@ BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& res
   std::cout << "onResponseReceived " << std::endl;
   if(response->success)
   {
-    RCLCPP_INFO(logger(), "SetString service succeeded.");
+    RCLCPP_INFO(logger(), "SetBool service succeeded.");
     return BT::NodeStatus::SUCCESS;
   }
   else
   {
-    RCLCPP_INFO(logger(), "SetString service failed: %s", response->message.c_str());
+    RCLCPP_INFO(logger(), "SetBool service failed: %s", response->message.c_str());
     return BT::NodeStatus::FAILURE;
   }
 }
@@ -28,54 +28,14 @@ BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error)
   return BT::NodeStatus::FAILURE;
 }
 
-//------------------------------------------------------
-//------------------------------------------------------
-//------------------------------------------------------
+//-----------------------------------------------------------
 
-NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
-                                     const BT::RosNodeParams& params)
-  : BT::ActionNodeBase(name, conf)
-  , local_bb_(BT::Blackboard::create(conf.blackboard))
-  , service_name_(params.default_port_value)
-{
-  BT::NodeConfig impl_config;
-  impl_config.blackboard = local_bb_;
-  impl_config.input_ports["service_name"] = "{=}";
-  impl_config.input_ports["value"] = "{=}";
-
-  BT::RosNodeParams impl_params = params;
-  impl_params.default_port_value = {};  // postpone this
-  set_bool_service_ = std::make_unique<SetBoolService>(name, impl_config, impl_params);
-}
-
-NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
-                                     const std::string& service_name,
-                                     rclcpp::Node::SharedPtr node)
-  : BT::ActionNodeBase(name, conf)
-  , local_bb_(BT::Blackboard::create(conf.blackboard))
-  , service_name_(service_name)
-{
-  BT::NodeConfig impl_config;
-  impl_config.blackboard = local_bb_;
-  impl_config.input_ports["service_name"] = "{=}";
-  impl_config.input_ports["value"] = "{=}";
-
-  BT::RosNodeParams impl_params;
-  impl_params.nh = node;
-  set_bool_service_ = std::make_unique<SetBoolService>(name, impl_config, impl_params);
-}
-
-BT::NodeStatus NamespacedSetBool::tick()
+BT::NodeStatus SetRobotBoolService::tick()
 {
   std::string robot;
-  bool command;
-  if(!getInput("robot", robot) || !getInput("command", command))
+  if(getInput("robot", robot) && !robot.empty())
   {
-    throw BT::RuntimeError("NamespacedSetBool: Missing inputs");
+    setServiceName(robot + "/" + service_suffix_);
   }
-
-  local_bb_->set("service_name", robot + "/" + service_name_);
-  local_bb_->set("value", command);
-  std::cout << "ticking " << std::endl;
-  return set_bool_service_->tick();
+  return SetBoolService::tick();
 }