Improved switching BTs with active Groot monitoring (ZMQ logger destruction)

[gramss]

Switching BTs with active ZMQ logging (for Groot) enabled increased the switching time varying from ~20-99ms.

Switching is performed by destructing the zmq logger, loading a new tree based on xml with currently the same factory (plugins), and finally adding a new zmq logger to the new tree. For reference, see this PR here: ros-navigation/navigation2#1958

This is due to the timeout set for the ZMQ server. The detached thread always waits the timeout before the next while loop starts (which would end if the server is requested to end through the global variable).

BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp

Line 68 in f54f6d8

bool received = zmq_->server.recv(&req, 0);

( Timeout set in line 57 + 58 )

After consulting the ZMQ documentation, a few functions promised to directly halt any ongoing requests in the ZMQ context.

See the documentation here:
http://api.zeromq.org/master:zmq-ctx-shutdown

Context shutdown will cause any blocking operations currently in progress on sockets open within context to return immediately with an error code of ETERM. [...]

This successfully decreased the max blocking time of 100ms to 1 - 1,5ms for deconstructing the zmq logger.

( close() is already performed afterwards by the default constructor of the server and publisher objects -> delete zmq_ )

[SteveMacenski]

That latency for creating a BT was really nasty for nav2 when we have to do that relatively often when loading new BTs. My preference would be an option to reset() the ZMQ logger so that we can just use the same object over and over - but speeding up destruction is OK too.

[SteveMacenski]

Not my code base to tell you what to do, but if without brackets makes my skin crawl. Might want to make it explicit like the styling of the rest of the file (+ L177 below). I've been burned alot by this in the past when working in codebases with python developers not realizing that only the 1st line after the if is used in the condition without brackets in C++.

[gramss]

The python perspective argument is really good.
This burned in somehow as a nice little c++ feature..
But better to skip stuff like that and keep it more readable / less future error prone..

[SteveMacenski]

I'm also Mr. Explicit in my code, so I take the exact opposite extreme. I avoid auto unless the type is so long that I don't want to write it all out (and using it <2 times so not worth typedefing). Do as you like here, not up to me 😄

[gramss]

option to reset() the ZMQ logger

yes. this would still be a little bit nicer, but involves bigger changes to the architecture how the logger is "attached" to the tree.

Also for the reset(), the server probably needs to get turn down securely before switching the tree underneath. Otherwise the server might send an old tree to Groot in the process of a switch/reset due to the detached thread. So having a fast destruction might also be necessary in this case.

If interested by @facontidavide this could be tracked as future issue/future with a "nice to have" - label?

[facontidavide]

it looks good to me. Thanks a lot for your contribution.

Actually that logger needs a full refactoring at some point, because I am not 100% happy with the fact that the subtrees are not modelled in the serialized message.

Anyway, this is another story....

[facontidavide]

@gramss

[gramss]

???

But it builds on my machine™️ ..
Have you setup -Wdeprecated-declarations as error? ZMQ complains about the send method currently used. But I did not touch it for now..

Also just tried with a clean build now.. Also just warnings and notes.
Maybe the failed build is due to an update of zmq behind the curtain?

--- stderr: behaviortree_cpp_v3                                  
In file included from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h:5,
                 from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_file_logger.cpp:2:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h: In constructor ‘Serialization::StatusChange::StatusChange()’:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h:165:41: warning: ‘void* memset(void*, int, size_t)’ clearing an object of non-trivial type ‘struct Serialization::StatusChange’; use assignment or value-initialization instead [-Wclass-memaccess]
  165 |     memset(this, 0, sizeof(StatusChange));
      |                                         ^
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h:155:40: note: ‘struct Serialization::StatusChange’ declared here
  155 | FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) StatusChange FLATBUFFERS_FINAL_CLASS {
      |                                        ^~~~~~~~~~~~
In file included from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h:5,
                 from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:2:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h: In constructor ‘Serialization::StatusChange::StatusChange()’:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h:165:41: warning: ‘void* memset(void*, int, size_t)’ clearing an object of non-trivial type ‘struct Serialization::StatusChange’; use assignment or value-initialization instead [-Wclass-memaccess]
  165 |     memset(this, 0, sizeof(StatusChange));
      |                                         ^
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h:155:40: note: ‘struct Serialization::StatusChange’ declared here
  155 | FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) StatusChange FLATBUFFERS_FINAL_CLASS {
      |                                        ^~~~~~~~~~~~
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp: In lambda function:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:68:58: warning: ‘bool zmq::detail::socket_base::recv(zmq::message_t*, int)’ is deprecated: from 4.3.1, use recv taking a reference to message_t and recv_flags [-Wdeprecated-declarations]
   68 |                 bool received = zmq_->server.recv(&req, 0);
      |                                                          ^
In file included from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:4:
/usr/include/zmq.hpp:1407:10: note: declared here
 1407 |     bool recv(message_t *msg_, int flags_ = 0)
      |          ^~~~
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:73:47: warning: ‘bool zmq::detail::socket_base::send(zmq::message_t&, int)’ is deprecated: from 4.3.1, use send taking message_t and send_flags [-Wdeprecated-declarations]
   73 |                     zmq_->server.send(reply, 0);
      |                                               ^
In file included from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:4:
/usr/include/zmq.hpp:1326:10: note: declared here
 1326 |     bool send(message_t &msg_,
      |          ^~~~
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp: In member function ‘virtual void BT::PublisherZMQ::flush()’:
/home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:171:40: warning: ‘bool zmq::detail::socket_base::send(zmq::message_t&, int)’ is deprecated: from 4.3.1, use send taking message_t and send_flags [-Wdeprecated-declarations]
  171 |         zmq_->publisher.send(message, 0);
      |                                        ^
In file included from /home/flo/nav2_ws/ros2_nav_dependencies_ws/src/BehaviorTree/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:4:
/usr/include/zmq.hpp:1326:10: note: declared here
 1326 |     bool send(message_t &msg_,
      |          ^~~~
---
Finished <<< behaviortree_cpp_v3 [41.9s]

[gramss]

@facontidavide

This is the error from CI:

/root/target_ws/src/BehaviorTree.CPP/src/loggers/bt_zmq_publisher.cpp:94:19: error: ‘class zmq::context_t’ has no member named ‘shutdown’
       zmq_->context.shutdown();
                     ^~~~~~~~

Looks to me that the CI has an older version of the zmq library..?
I did not willingly/manually updated this library on my machine.. Have you maybe forced CI to hold onto a specific zmq lib version?
Or is this due to a messed up setup on my side..?

I'm building BT.CPP as part of the nav2_ws (including ROS2 from source and all dependencies, including BT.CPP)

Edit:

This is my installed version of libzmq3-dev:

$ apt info libzmq3-dev
Package: libzmq3-dev
Version: 4.3.2-2ubuntu1
[...]
Depends: libzmq5 (= 4.3.2-2ubuntu1), libpgm-dev (>= 5.2.122~dfsg), libsodium-dev, libnorm-dev, libkrb5-dev

$ apt list --installed | grep zmq

libzmq3-dev/focal,now 4.3.2-2ubuntu1 amd64 [installed]
libzmq5/focal,now 4.3.2-2ubuntu1 amd64 [installed,automatic]

[facontidavide]

Don't worry, I can fix this myself later