foxy Sync 5.1 (#2291)

* merge conflict

* Add groot monitoring behavior tree visualization (#1958)

* include ZMQ publisher for Groot

very plain integration, should be made optionally through a launch parameter

* fix Groot crashing finding custom nodes in monitor mode

straight forward working fix. The manifest was missing, so Groot searched custom node IDs that it did not have. This is implemented correctly directly in BT.CPP V3 and should be used instead of an implementation in nav2_bt_engine

* refactor buildTreeFromText to createTreeFromText as in BT.CPP v3

* forward XML to createTreeFromText from BT.CPP v3 factory function

* Add createTreeFromFile forware to BT-factory function

* fix createTreeFromFile args..

* add personal copyright

I think this is okay for finding a nasty bug.. :)

* move creating ZMQ Publisher from run to dedicated function

this way the ZMQ Publisher ca be added to individual trees within the same factory. Should be important for switching trees (XML files)

* Add parameter for Groot Monitoring - default true. Also cleanup ZMQ

* Move haltAllActions() Implementation from .hpp to .cpp

* update Copyright in hpp of BT-engine

* make linters happy.. :)

* Update Groot parameter naming and chg default=0

* rename resetZMQGrootMonitor -> resetGrootMonitor

* add parameter to nav2_params.yaml - default = false

* add ZMQ params and logic for server/pub ports

* Fix RewrittenYaml ignoring Integers

Integers where converted as floats before which crashes get_parameter.. fun thing....

* add launch based tests for params and ZMQ

* Activate Dijkstra and A* switching tests, thanks to RewrittenYaml

* add pyzmq==19.0.2 via pip3 to CI test_workspace

* make flake8 linter happy

* make cpp linters happy

* add personal copyright

* add GoalUpdated BT node description in order to view the full default BT

only affects editor mode of Groot and not live monitoring

* make linter happy (unused import)

* remove unused groot-port replacement functions in test_system_launch.py

* add groot parameters to params.md

* get reloading BTs to work nicely with Groot

* pretty space for smac :)

* switch from unsinged to uint16_t

* fix converting string into float or int

* Revert "add pyzmq==19.0.2 via pip3 to CI test_workspace"

This reverts commit 7bca081.

* Switch to 4 spaces indent and other linter stuff for RewrittenYaml

* removed prints in test_system_launch.py

* linter stuff

* add python-zmq as test_depend in package.xml (instead of .CI_conf)

* enable groot monitoring by default

* remove ZMQ from naming (function / variable)

* remove variable zmq ports from testing scripts

* remove default ports in BT_engine, as they are set through (def-)params

* Remove complete test for "dynamic" ZMQ ports testing

* fix python-zmq depend location

* fix style

* swap missing Groot to default True

* fix rosdep zmq + flake8 fixes in system_tests

* remove debug logs + c_str()

* remove final debug_log

* return failure on plugin failure (#2119)

* Move voxel publisher activation into conditional that its on

* fix boundary point exclusion in convexFillCells (#2161)

* Regulated pure pursuit controller (#2152)

* regulated pure pursuit migration commit

* adding speed limit API

* adding review comments + adding rotate to goal heading

* adding test dir

* add some initial tests

* more tests

* remove old comment

* improve readme

* fix CI

* first attempt at changing algos in tests

* allowing full path parameter substitutions

* adding integration tests

* enable SMAC testing too with new changes

* swap algos

* revert

* Update angular velocity after constraining linear velocity (#2165)

This ensures the robot moves towards the lookahead point more closely.
If the angular velocity is not updated, then the robot tries to take cuts while turning,
which could lead to collisions when near obstacles

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Update cost scaling heuristic to vary speed linearly with distance (#2164)

* Update cost scaling to vary linearly with distance instead of relying on costmap cost

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Resolve suggested changes

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add documentation for cost scaling parameters

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Improve parameter descriptions

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Comment cost scaling tests since layered costmap is not initialized

A valid layered costmap reference is needed to get the inscribed radius

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

Co-authored-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Updating example yaml to include extra params (#2183)

* Fixing control_frequency to controller_frequency typo (#2182)

* Write doxygen for navfn (#2184)

* Write doxygen for navfn

* Remove forward slashes

* expose dwb's shorten_transformed_plan param

* Adding RPP to metapackage.xml

* [NavFn] Make the 3 parameters changeable at runtime (#2181)

* make the 3 params changeable at runtime

* use parameter events callbacks

* doxygen

* lint

* Install test_updown to lib/ (#2208)

* Remove optimization check on carrot, incorrect optimization (#2209)

* [RPP] Remove dependency on collision checking to carrot location (#2211)

* Remove dependency on collision checking to carrot location

* Fix i removal

* changing API to be consistent with collision updates

* fix typo in regulated pure pursuit readme (#2228)

* Rviz state machine waypoint follower updates (#2227)

* working on canceling state machine for waypoint mode

* fixing cancelation logic

* fix linting isue

* adding cherry pick fixes

Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
Co-authored-by: Florian Gramß <6034322+gramss@users.noreply.github.com>
Co-authored-by: ChristofDubs <christof.dubs@gmx.ch>
Co-authored-by: Shrijit Singh <shrijitsingh99@gmail.com>
Co-authored-by: Phone Thiha Kyaw <mlsdphonethk@gmail.com>
Co-authored-by: simutisernestas <35775651+simutisernestas@users.noreply.github.com>
Co-authored-by: G.Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Uladzslau <79460842+Uladzslau@users.noreply.github.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>

doc/parameters/param_list.md

@@ -5,6 +5,9 @@
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | default_bt_xml_filename | N/A | path to the default behavior tree XML description |
+| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree |
+| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot |
+| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot |
 | plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |
@@ -237,6 +240,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | `<dwb plugin>`.critics | N/A | List of critic plugins to use |
 | `<dwb plugin>`.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in |
 | `<dwb plugin>`.prune_plan | true | Whether to prune the path of old, passed points |
+| `<dwb plugin>`.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics |
 | `<dwb plugin>`.prune_distance | 1.0 | Distance (m) to prune backward until |
 | `<dwb plugin>`.debug_trajectory_details | false | Publish debug information |
 | `<dwb plugin>`.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name |

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,6 +23,8 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
 
 namespace nav2_behavior_tree
 {
@@ -40,28 +43,29 @@ class BehaviorTreeEngine
     std::function<bool()> cancelRequested,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
-  BT::Tree buildTreeFromText(
+  BT::Tree createTreeFromText(
     const std::string & xml_string,
     BT::Blackboard::Ptr blackboard);
 
+  BT::Tree createTreeFromFile(
+    const std::string & file_path,
+    BT::Blackboard::Ptr blackboard);
+
+  void addGrootMonitoring(
+    BT::Tree * tree,
+    uint16_t publisher_port,
+    uint16_t server_port,
+    uint16_t max_msg_per_second = 25);
+
+  void resetGrootMonitor();
+
   // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
-  void haltAllActions(BT::TreeNode * root_node)
-  {
-    // this halt signal should propagate through the entire tree.
-    root_node->halt();
-
-    // but, just in case...
-    auto visitor = [](BT::TreeNode * node) {
-        if (node->status() == BT::NodeStatus::RUNNING) {
-          node->halt();
-        }
-      };
-    BT::applyRecursiveVisitor(root_node, visitor);
-  }
+  void haltAllActions(BT::TreeNode * root_node);
 
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+  std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -54,6 +54,8 @@
       <input_port name="parent">Parent frame for transform</input_port>
     </Condition>
 
+    <Condition ID="GoalUpdated"/>
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,8 +22,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp_v3/utils/shared_library.h"
 
-using namespace std::chrono_literals;
-
 namespace nav2_behavior_tree
 {
 
@@ -62,13 +61,54 @@ BehaviorTreeEngine::run(
 }
 
 BT::Tree
-BehaviorTreeEngine::buildTreeFromText(
+BehaviorTreeEngine::createTreeFromText(
   const std::string & xml_string,
   BT::Blackboard::Ptr blackboard)
 {
-  BT::XMLParser p(factory_);
-  p.loadFromText(xml_string);
-  return p.instantiateTree(blackboard);
+  return factory_.createTreeFromText(xml_string, blackboard);
+}
+
+BT::Tree
+BehaviorTreeEngine::createTreeFromFile(
+  const std::string & file_path,
+  BT::Blackboard::Ptr blackboard)
+{
+  return factory_.createTreeFromFile(file_path, blackboard);
+}
+
+void
+BehaviorTreeEngine::addGrootMonitoring(
+  BT::Tree * tree,
+  uint16_t publisher_port,
+  uint16_t server_port,
+  uint16_t max_msg_per_second)
+{
+  // This logger publish status changes using ZeroMQ. Used by Groot
+  groot_monitor_ = std::make_unique<BT::PublisherZMQ>(
+    *tree, max_msg_per_second, publisher_port,
+    server_port);
+}
+
+void
+BehaviorTreeEngine::resetGrootMonitor()
+{
+  groot_monitor_.reset();
+}
+
+// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
+void
+BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)
+{
+  // this halt signal should propagate through the entire tree.
+  root_node->halt();
+
+  // but, just in case...
+  auto visitor = [](BT::TreeNode * node) {
+      if (node->status() == BT::NodeStatus::RUNNING) {
+        node->halt();
+      }
+    };
+  BT::applyRecursiveVisitor(root_node, visitor);
 }
 
 }  // namespace nav2_behavior_tree

nav2_bringup/bringup/params/nav2_params.yaml

@@ -53,6 +53,9 @@ bt_navigator:
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /odom
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
     default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node

nav2_bt_navigator/src/bt_navigator.cpp

@@ -21,6 +21,7 @@
 #include <utility>
 #include <vector>
 #include <set>
+#include <exception>
 
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -68,6 +69,9 @@ BtNavigator::BtNavigator()
   declare_parameter("global_frame", std::string("map"));
   declare_parameter("robot_base_frame", std::string("base_link"));
   declare_parameter("odom_topic", std::string("odom"));
+  declare_parameter("enable_groot_monitoring", true);
+  declare_parameter("groot_zmq_publisher_port", 1666);
+  declare_parameter("groot_zmq_server_port", 1667);
 }
 
 BtNavigator::~BtNavigator()
@@ -146,9 +150,13 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename)
 {
   // Use previous BT if it is the existing one
   if (current_bt_xml_filename_ == bt_xml_filename) {
+    RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded");
     return true;
   }
 
+  // if a new tree is created, than the ZMQ Publisher must be destroyed
+  bt_->resetGrootMonitor();
+
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(bt_xml_filename);
 
@@ -161,13 +169,21 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename)
     std::istreambuf_iterator<char>(xml_file),
     std::istreambuf_iterator<char>());
 
-  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
-  tree_ = bt_->buildTreeFromText(xml_string, blackboard_);
+  tree_ = bt_->createTreeFromText(xml_string, blackboard_);
   current_bt_xml_filename_ = bt_xml_filename;
 
+  // get parameter for monitoring with Groot via ZMQ Publisher
+  if (get_parameter("enable_groot_monitoring").as_bool()) {
+    uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int();
+    uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int();
+    // optionally add max_msg_per_second = 25 (default) here
+    try {
+      bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port);
+    } catch (const std::logic_error & e) {
+      RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what());
+    }
+  }
   return true;
 }
 
@@ -211,6 +227,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   current_bt_xml_filename_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_.rootNode());
+  bt_->resetGrootMonitor();
   bt_.reset();
 
   RCLCPP_INFO(get_logger(), "Completed Cleaning up");
@@ -243,15 +260,15 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-  auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
+  std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
 
   // Empty id in request is default for backward compatibility
   bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename;
 
   if (!loadBehaviorTree(bt_xml_filename)) {
     RCLCPP_ERROR(
-      get_logger(), "BT file not found: %s. Navigation canceled",
-      bt_xml_filename.c_str(), current_bt_xml_filename_.c_str());
+      get_logger(), "BT file not found: %s. Navigation canceled.",
+      bt_xml_filename.c_str());
     action_server_->terminate_current();
     return;
   }