Provide behavior tree current node on a ROS topic

[crdelsey]

Feature request

Feature description

For debugging issues, it would be helpful to know the currently active nodes in the behavior tree. Providing this information on a ROS topic would be the preferred way to get this data.

Implementation considerations

A behavior tree potentially goes through each bt node on every tick of the tree. What is the best representation of the tree to understand what is happening. Perhaps on each tick it would list the state of each node?

[mjeronimo]

I think one could write a ROS logger for the BT library, similar to these:

https://github.com/BehaviorTree/BehaviorTree.CPP/tree/master/src/loggers

Which derive from this base class:

https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp/loggers/abstract_logger.h#L17

Then, the logger is used from the application, like this:

https://github.com/BehaviorTree/BehaviorTree.CPP/blob/36e614e2eada5c200efe6fc83fd31f69e62c33ac/examples/t05_crossdoor.cpp#L72

[cwbollinger]

I mentioned I'd be looking at this last Thursday, and figured I should give an update on my progress.

I've been trying to build various branches of navigation2 for the last couple days but have had limited success. I did some investigation into the issues and modified Mike's staging branch to get something that at least builds here:
https://github.com/cwbollinger/navigation2/tree/param_init_refactor

Questions:

1. Is there a branch that I'm missing that builds currently?
2. Is this even close to the correct way to set parameters with the new API? I don't really understand the functional difference between declare_parameter("name", value) which seems to be removed, and set_parameters({rclcpp::Parameter("name", value)}) which builds just fine.

If these changes are approximately correct, I'll work off of this just to get a prototype started. As far as the actual logging goes, the BT loggers seem well documented.

[crdelsey]

declare_parameter("name", value) which seems to be removed

This was recently added to the ROS 2 framework in the rclcpp package. Since you're thinking it was removed, does that mean you are using the either the ROS 2 crystal release or building from the ROS 2 crystal branch?

Is there a branch that I'm missing that builds currently?

We've got two main branches:

1. crystal-devel which builds with ROS2 crystal release. It is pretty old and out of date at this point. We've made a lot of important fixes since then and they've not been backported since the dashing release is almost here.
2. master which builds against the ROS2 master. Unfortunately, they've recently made an API change that has broken us. We've got a fix in Add default NodeOptions to fix new parameter changes #677 but it isn't quite merged yet. You could work off that PR branch or merge it in to your master as a short term workaround.

[mjeronimo]

@cwbollinger This is the branch that I'm actively working in: https://github.com/mjeronimo/navigation2/tree/lifecycle-staging

I've done the lifecycle node work here as well as updated for ROS2 actions and have adapted to the recent ROS2 parameter changes (one now has to explicitly declare parameters). I keep this branch working as I'm working on it every day and frequently commit changes. This is the code that we're currently working to integrate into the main repo, first into the 'lifecycle' branch and then into the master. So, if you want to work with my lifecycle-staging branch, I can help if you have any questions or trouble getting it going.

[cwbollinger]

Hi @mjeronimo, thanks for the direction. I have been able to build a branch based on PR #677 and, since that has merged, master branch. I am having some trouble with the branch you linked (commit 13b9379) while building nav2_map_server:

/home/olorin/navigation2_ws/src/navigation2/nav2_map_server/src/map_server.cpp: In constructor ‘nav2_map_server::MapServer::MapServer()’: /home/olorin/navigation2_ws/src/navigation2/nav2_map_server/src/map_server.cpp:36:3: error: ‘declare_parameter’ was not declared in this scope declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml"))); ^~~~~~~~~~~~~~~~~ /home/olorin/navigation2_ws/src/navigation2/nav2_map_server/src/map_server.cpp:36:3: note: suggested alternative: ‘describe_parameters’ declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml"))); ^~~~~~~~~~~~~~~~~ describe_parameters

I have double checked that I'm able to build the nav2 master branch with my current ros2 checkout, so I am not sure why declare_parameter is not found. Any thoughts?

[mjeronimo]

@cwbollinger There were some changes recently in the rclcpp layer such that parameters have to be explicitly declared. We've worked around this in the master branch using a flag to the NodeOptions class to automatically declare parameters. However, in the lifecycle branch, I've attempted to adapt to this change and declare every parameter using the new API.

As you probably know, the node and lifecycle node classes have several interface pointers internally that they use (node_parameters_interface, node_topics_interface, etc.). There are also "wrapper" functions in the node and lifecycle_node header files that are usually templates and often provide a simplified interface over directly using the interface pointer. Unfortunately, with this latest change, the wrapper functions were not also added to the lifecycle node class. I've entered a PR for this in rclcpp: ros2/rclcpp#707

Meanwhile, to get my lifecycle-staging branch to build, you'd have to use my rclcpp ("add-missing-parameter-templates" branch), which has this change: https://github.com/mjeronimo/rclcpp/tree/add-missing-parameter-templates. I'll try to fix this some other way so people don't have to use my version of rclcpp.

[crdelsey]

@cwbollinger Since master is building again, I'd just work there unless you need some of the lifecycle or other changes that @mjeronimo has been working on.

Everything we're built on top of keeps moving, so things break from time to time. No guarantees that it will work tomorrow, but we try to stay on top of it. If things break for you, let us know and we'll help out.