Change the way default port is passed, to allow namespaces

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;
+}