set bool examples

Author: facontidavide

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,33 @@
+#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>
+            <RobotSetBool robot="robotA" command="true"/>
+            <RobotSetBool robot="robotB" command="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;
+  factory.registerNodeType<NamespacedSetBool>("RobotSetBool", "set_bool", nh);
+
+  auto tree = factory.createTreeFromText(xml_text);
+
+  tree.tickWhileRunning();
+
+  return 0;
+}

btcpp_ros2_samples/src/bool_server.cpp

@@ -0,0 +1,37 @@
+#include "rclcpp/rclcpp.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+#include <memory>
+
+void CallbackA(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+               std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+{
+  response->success = request->data;
+  auto logger = rclcpp::get_logger("rclcpp");
+  RCLCPP_INFO(logger, "Incoming request A: %s", request->data ? "true" : "false");
+}
+
+void CallbackB(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+               std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+{
+  response->success = request->data;
+  auto logger = rclcpp::get_logger("rclcpp");
+  RCLCPP_INFO(logger, "Incoming request B: %s", request->data ? "true" : "false");
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");
+
+  auto serviceA =
+      node->create_service<std_srvs::srv::SetBool>("robotA/set_bool", &CallbackA);
+  auto serviceB =
+      node->create_service<std_srvs::srv::SetBool>("robotB/set_bool", &CallbackB);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready.");
+
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+}

btcpp_ros2_samples/src/set_bool_node.cpp

@@ -0,0 +1,64 @@
+#include "set_bool_node.hpp"
+
+bool SetBoolService::setRequest(Request::SharedPtr& request)
+{
+  getInput("value", request->data);
+  std::cout << "setRequest " << std::endl;
+  return true;
+}
+
+BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& response)
+{
+  std::cout << "onResponseReceived " << std::endl;
+  if(response->success)
+  {
+    RCLCPP_INFO(logger(), "SetString service succeeded.");
+    return BT::NodeStatus::SUCCESS;
+  }
+  else
+  {
+    RCLCPP_INFO(logger(), "SetString service failed: %s", response->message.c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
+BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error)
+{
+  RCLCPP_ERROR(logger(), "Error: %d", error);
+  return BT::NodeStatus::FAILURE;
+}
+
+//------------------------------------------------------
+//------------------------------------------------------
+//------------------------------------------------------
+
+NamespacedSetBool::NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
+                                     const std::string& service_name,
+                                     std::weak_ptr<rclcpp::Node> 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()
+{
+  std::string robot;
+  bool command;
+  if(!getInput("robot", robot) || !getInput("command", command))
+  {
+    throw BT::RuntimeError("NamespacedSetBool: Missing inputs");
+  }
+
+  local_bb_->set("service_name", robot + "/" + service_name_);
+  local_bb_->set("value", command);
+  std::cout << "ticking " << std::endl;
+  return set_bool_service_->tick();
+}

btcpp_ros2_samples/src/set_bool_node.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <behaviortree_ros2/bt_service_node.hpp>
+#include "std_srvs/srv/set_bool.hpp"
+
+using SetBool = std_srvs::srv::SetBool;
+
+class SetBoolService : public BT::RosServiceNode<SetBool>
+{
+public:
+  SetBoolService(const std::string& name, const BT::NodeConfig& conf,
+                 const BT::RosNodeParams& params)
+    : RosServiceNode<SetBool>(name, conf, params)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({ BT::InputPort<bool>("value") });
+  }
+
+  bool setRequest(Request::SharedPtr& request) override;
+
+  BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override;
+
+  virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override;
+};
+
+//------------------------------------------------------
+
+class NamespacedSetBool : public BT::ActionNodeBase
+{
+public:
+  NamespacedSetBool(const std::string& name, const BT::NodeConfig& conf,
+                    const std::string& service_name, std::weak_ptr<rclcpp::Node> node);
+
+  static BT::PortsList providedPorts()
+  {
+    return { BT::InputPort<std::string>("robot"), BT::InputPort<bool>("command") };
+  }
+
+  BT::NodeStatus tick() override;
+
+  void halt() override
+  {
+    set_bool_service_->halt();
+  }
+
+private:
+  BT::Blackboard::Ptr local_bb_;
+  std::string service_name_;
+  std::unique_ptr<SetBoolService> set_bool_service_;
+};