Add get_clock() method

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<rclcpp::Clock>(RCL_ROS_TIME);
+    return clock;
+  }
+
   using ClientsRegistry =
       std::unordered_map<std::string, std::weak_ptr<ActionClientInstance>>;
   // contains the fully-qualified name of the node and the name of the client

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<rclcpp::Clock>(RCL_ROS_TIME);
+    return clock;
+  }
+
   using ClientsRegistry =
       std::unordered_map<std::string, std::weak_ptr<ServiceClientInstance>>;
   // contains the fully-qualified name of the node and the name of the client

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<const Feedback> 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());

btcpp_ros2_samples/src/sleep_action.hpp

@@ -23,4 +23,6 @@ class SleepAction : public RosActionNode<btcpp_ros2_interfaces::action::Sleep>
   BT::NodeStatus onResultReceived(const WrappedResult& wr) override;
 
   virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override;
+
+  virtual BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) override;
 };