[Continued from 1329] Provide behavior tree logging output on the console and a topic (#1486)

* Add console logging

* Fix node names so every node is uniquely identified in log

* Adding ros topic bt logger

* update BT nav + recovery for merge issues

* rename compute path -> compute path to pose

* renaming to follow literature

Co-authored-by: Carl Delsey <[email protected]>

Author: SteveMacenski

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -51,10 +51,10 @@ BehaviorTreeEngine::run(
       return BtStatus::CANCELED;
     }
 
-    onLoop();
-
     result = tree->root_node->executeTick();
 
+    onLoop();
+
     loopRate.sleep();
   }
 

nav2_bt_navigator/CMakeLists.txt

@@ -32,6 +32,7 @@ set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
   src/bt_navigator.cpp
+  src/ros_topic_logger.cpp
 )
 
 set(dependencies

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -1,3 +1,4 @@
+
 <!--
   This Behavior Tree replans the global path periodically at 1 Hz and it also has
   recovery actions.
@@ -9,17 +10,17 @@
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
             <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-            <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
-          <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </PipelineSequence>
       <SequenceStar name="RecoveryActions">
-        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
-        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
+        <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+        <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
         <Spin spin_dist="1.57"/>
         <Wait wait_duration="5"/>
       </SequenceStar>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml

@@ -11,15 +11,15 @@
           <RecoveryNode number_of_retries="4" name="ComputePathToPose">
             <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
             <RoundRobin name="GlobalPlannerRecoveryActions">
-              <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
               <Wait wait_duration="2"/>
             </RoundRobin>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="4" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
           <RoundRobin name="PlannerRecoveryActions">
-            <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
             <Spin spin_dist="1.57"/>
           </RoundRobin>
         </RecoveryNode>

nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp

@@ -0,0 +1,48 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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 NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_
+#define NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_
+
+#include <vector>
+#include "behaviortree_cpp_v3/loggers/abstract_logger.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_msgs/msg/behavior_tree_log.hpp"
+#include "nav2_msgs/msg/behavior_tree_status_change.h"
+
+namespace nav2_bt_navigator
+{
+
+class RosTopicLogger : public BT::StatusChangeLogger
+{
+public:
+  RosTopicLogger(const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree);
+
+  void callback(
+    BT::Duration timestamp,
+    const BT::TreeNode & node,
+    BT::NodeStatus prev_status,
+    BT::NodeStatus status) override;
+
+  void flush() override;
+
+protected:
+  rclcpp::Node::SharedPtr ros_node_;
+  rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
+  std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
+};
+
+}   // namespace nav2_bt_navigator
+
+#endif   // NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_

nav2_bt_navigator/src/bt_navigator.cpp

@@ -22,6 +22,7 @@
 #include <vector>
 
 #include "nav2_behavior_tree/bt_conversions.hpp"
+#include "nav2_bt_navigator/ros_topic_logger.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -206,12 +207,15 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-  auto on_loop = [this]() {
+  RosTopicLogger topic_logger(client_node_, *tree_);
+
+  auto on_loop = [&]() {
       if (action_server_->is_preempt_requested()) {
         RCLCPP_INFO(get_logger(), "Received goal preemption request");
         action_server_->accept_pending_goal();
         initializeGoalPose();
       }
+      topic_logger.flush();
     };
 
   // Execute the BT that was previously created in the configure step

nav2_bt_navigator/src/ros_topic_logger.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+
+#include <utility>
+#include "nav2_bt_navigator/ros_topic_logger.hpp"
+#include "tf2_ros/buffer_interface.h"
+
+namespace nav2_bt_navigator
+{
+
+RosTopicLogger::RosTopicLogger(
+  const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree)
+: StatusChangeLogger(tree.root_node), ros_node_(ros_node)
+{
+  log_pub_ = ros_node_->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
+    "behavior_tree_log",
+    rclcpp::QoS(10));
+}
+
+void RosTopicLogger::callback(
+  BT::Duration timestamp,
+  const BT::TreeNode & node,
+  BT::NodeStatus prev_status,
+  BT::NodeStatus status)
+{
+  nav2_msgs::msg::BehaviorTreeStatusChange event;
+
+  // BT timestamps are a duration since the epoch. Need to convert to a time_point
+  // before converting to a msg.
+  event.timestamp =
+    tf2_ros::toMsg(std::chrono::time_point<std::chrono::high_resolution_clock>(timestamp));
+  event.node_name = node.name();
+  event.previous_status = toStr(prev_status, false);
+  event.current_status = toStr(status, false);
+  event_log_.push_back(std::move(event));
+
+  RCLCPP_DEBUG(ros_node_->get_logger(), "[%.3f]: %25s %s -> %s",
+    std::chrono::duration<double>(timestamp).count(),
+    node.name().c_str(),
+    toStr(prev_status, true).c_str(),
+    toStr(status, true).c_str() );
+}
+
+void RosTopicLogger::flush()
+{
+  if (event_log_.size() > 0) {
+    nav2_msgs::msg::BehaviorTreeLog log_msg;
+    log_msg.timestamp = ros_node_->now();
+    log_msg.event_log = event_log_;
+    log_pub_->publish(log_msg);
+    event_log_.clear();
+  }
+}
+
+}   // namespace nav2_bt_navigator

nav2_msgs/CMakeLists.txt

@@ -16,6 +16,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"
   "msg/VoxelGrid.msg"
+  "msg/BehaviorTreeStatusChange.msg"
+  "msg/BehaviorTreeLog.msg"
   "srv/GetCostmap.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"

nav2_msgs/msg/BehaviorTreeLog.msg

@@ -0,0 +1,2 @@
+builtin_interfaces/Time timestamp    # ROS time that this log message was sent.
+BehaviorTreeStatusChange[] event_log

nav2_msgs/msg/BehaviorTreeStatusChange.msg

@@ -0,0 +1,4 @@
+builtin_interfaces/Time timestamp  # internal behavior tree event timestamp. Typically this is wall clock time
+string node_name
+string previous_status              # IDLE, RUNNING, SUCCESS or FAILURE
+string current_status               # IDLE, RUNNING, SUCCESS or FAILURE