replaced RosTopicLogger with custom logger

RobotCodeLab · Sep 12, 2022 · 4be5f56 · 4be5f56
1 parent 0aa7342
commit 4be5f56
Show file tree

Hide file tree

Showing 6 changed files with 19 additions and 4 deletions.
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(behaviortree_cpp_v3_ros2_publisher REQUIRED)
 
 nav2_package()
 
@@ -41,6 +42,7 @@ set(dependencies
   std_msgs
   std_srvs
   nav2_util
+  behaviortree_cpp_v3_ros2_publisher
 )
 
 add_library(${library_name} SHARED

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -21,10 +21,12 @@
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_behavior_tree/behavior_tree_engine.hpp"
-#include "nav2_behavior_tree/ros_topic_logger.hpp"
+// #include "nav2_behavior_tree/ros_topic_logger.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/simple_action_server.hpp"
 
+#include "behaviortree_cpp_v3_ros2_publisher/bt_ros_publisher.hpp"
+
 namespace nav2_behavior_tree
 {
 /**
@@ -223,7 +225,8 @@ class BtActionServer
   rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")};
 
   // To publish BT logs
-  std::unique_ptr<RosTopicLogger> topic_logger_;
+  // std::unique_ptr<RosTopicLogger> topic_logger_;
+  std::unique_ptr<BTRosPublisher> topic_logger_;
 
   // Duration for each iteration of BT execution
   std::chrono::milliseconds bt_loop_duration_;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -152,6 +152,7 @@ bool BtActionServer<ActionT>::on_cleanup()
   client_node_.reset();
   action_server_.reset();
   topic_logger_.reset();
+//   tree_ros_server_.reset();
   plugin_lib_names_.clear();
   current_bt_xml_filename_.clear();
   blackboard_.reset();
@@ -190,7 +191,7 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
 
   // Create the Behavior Tree from the XML input
   tree_ = bt_->createTreeFromText(xml_string, blackboard_);
-  topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
+  topic_logger_ = std::make_unique<BTRosPublisher>(client_node_, tree_, filename);
 
   current_bt_xml_filename_ = filename;
 
@@ -232,6 +233,7 @@ void BtActionServer<ActionT>::executeCallback()
         on_preempt_callback_(action_server_->get_pending_goal());
       }
       topic_logger_->flush();
+
       on_loop_callback_();
     };
 
@@ -263,6 +265,7 @@ void BtActionServer<ActionT>::executeCallback()
       action_server_->terminate_all(result);
       break;
   }
+
 }
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -18,6 +18,7 @@
   <build_depend>rclcpp_action</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>behaviortree_cpp_v3</build_depend>
+  <build_depend>behaviortree_cpp_v3_ros2_publisher</build_depend>
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -31,7 +32,8 @@
   <build_depend>nav2_util</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
   <build_depend>nav2_common</build_depend>
-
+  <build_depend>behaviortree_cpp_v3_ros2_publisher</build_depend>
+
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>
   <exec_depend>rclcpp_lifecycle</exec_depend>
@@ -47,6 +49,7 @@
   <exec_depend>tf2_geometry_msgs</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>behaviortree_cpp_v3_ros2_publisher</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(behaviortree_cpp_v3_ros2_publisher REQUIRED)
 
 nav2_package()
 
@@ -45,6 +46,7 @@ set(dependencies
   nav2_util
   nav2_core
   tf2_ros
+  behaviortree_cpp_v3_ros2_publisher
 )
 
 add_library(${library_name} SHARED

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -23,6 +23,7 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>nav2_util</build_depend>
   <build_depend>nav2_core</build_depend>
+  <build_depend>behaviortree_cpp_v3_ros2_publisher</build_depend>
 
   <exec_depend>behaviortree_cpp_v3</exec_depend>
   <exec_depend>rclcpp</exec_depend>
@@ -35,6 +36,7 @@
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_core</exec_depend>
+  <exec_depend>behaviortree_cpp_v3_ros2_publisher</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>