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]>

Author: 3 people

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>