Eloquent sync (#1577)

* added a red circle for target pose location. changed the way marker id numbers are generated. resized markers

* edited a comment line

* cherry-picking mjeronimo:fix_bt_crash_after_reset (#1515)

* syncing eloquent with 1940266

* bump to 0.3.4

Co-authored-by: Melih Erdogan <[email protected]>
Co-authored-by: John Connolly <[email protected]>

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -35,7 +35,7 @@ class BehaviorTreeEngine
   virtual ~BehaviorTreeEngine() {}
 
   BtStatus run(
-    std::unique_ptr<BT::Tree> & tree,
+    BT::Tree * tree,
     std::function<void()> onLoop,
     std::function<bool()> cancelRequested,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -36,7 +36,7 @@ BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_l
 
 BtStatus
 BehaviorTreeEngine::run(
-  std::unique_ptr<BT::Tree> & tree,
+  BT::Tree * tree,
   std::function<void()> onLoop,
   std::function<bool()> cancelRequested,
   std::chrono::milliseconds loopTimeout)
@@ -51,10 +51,10 @@ BehaviorTreeEngine::run(
       return BtStatus::CANCELED;
     }
 
-    onLoop();
-
     result = tree->root_node->executeTick();
 
+    onLoop();
+
     loopRate.sleep();
   }
 

nav2_bringup/bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Bringup scripts and configurations for the navigation2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

nav2_bringup/nav2_gazebo_spawner/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_gazebo_spawner</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Package for spawning a robot model into Gazebo for navigation2</description>
   <maintainer email="[email protected]">lkumarbe</maintainer>
   <maintainer email="[email protected]">lkumarbe</maintainer>

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/include/nav2_bt_navigator/bt_navigator.hpp

@@ -119,9 +119,6 @@ class BtNavigator : public nav2_util::LifecycleNode
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
 
-  // The complete behavior tree that results from parsing the incoming XML
-  std::unique_ptr<BT::Tree> tree_;
-
   // Libraries to pull plugins (BT Nodes) from
   std::vector<std::string> plugin_lib_names_;
 

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/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

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
 {
@@ -111,16 +112,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
   RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str());
 
-  // Create the Behavior Tree from the XML input (after registering our own node types)
-  BT::Tree temp_tree = bt_->buildTreeFromText(xml_string_, blackboard_);
-
-  // Unfortunately, the BT library provides the tree as a struct instead of a pointer. So, we will
-  // createa new BT::Tree ourselves and move the data over
-  tree_ = std::make_unique<BT::Tree>();
-  tree_->root_node = temp_tree.root_node;
-  tree_->nodes = std::move(temp_tree.nodes);
-  temp_tree.root_node = nullptr;
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -163,10 +154,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   action_server_.reset();
   plugin_lib_names_.clear();
   xml_string_.clear();
-
-  RCLCPP_INFO(get_logger(), "Cleaning tree");
-
-  tree_.reset();
   blackboard_.reset();
   bt_.reset();
 
@@ -207,16 +194,22 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-  auto on_loop = [this]() {
+  // Create the Behavior Tree from the XML input
+  BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_);
+
+  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_behavior_tree::BtStatus rc = bt_->run(tree_, on_loop, is_canceling);
+  nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:
@@ -232,8 +225,6 @@ BtNavigator::navigateToPose()
     case nav2_behavior_tree::BtStatus::CANCELED:
       RCLCPP_INFO(get_logger(), "Navigation canceled");
       action_server_->terminate_all();
-      // Reset the BT so that it can be run again in the future
-      bt_->resetTree(tree_->root_node);
       break;
 
     default:

nav2_bt_navigator/src/ros_topic_logger.cpp

@@ -0,0 +1,67 @@
+// 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_common/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>A set of headers for plugins core to the navigation2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Carl Delsey</maintainer>

nav2_costmap_2d/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

nav2_dwb_controller/costmap_queue/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_critics/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

nav2_dwb_controller/nav2_dwb_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

nav2_dwb_controller/nav_2d_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_msgs</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/nav_2d_utils/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_utils</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_experimental/nav2_rl/nav2_turtlebot3_rl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>nav2_turtlebot3_rl</name>
-  <version>0.3.3</version>
+  <version>0.3.4</version>
   <description>This package enables Reinfocement Learning with Gazebo and Turtlebot3</description>
 
   <maintainer email="[email protected]">Mohammad Haghighipanah</maintainer>