diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 05c664d..a30b352 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -206,6 +206,16 @@ class RosActionNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } + rclcpp::Clock::SharedPtr clock() + { + if(auto node = node_.lock()) + { + return node->get_clock(); + } + static auto clock = std::make_shared(RCL_ROS_TIME); + return clock; + } + 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..3f56ed8 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -177,6 +177,16 @@ class RosServiceNode : public BT::ActionNodeBase return rclcpp::Clock(RCL_ROS_TIME).now(); } + rclcpp::Clock::SharedPtr clock() + { + if(auto node = node_.lock()) + { + return node->get_clock(); + } + static auto clock = std::make_shared(RCL_ROS_TIME); + return clock; + } + 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..b3c0c63 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(), *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; };