From 006bded41986e2747cdcc3616b342eee4d799d4c Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Thu, 10 Aug 2023 18:49:33 +0200 Subject: [PATCH 1/8] Migrate to v4 --- CMakeLists.txt | 4 ++-- include/behaviortree_ros/bt_action_node.h | 4 ++-- include/behaviortree_ros/bt_param_node.h | 4 ++-- include/behaviortree_ros/bt_service_node.h | 4 ++-- include/behaviortree_ros/bt_topic_pub_node.h | 4 ++-- include/behaviortree_ros/bt_topic_sub_node.h | 4 ++-- include/behaviortree_ros/loggers/rosout_logger.h | 2 +- package.xml | 6 +++--- test/test_bt.cpp | 6 +++--- 9 files changed, 19 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d3e24e..dd2510a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5.1) # version on Ubuntu Trusty project(behaviortree_ros) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) @@ -9,7 +9,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(ROS_DEPENDENCIES roscpp std_msgs - behaviortree_cpp_v3 + behaviortree_cpp actionlib_msgs actionlib message_generation) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index a8c21bb..fb6ab0e 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -16,8 +16,8 @@ #ifndef BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ #define BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ -#include -#include +#include +#include #include #include diff --git a/include/behaviortree_ros/bt_param_node.h b/include/behaviortree_ros/bt_param_node.h index bcaed07..1214971 100644 --- a/include/behaviortree_ros/bt_param_node.h +++ b/include/behaviortree_ros/bt_param_node.h @@ -16,8 +16,8 @@ #define BEHAVIOR_TREE_BT_PARAM_NODE_HPP_ #include -#include -#include +#include +#include namespace BT { diff --git a/include/behaviortree_ros/bt_service_node.h b/include/behaviortree_ros/bt_service_node.h index a60eff6..e3120f2 100644 --- a/include/behaviortree_ros/bt_service_node.h +++ b/include/behaviortree_ros/bt_service_node.h @@ -16,8 +16,8 @@ #ifndef BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_ #define BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_ -#include -#include +#include +#include #include #include diff --git a/include/behaviortree_ros/bt_topic_pub_node.h b/include/behaviortree_ros/bt_topic_pub_node.h index 7abeab1..58348df 100644 --- a/include/behaviortree_ros/bt_topic_pub_node.h +++ b/include/behaviortree_ros/bt_topic_pub_node.h @@ -16,8 +16,8 @@ #define BEHAVIOR_TREE_BT_TOPIC_PUBLISHER_NODE_HPP_ #include -#include -#include +#include +#include namespace BT { diff --git a/include/behaviortree_ros/bt_topic_sub_node.h b/include/behaviortree_ros/bt_topic_sub_node.h index 363bad2..e72243c 100644 --- a/include/behaviortree_ros/bt_topic_sub_node.h +++ b/include/behaviortree_ros/bt_topic_sub_node.h @@ -16,8 +16,8 @@ #define BEHAVIOR_TREE_BT_TOPIC_SUBSCRIBER_NODE_HPP_ #include -#include -#include +#include +#include namespace BT { diff --git a/include/behaviortree_ros/loggers/rosout_logger.h b/include/behaviortree_ros/loggers/rosout_logger.h index f79cd2b..47b21e6 100644 --- a/include/behaviortree_ros/loggers/rosout_logger.h +++ b/include/behaviortree_ros/loggers/rosout_logger.h @@ -1,7 +1,7 @@ #ifndef BT_ROSOUT_LOGGER_H #define BT_ROSOUT_LOGGER_H -#include +#include #include namespace BT diff --git a/package.xml b/package.xml index e97b070..70de677 100644 --- a/package.xml +++ b/package.xml @@ -13,13 +13,13 @@ roscpp std_msgs - behaviortree_cpp_v3 + behaviortree_cpp actionlib actionlib_msgs - + roscpp std_msgs - behaviortree_cpp_v3 + behaviortree_cpp actionlib actionlib_msgs diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 7b5070d..e00e582 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -158,8 +158,8 @@ RosActionNode(handle, name, conf) {} // Simple tree, used to execute once each action. static const char* xml_text = R"( - - + + Date: Fri, 11 Aug 2023 11:38:57 +0200 Subject: [PATCH 2/8] Add service server node --- ...ervice_node.h => bt_service_client_node.h} | 36 ++++---- .../behaviortree_ros/bt_service_server_node.h | 83 +++++++++++++++++++ 2 files changed, 102 insertions(+), 17 deletions(-) rename include/behaviortree_ros/{bt_service_node.h => bt_service_client_node.h} (79%) create mode 100644 include/behaviortree_ros/bt_service_server_node.h diff --git a/include/behaviortree_ros/bt_service_node.h b/include/behaviortree_ros/bt_service_client_node.h similarity index 79% rename from include/behaviortree_ros/bt_service_node.h rename to include/behaviortree_ros/bt_service_client_node.h index e3120f2..ec60436 100644 --- a/include/behaviortree_ros/bt_service_node.h +++ b/include/behaviortree_ros/bt_service_client_node.h @@ -13,8 +13,8 @@ // 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_ +#ifndef BEHAVIOR_TREE_BT_SERVICE_CLIENT_NODE_HPP_ +#define BEHAVIOR_TREE_BT_SERVICE_CLIENT_NODE_HPP_ #include #include @@ -25,26 +25,27 @@ namespace BT { /** - * Base Action to implement a ROS Service + * Base Action to implement a ROS Service Client */ template -class RosServiceNode : public BT::SyncActionNode +class RosServiceClientNode : public BT::SyncActionNode { protected: - - RosServiceNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf): - BT::SyncActionNode(name, conf), node_(nh) { } + RosServiceClientNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf) + : BT::SyncActionNode(name, conf) + , node_(nh) + { + } public: - - using BaseClass = RosServiceNode; + using BaseClass = RosServiceClientNode; using ServiceType = ServiceT; using RequestType = typename ServiceT::Request; using ResponseType = typename ServiceT::Response; - RosServiceNode() = delete; + RosServiceClientNode() = delete; - virtual ~RosServiceNode() = default; + virtual ~RosServiceClientNode() = default; /// These ports will be added automatically if this Node is /// registered using RegisterRosAction() @@ -113,10 +114,11 @@ class RosServiceNode : public BT::SyncActionNode /// Method to register the service into a factory. /// It gives you the opportunity to set the ros::NodeHandle. -template static - void RegisterRosService(BT::BehaviorTreeFactory& factory, - const std::string& registration_ID, - ros::NodeHandle& node_handle) +template +static void +RegisterRosServiceClient(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + ros::NodeHandle& node_handle) { NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { return std::make_unique(node_handle, name, config ); @@ -126,7 +128,7 @@ template static manifest.type = getType(); manifest.ports = DerivedT::providedPorts(); manifest.registration_ID = registration_ID; - const auto& basic_ports = RosServiceNode< typename DerivedT::ServiceType>::providedPorts(); + const auto& basic_ports = RosServiceClientNode::providedPorts(); manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); factory.registerBuilder( manifest, builder ); @@ -135,4 +137,4 @@ template static } // namespace BT -#endif // BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_ +#endif // BEHAVIOR_TREE_BT_SERVICE_CLIENT_NODE_HPP_ diff --git a/include/behaviortree_ros/bt_service_server_node.h b/include/behaviortree_ros/bt_service_server_node.h new file mode 100644 index 0000000..7d0400b --- /dev/null +++ b/include/behaviortree_ros/bt_service_server_node.h @@ -0,0 +1,83 @@ +/* + * Enway GmbH - All Rights reserved. + * Proprietary & confidential. + */ + +#ifndef BEHAVIOR_TREE_BT_SERVICE_SERVER_NODE_HPP_ +#define BEHAVIOR_TREE_BT_SERVICE_SERVER_NODE_HPP_ + +#include +#include +#include + +namespace BT +{ + +template +class RosServiceServerNode : public BT::SyncActionNode +{ +protected: + ros::NodeHandle node_; + ros::ServiceServer service_server_; + + RosServiceServerNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration& conf) + : BT::SyncActionNode(name, conf) + , node_(nh) + { + if (service_server_ == nullptr) + { + std::string service_name = getInput("service_name").value(); + service_server_ = node_.advertiseService(service_name, &RosServiceServerNode::serviceCallback, this); + } + } + + BT::NodeStatus node_status = BT::NodeStatus::FAILURE; + BT::NodeStatus + tick() override + { + return node_status; + } + +public: + using BaseClass = RosServiceServerNode; + using ServiceType = ServiceT; + using RequestType = typename ServiceT::Request; + using ResponseType = typename ServiceT::Response; + + RosServiceServerNode() = delete; + virtual ~RosServiceServerNode() = default; + + static PortsList + providedPorts() + { + return {InputPort("service_name", "name of the ROS service"), + InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + } + + virtual bool serviceCallback(RequestType& request, ResponseType& response) = 0; +}; + +template +static void +RegisterRosServiceServer(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + ros::NodeHandle& node_handle) +{ + NodeBuilder builder = [&node_handle](const std::string& name, const BT::NodeConfiguration& config) + { + return std::make_unique(node_handle, name, config); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = RosServiceServerNode::providedPorts(); + manifest.ports.insert(basic_ports.begin(), basic_ports.end()); + + factory.registerBuilder(manifest, builder); +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_BT_SERVICE_SERVER_NODE_HPP_ From c46d051e9364630bd55ea5a4f1e7e954c6262e82 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Mon, 14 Aug 2023 15:51:00 +0200 Subject: [PATCH 3/8] Make nodes loadable by plugin --- include/behaviortree_ros/bt_action_node.h | 19 +++-- include/behaviortree_ros/bt_param_node.h | 15 ++-- .../behaviortree_ros/bt_service_client_node.h | 19 +++-- .../behaviortree_ros/bt_service_server_node.h | 12 +++- include/behaviortree_ros/bt_topic_pub_node.h | 17 +++-- include/behaviortree_ros/bt_topic_sub_node.h | 18 +++-- .../behaviortree_ros/register_ros_plugin.h | 69 +++++++++++++++++++ 7 files changed, 137 insertions(+), 32 deletions(-) create mode 100644 include/behaviortree_ros/register_ros_plugin.h diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index fb6ab0e..fb88383 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -59,14 +59,19 @@ class RosActionNode : public BT::ActionNodeBase virtual ~RosActionNode() = default; - /// These ports will be added automatically if this Node is - /// registered using RegisterRosAction() - static PortsList providedPorts() + static PortsList + providedBasicPorts(PortsList addition) { - return { - InputPort("server_name", "name of the Action Server"), - InputPort("timeout", 500, "timeout to connect (milliseconds)") - }; + PortsList basic = {InputPort("server_name", "name of the Action Server"), + InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + static PortsList + providedPorts() + { + return providedBasicPorts({}); } /// Method called when the Action makes a transition from IDLE to RUNNING. diff --git a/include/behaviortree_ros/bt_param_node.h b/include/behaviortree_ros/bt_param_node.h index 1214971..e24d25d 100644 --- a/include/behaviortree_ros/bt_param_node.h +++ b/include/behaviortree_ros/bt_param_node.h @@ -40,14 +40,21 @@ class RosParamNode : public BT::SyncActionNode { RosParamNode() = delete; virtual ~RosParamNode() = default; - static PortsList providedPorts() + static PortsList + providedBasicPorts(PortsList addition) { - return - { - InputPort("param_name", "name of the parameter on the Parameter Server"), + PortsList basic = { + InputPort("param_name", "name of the parameter on the Parameter Server"), }; + basic.insert(addition.begin(), addition.end()); + return basic; } + static PortsList + providedPorts() + { + return providedBasicPorts({}); + } }; diff --git a/include/behaviortree_ros/bt_service_client_node.h b/include/behaviortree_ros/bt_service_client_node.h index ec60436..cee2703 100644 --- a/include/behaviortree_ros/bt_service_client_node.h +++ b/include/behaviortree_ros/bt_service_client_node.h @@ -47,14 +47,19 @@ class RosServiceClientNode : public BT::SyncActionNode virtual ~RosServiceClientNode() = default; - /// These ports will be added automatically if this Node is - /// registered using RegisterRosAction() - static PortsList providedPorts() + static PortsList + providedBasicPorts(PortsList addition) { - return { - InputPort("service_name", "name of the ROS service"), - InputPort("timeout", 100, "timeout to connect to server (milliseconds)") - }; + PortsList basic = {InputPort("service_name", "name of the ROS service"), + InputPort("timeout", 100, "timeout to connect to server (milliseconds)")}; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + static PortsList + providedPorts() + { + return providedBasicPorts({}); } /// User must implement this method. diff --git a/include/behaviortree_ros/bt_service_server_node.h b/include/behaviortree_ros/bt_service_server_node.h index 7d0400b..fcc5460 100644 --- a/include/behaviortree_ros/bt_service_server_node.h +++ b/include/behaviortree_ros/bt_service_server_node.h @@ -47,11 +47,19 @@ class RosServiceServerNode : public BT::SyncActionNode RosServiceServerNode() = delete; virtual ~RosServiceServerNode() = default; + static PortsList + providedBasicPorts(PortsList addition) + { + PortsList basic = {InputPort("service_name", "name of the ROS service"), + InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + basic.insert(addition.begin(), addition.end()); + return basic; + } + static PortsList providedPorts() { - return {InputPort("service_name", "name of the ROS service"), - InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + return providedBasicPorts({}); } virtual bool serviceCallback(RequestType& request, ResponseType& response) = 0; diff --git a/include/behaviortree_ros/bt_topic_pub_node.h b/include/behaviortree_ros/bt_topic_pub_node.h index 58348df..8a56c31 100644 --- a/include/behaviortree_ros/bt_topic_pub_node.h +++ b/include/behaviortree_ros/bt_topic_pub_node.h @@ -50,15 +50,20 @@ class RosTopicPublisherNode : public BT::SyncActionNode { RosTopicPublisherNode() = delete; virtual ~RosTopicPublisherNode() = default; - static PortsList providedPorts() + static PortsList + providedBasicPorts(PortsList addition) { - return - { - InputPort("topic_name", "name of the ROS Topic"), - InputPort("timeout", 500, "timeout to connect (milliseconds)") - }; + PortsList basic = {InputPort("topic_name", "name of the ROS Topic"), + InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + basic.insert(addition.begin(), addition.end()); + return basic; } + static PortsList + providedPorts() + { + return providedBasicPorts({}); + } }; diff --git a/include/behaviortree_ros/bt_topic_sub_node.h b/include/behaviortree_ros/bt_topic_sub_node.h index e72243c..9c2087c 100644 --- a/include/behaviortree_ros/bt_topic_sub_node.h +++ b/include/behaviortree_ros/bt_topic_sub_node.h @@ -54,13 +54,19 @@ class RosTopicSubscriberNode : public BT::SyncActionNode { RosTopicSubscriberNode() = delete; virtual ~RosTopicSubscriberNode() = default; - static PortsList providedPorts() + static PortsList + providedBasicPorts(PortsList addition) { - return - { - InputPort("topic_name", "name of the ROS Topic"), - InputPort("timeout", 500, "timeout to connect (milliseconds)") - }; + PortsList basic = {InputPort("topic_name", "name of the ROS Topic"), + InputPort("timeout", 500, "timeout to connect (milliseconds)")}; + basic.insert(addition.begin(), addition.end()); + return basic; + } + + static PortsList + providedPorts() + { + return providedBasicPorts({}); } virtual void topicCallback(const typename TopicType::ConstPtr &message) = 0; diff --git a/include/behaviortree_ros/register_ros_plugin.h b/include/behaviortree_ros/register_ros_plugin.h new file mode 100644 index 0000000..2a5e498 --- /dev/null +++ b/include/behaviortree_ros/register_ros_plugin.h @@ -0,0 +1,69 @@ +// Copyright (c) 2023 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. + +#pragma once + +#include + +#include +#include + +#include + +// Use this macro to generate a plugin for: +// +// - BT::RosActionNode +// - BT::RosServiceNode +// - BT::RosServiceNode +// - BT::RosTopicPubNode +// - BT::RosTopicSubNode +// +// - First argument: type to register (name of the class) +// - Second argument: string with the registration name +// +// Usage example: +// CreateRosNodePlugin(MyClassName, "MyClassName"); + +#define CreateRosNodePlugin(TYPE, REGISTRATION_NAME) \ + BTCPP_EXPORT void BT_RegisterRosNodeFromPlugin(BT::BehaviorTreeFactory& factory, ros::NodeHandle& node_handle) \ + { \ + BT::NodeBuilder builder = [&node_handle](const std::string& name, const BT::NodeConfiguration& config) \ + { \ + return std::make_unique(node_handle, name, config); \ + }; \ + \ + BT::TreeNodeManifest manifest; \ + manifest.type = BT::getType(); \ + manifest.ports = TYPE::providedPorts(); \ + manifest.registration_ID = REGISTRATION_NAME; \ + \ + factory.registerBuilder(manifest, builder); \ + } + +/** + * @brief RegisterRosNode function used to load a plugin and register + * the containing Node definition. + * + * @param factory the factory where the node should be registered. + * @param filepath path to the plugin. + * @param node_handle node handleto pass to the instances of the Node. + */ +inline void +RegisterRosNode(BT::BehaviorTreeFactory& factory, const std::filesystem::path& filepath, ros::NodeHandle& node_handle) +{ + BT::SharedLibrary loader(filepath); + typedef void (*Func)(BT::BehaviorTreeFactory&, ros::NodeHandle&); + auto func = (Func)loader.getSymbol("BT_RegisterRosNodeFromPlugin"); + func(factory, node_handle); +} From ec3878cff853516e5340f191f1b47030a465ecf2 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Tue, 15 Aug 2023 14:53:41 +0200 Subject: [PATCH 4/8] Update test for new service nodes --- test/test_bt.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/test/test_bt.cpp b/test/test_bt.cpp index e00e582..5b848d7 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -38,19 +38,19 @@ class PrintValue : public BT::SyncActionNode // http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29 //------------------------------------------------------------- -class AddTwoIntsAction: public RosServiceNode +class AddTwoIntsAction: public RosServiceClientNode { public: AddTwoIntsAction( ros::NodeHandle& handle, const std::string& node_name, const NodeConfiguration & conf): - RosServiceNode(handle, node_name, conf) {} + RosServiceClientNode(handle, node_name, conf) {} static PortsList providedPorts() { - return { + return providedBasicPorts({ InputPort("first_int"), InputPort("second_int"), - OutputPort("sum") }; + OutputPort("sum") }); } void sendRequest(RequestType& request) override @@ -75,7 +75,7 @@ class AddTwoIntsAction: public RosServiceNode } } - virtual NodeStatus onFailedRequest(RosServiceNode::FailureCause failure) override + virtual NodeStatus onFailedRequest(RosServiceClientNode::FailureCause failure) override { ROS_ERROR("AddTwoInts request failed %d", static_cast(failure)); return NodeStatus::FAILURE; @@ -99,9 +99,9 @@ RosActionNode(handle, name, conf) {} static PortsList providedPorts() { - return { + return providedBasicPorts({ InputPort("order"), - OutputPort("result") }; + OutputPort("result") }); } bool sendGoal(GoalType& goal) override @@ -186,7 +186,7 @@ int main(int argc, char **argv) BehaviorTreeFactory factory; factory.registerNodeType("PrintValue"); - RegisterRosService(factory, "AddTwoInts", nh); + RegisterRosServiceClient(factory, "AddTwoInts", nh); RegisterRosAction(factory, "Fibonacci", nh); auto tree = factory.createTreeFromText(xml_text); From 90fe98affaf3b151eae70fb888659e4b16a3df68 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Mon, 21 Aug 2023 10:10:04 +0200 Subject: [PATCH 5/8] Update license --- .../behaviortree_ros/bt_service_server_node.h | 17 +++++++++++++---- include/behaviortree_ros/register_ros_plugin.h | 1 + 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/include/behaviortree_ros/bt_service_server_node.h b/include/behaviortree_ros/bt_service_server_node.h index fcc5460..2a0c962 100644 --- a/include/behaviortree_ros/bt_service_server_node.h +++ b/include/behaviortree_ros/bt_service_server_node.h @@ -1,7 +1,16 @@ -/* - * Enway GmbH - All Rights reserved. - * Proprietary & confidential. - */ +// Copyright (c) 2023 Enway GmbH +// +// 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_SERVER_NODE_HPP_ #define BEHAVIOR_TREE_BT_SERVICE_SERVER_NODE_HPP_ diff --git a/include/behaviortree_ros/register_ros_plugin.h b/include/behaviortree_ros/register_ros_plugin.h index 2a5e498..87b8d7a 100644 --- a/include/behaviortree_ros/register_ros_plugin.h +++ b/include/behaviortree_ros/register_ros_plugin.h @@ -1,4 +1,5 @@ // Copyright (c) 2023 Davide Faconti +// Copyright (c) 2023 Enway GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 4919f65b0658fb1ec2a48d56dd24e5262e6093c4 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Wed, 23 Aug 2023 12:28:41 +0200 Subject: [PATCH 6/8] Call service only if request is valid --- include/behaviortree_ros/bt_service_client_node.h | 13 +++++++++---- test/test_bt.cpp | 5 ++++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/include/behaviortree_ros/bt_service_client_node.h b/include/behaviortree_ros/bt_service_client_node.h index cee2703..6af0789 100644 --- a/include/behaviortree_ros/bt_service_client_node.h +++ b/include/behaviortree_ros/bt_service_client_node.h @@ -63,15 +63,17 @@ class RosServiceClientNode : public BT::SyncActionNode } /// User must implement this method. - virtual void sendRequest(RequestType& request) = 0; + virtual bool 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{ + enum FailureCause + { MISSING_SERVER = 0, - FAILED_CALL = 1 + FAILED_CALL = 1, + REQUEST_INVALID = 2, }; /// Called when a service call failed. Can be overriden by the user. @@ -106,7 +108,10 @@ class RosServiceClientNode : public BT::SyncActionNode } typename ServiceT::Request request; - sendRequest(request); + if (!sendRequest(request)) + { + return onFailedRequest(REQUEST_INVALID); + } bool received = service_client_.call( request, reply_ ); if( !received ) { diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 5b848d7..52d37ff 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -53,12 +53,15 @@ class AddTwoIntsAction: public RosServiceClientNode("sum") }); } - void sendRequest(RequestType& request) override + bool + sendRequest(RequestType& request) override { getInput("first_int", request.a); getInput("second_int", request.b); expected_result_ = request.a + request.b; ROS_INFO("AddTwoInts: sending request"); + + return true; } NodeStatus onResponse(const ResponseType& rep) override From ba1ae8c8560bfddec243d607a24cf6dfe59d2b94 Mon Sep 17 00:00:00 2001 From: Jacob Seibert Date: Thu, 30 Nov 2023 12:32:35 +0100 Subject: [PATCH 7/8] Fix RosoutLogger implementation/building --- CMakeLists.txt | 14 +++++++++----- include/behaviortree_ros/loggers/rosout_logger.h | 2 ++ src/loggers/rosout_logger.cpp | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dd2510a..6dd5ae5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,7 +45,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include - LIBRARIES + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS ${ROS_DEPENDENCIES} ) @@ -54,10 +54,8 @@ include_directories( include ${catkin_INCLUDE_DIRS}) ###################################################### # LIBRARIES -#header only for the time being - -#add_library(behaviortree_ros ... ) -#target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME} src/loggers/rosout_logger.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ###################################################### # TESTS @@ -79,5 +77,11 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + ###################################################### # EXAMPLES and TOOLS diff --git a/include/behaviortree_ros/loggers/rosout_logger.h b/include/behaviortree_ros/loggers/rosout_logger.h index 47b21e6..d7973e6 100644 --- a/include/behaviortree_ros/loggers/rosout_logger.h +++ b/include/behaviortree_ros/loggers/rosout_logger.h @@ -4,6 +4,8 @@ #include #include +#include + namespace BT { diff --git a/src/loggers/rosout_logger.cpp b/src/loggers/rosout_logger.cpp index af11584..bf22995 100644 --- a/src/loggers/rosout_logger.cpp +++ b/src/loggers/rosout_logger.cpp @@ -56,7 +56,7 @@ void RosoutLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus ROS_INFO("[%s%s]: %s -> %s", node_name.c_str(), &whitespaces[std::min(ws_count, node_name.size())], toStr(prev_status, true).c_str(), - toStr(status, true).c_srt()); + toStr(status, true).c_str()); break; } } From e1f35024e24fc494fcb12cec9a9da59d027f734b Mon Sep 17 00:00:00 2001 From: Jacob Seibert Date: Mon, 25 Mar 2024 14:10:38 +0100 Subject: [PATCH 8/8] Fix ROS action node BehaviorTree.CPP doesn't allow setting node status to IDLE manually (anymore?) --- include/behaviortree_ros/bt_action_node.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index fb88383..883df47 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -105,7 +105,6 @@ class RosActionNode : public BT::ActionNodeBase { action_client_->cancelGoal(); } - setStatus(NodeStatus::IDLE); } protected: