From f293f3521f2434d184086fcccc69c1fc90803e65 Mon Sep 17 00:00:00 2001 From: Elvin Date: Thu, 27 Jun 2024 11:20:45 +0200 Subject: [PATCH 1/2] Add get_clock() method, and example of usage for log throttling in sleep node example --- .../include/behaviortree_ros2/bt_action_node.hpp | 9 +++++++++ .../include/behaviortree_ros2/bt_service_node.hpp | 9 +++++++++ btcpp_ros2_samples/src/sleep_action.cpp | 6 ++++++ btcpp_ros2_samples/src/sleep_action.hpp | 2 ++ 4 files changed, 26 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 05c664d..58de542 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -206,6 +206,15 @@ class RosActionNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } + rclcpp::Clock::SharedPtr get_clock() + { + if(auto node = node_.lock()) + { + return node->get_clock(); + } + return std::make_shared(RCL_ROS_TIME); + } + using ClientsRegistry = std::unordered_map>; // contains the fully-qualified name of the node and the name of the client diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 77958cd..a8f9ac3 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -177,6 +177,15 @@ class RosServiceNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } + rclcpp::Clock::SharedPtr get_clock() + { + if(auto node = node_.lock()) + { + return node->get_clock(); + } + return std::make_shared(RCL_ROS_TIME); + } + using ClientsRegistry = std::unordered_map>; // contains the fully-qualified name of the node and the name of the client diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index 9b8112e..24f241b 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -22,6 +22,12 @@ NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) return NodeStatus::FAILURE; } +BT::NodeStatus SleepAction::onFeedback(const std::shared_ptr feedback) +{ + RCLCPP_INFO_THROTTLE(logger(), *get_clock(), 1000, "Feedback cycle %d", feedback->cycle); + return NodeStatus::RUNNING; +} + void SleepAction::onHalt() { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); diff --git a/btcpp_ros2_samples/src/sleep_action.hpp b/btcpp_ros2_samples/src/sleep_action.hpp index 84ec3a8..8d35900 100644 --- a/btcpp_ros2_samples/src/sleep_action.hpp +++ b/btcpp_ros2_samples/src/sleep_action.hpp @@ -23,4 +23,6 @@ class SleepAction : public RosActionNode BT::NodeStatus onResultReceived(const WrappedResult& wr) override; virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; + + virtual BT::NodeStatus onFeedback(const std::shared_ptr feedback) override; }; From 2b5cfe970b530aee186e531b4fda38a75ef35869 Mon Sep 17 00:00:00 2001 From: Elvin <45923444+EGAlberts@users.noreply.github.com> Date: Wed, 18 Sep 2024 10:59:27 +0200 Subject: [PATCH 2/2] making requested changes to method name and default return behavior --- .../include/behaviortree_ros2/bt_action_node.hpp | 5 +++-- .../include/behaviortree_ros2/bt_service_node.hpp | 5 +++-- btcpp_ros2_samples/src/sleep_action.cpp | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 58de542..a30b352 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -206,13 +206,14 @@ class RosActionNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } - rclcpp::Clock::SharedPtr get_clock() + rclcpp::Clock::SharedPtr clock() { if(auto node = node_.lock()) { return node->get_clock(); } - return std::make_shared(RCL_ROS_TIME); + static auto clock = std::make_shared(RCL_ROS_TIME); + return clock; } using ClientsRegistry = diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index a8f9ac3..3f56ed8 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -177,13 +177,14 @@ class RosServiceNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } - rclcpp::Clock::SharedPtr get_clock() + rclcpp::Clock::SharedPtr clock() { if(auto node = node_.lock()) { return node->get_clock(); } - return std::make_shared(RCL_ROS_TIME); + static auto clock = std::make_shared(RCL_ROS_TIME); + return clock; } using ClientsRegistry = diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index 24f241b..b3c0c63 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -24,7 +24,7 @@ NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) BT::NodeStatus SleepAction::onFeedback(const std::shared_ptr feedback) { - RCLCPP_INFO_THROTTLE(logger(), *get_clock(), 1000, "Feedback cycle %d", feedback->cycle); + RCLCPP_INFO_THROTTLE(logger(), *clock(), 1000, "Feedback cycle %d", feedback->cycle); return NodeStatus::RUNNING; }