AGVFM-372 Nav2 merge foxy sync 5.1 (#70)

* Fix deprecated usage of FutureReturnCode::SUCCESS (ros-navigation#2140)

Use rclcpp::FutureReturnCode::SUCCESS replace rclcpp::executor::FutureReturnCode::SUCCESS

* backporting to foxy checking goal index on backchecks (ros-navigation#2202)

* Fix recovery action collision check. (ros-navigation#2193)

* Fix recovery action collision check.

* Fix linting issue.

* Fix back up action behavior tree node name issue (ros-navigation#2206)

* Export nav2_bt_navigator library and dependencies (ros-navigation#2212)

Signed-off-by: Marco Lampacrescia <Marco.Lampacrescia@de.bosch.com>

* Optional transient map saver (ros-navigation#2215)

* Added transient local subscription qos profile parameter to map saver (ros-navigation#1871)

* Added transient local subscription qos profile parameter to map saver

* Made transient local default true

* Fixed linter problem

* switched back house world to waffle model

* Make transient map subscribe backwards compatible for foxy

Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com>

* Add has_node_params launch utility (ros-navigation#2257)

* corrected backup plugin name for multirobot params (ros-navigation#2288)

Co-authored-by: Simon Honigmann <shonigmann@blueorigin.com>

* foxy Sync 5.1 (ros-navigation#2291)

* merge conflict

* Add groot monitoring behavior tree visualization (ros-navigation#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 (ros-navigation#2119)

* Move voxel publisher activation into conditional that its on

* fix boundary point exclusion in convexFillCells (ros-navigation#2161)

* Regulated pure pursuit controller (ros-navigation#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 (ros-navigation#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 (ros-navigation#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 (ros-navigation#2183)

* Fixing control_frequency to controller_frequency typo (ros-navigation#2182)

* Write doxygen for navfn (ros-navigation#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 (ros-navigation#2181)

* make the 3 params changeable at runtime

* use parameter events callbacks

* doxygen

* lint

* Install test_updown to lib/ (ros-navigation#2208)

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

* [RPP] Remove dependency on collision checking to carrot location (ros-navigation#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 (ros-navigation#2228)

* Rviz state machine waypoint follower updates (ros-navigation#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>

* bumping to 0.4.6

* bump to 0.4.7 and add missing dep to RPP

* [nav2_bringup] Update waffle.model (ros-navigation#2296)

Changed to the latest tire mesh file names for waffle as per the latest `turtlebot3_gazebo` package.

This results in faster loading and resolves the errors that come in `gazebo --verbose`

* Update list of behavioural tree nodes (ros-navigation#2329)

* Update list of nodes with nodes compiled in the branch and excluding unexistant to prevent runtime exceptions.

* Updated documentation

Co-authored-by: Pau Carre <paucarre@cm-robotics.com>

* fix merge errors

* not currenlty used -> no need to depend on it

Co-authored-by: Homalozoa X <21300069+paeunt@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Albert Yen <acyen8@hotmail.com>
Co-authored-by: MarcoLm993 <65171491+MarcoLm993@users.noreply.github.com>
Co-authored-by: Victor Lopez <3469405+v-lopez@users.noreply.github.com>
Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com>
Co-authored-by: shonigmann <simonhonigmann@gmail.com>
Co-authored-by: Simon Honigmann <shonigmann@blueorigin.com>
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>
Co-authored-by: Jovian Dsouza <dsouzajovian123@gmail.com>
Co-authored-by: Pau Carré Cardona <pau.carre@gmail.com>
Co-authored-by: Pau Carre <paucarre@cm-robotics.com>

Author: 21 people

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_adjust_pallet_goal_action_bt_node","nav2_narrow_pre_approach_action_bt_node","nav2_find_put_down_goal_action_bt_node",  "nav2_compute_path_to_pose_action_bt_node", "nav2_compute_waypoints_to_pose_action_bt_node", "nav2_compute_path_along_waypoints_action_bt_node", "nav2_find_free_goal_on_path_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_path_empty_condition_bt_node","nav2_is_lidar_triggered_condition_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_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.4.5</version>
+  <version>0.4.7</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

@@ -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.
@@ -44,13 +45,24 @@ 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);
 
-  void addGrootMonitoring(BT::Tree * tree);
+  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();
 
-  void resetGrootMonitoring();
+  // 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);
 
   void addFileLogging(BT::Tree * tree);
 
@@ -63,26 +75,9 @@ class BehaviorTreeEngine
   void addMinitraceLogging(BT::Tree * tree);
 
   void resetMinitraceLogging();
-
-  // 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);
-  }
-
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
-  // This logger publish status changes using ZeroMQ. Used by Groot
   std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
   // This logger saves state changes on file
   std::unique_ptr<BT::FileLogger> file_logger_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -242,7 +242,7 @@ class BtActionNode : public BT::ActionNodeBase
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
 
     if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
-      rclcpp::executor::FutureReturnCode::SUCCESS)
+      rclcpp::FutureReturnCode::SUCCESS)
     {
       throw std::runtime_error("send_goal failed");
     }

nav2_behavior_tree/nav2_tree_nodes.xml

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

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.4.5</version>
+  <version>0.4.7</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

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,21 +61,54 @@ BehaviorTreeEngine::run(
 }
 
 BT::Tree
-BehaviorTreeEngine::buildTreeFromText(
+BehaviorTreeEngine::createTreeFromText(
   const std::string & xml_string,
   BT::Blackboard::Ptr blackboard)
 {
   return factory_.createTreeFromText(xml_string, blackboard);
 }
 
-void BehaviorTreeEngine::addGrootMonitoring(BT::Tree * tree)
+BT::Tree
+BehaviorTreeEngine::createTreeFromFile(
+  const std::string & file_path,
+  BT::Blackboard::Ptr blackboard)
 {
-    groot_monitor_ = std::make_unique<BT::PublisherZMQ>(*tree);
+  return factory_.createTreeFromFile(file_path, blackboard);
 }
 
-void BehaviorTreeEngine::resetGrootMonitoring()
+void
+BehaviorTreeEngine::addGrootMonitoring(
+  BT::Tree * tree,
+  uint16_t publisher_port,
+  uint16_t server_port,
+  uint16_t max_msg_per_second)
 {
-    groot_monitor_.reset();
+  // 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);
 }
 
 void BehaviorTreeEngine::addFileLogging(BT::Tree * tree) {
@@ -101,6 +133,6 @@ void BehaviorTreeEngine::addMinitraceLogging(BT::Tree * tree) {
 
 void BehaviorTreeEngine::resetMinitraceLogging() {
     minitrace_logger_.reset();
-}
 
 }  // namespace nav2_behavior_tree
+}

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.4.5</version>
+  <version>0.4.7</version>
   <description>Bringup scripts and configurations for the navigation2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -226,7 +226,7 @@ recoveries_server:
     costmap_topic: local_costmap/costmap_raw
     footprint_topic: local_costmap/published_footprint
     cycle_frequency: 10.0
-    recovery_plugins: ["spin", "back_up", "wait"]
+    recovery_plugins: ["spin", "backup", "wait"]
     spin:
       plugin: "nav2_recoveries/Spin"
     backup:

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -226,7 +226,7 @@ recoveries_server:
     costmap_topic: local_costmap/costmap_raw
     footprint_topic: local_costmap/published_footprint
     cycle_frequency: 10.0
-    recovery_plugins: ["spin", "back_up", "wait"]
+    recovery_plugins: ["spin", "backup", "wait"]
     spin:
       plugin: "nav2_recoveries/Spin"
     backup: