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

Author: gramss

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