Skip to content

Commit c673a4b

Browse files
ajtudelaajtudela
authored
Enable Groot2 monitoring (#5065)
* Revert removing live groot monitoring from Nav2 (#2696) Signed-off-by: Alberto Tudela <[email protected]> * Update to Groot2 Signed-off-by: Alberto Tudela <[email protected]> * Added JSON conversions Signed-off-by: Alberto Tudela <[email protected]> * Fix rebase Signed-off-by: Alberto Tudela <[email protected]> * Update to nav_msgs::Goals Signed-off-by: Alberto Tudela <[email protected]> * Added nav_msgs to json utils Signed-off-by: Alberto Tudela <[email protected]> * Add register to types Signed-off-by: Alberto Tudela <[email protected]> * Fix null-dereference Signed-off-by: Alberto Tudela <[email protected]> * Added Json test Signed-off-by: Alberto Tudela <[email protected]> * Fix some tests Signed-off-by: Alberto Tudela <[email protected]> * Fix flake Signed-off-by: Alberto Tudela <[email protected]> * Update package dependency Signed-off-by: Alberto Tudela <[email protected]> * Minor fixes Signed-off-by: Alberto Tudela <[email protected]> * Fix test Signed-off-by: Alberto Tudela <[email protected]> * Rename groot_publisher_port parameter to groot_server_port Signed-off-by: Alberto Tudela <[email protected]> * Minor fix in tst Signed-off-by: Alberto Tudela <[email protected]> * Added JSON for waypoint_status Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]>
1 parent b815d16 commit c673a4b

40 files changed

+1749
-85
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222

2323
#include "behaviortree_cpp/behavior_tree.h"
2424
#include "behaviortree_cpp/bt_factory.h"
25+
#include "behaviortree_cpp/loggers/groot2_publisher.h"
2526
#include "behaviortree_cpp/xml_parsing.h"
2627

2728
#include "rclcpp/rclcpp.hpp"
@@ -85,6 +86,18 @@ class BehaviorTreeEngine
8586
const std::string & file_path,
8687
BT::Blackboard::Ptr blackboard);
8788

89+
/**
90+
* @brief Add Groot2 monitor to publish BT status changes
91+
* @param tree BT to monitor
92+
* @param publisher_port Publisher port for the Groot2 monitor
93+
*/
94+
void addGrootMonitoring(BT::Tree * tree, uint16_t publisher_port);
95+
96+
/**
97+
* @brief Reset groot monitor
98+
*/
99+
void resetGrootMonitor();
100+
88101
/**
89102
* @brief Function to explicitly reset all BT nodes to initial state
90103
* @param tree Tree to halt
@@ -97,6 +110,9 @@ class BehaviorTreeEngine
97110

98111
// Clock
99112
rclcpp::Clock::SharedPtr clock_;
113+
114+
// Groot2 monitor
115+
std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
100116
};
101117

102118
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,11 @@
2020
#include <chrono>
2121

2222
#include "behaviortree_cpp/action_node.h"
23+
#include "behaviortree_cpp/json_export.h"
2324
#include "nav2_util/node_utils.hpp"
2425
#include "rclcpp_action/rclcpp_action.hpp"
2526
#include "nav2_behavior_tree/bt_utils.hpp"
27+
#include "nav2_behavior_tree/json_utils.hpp"
2628

2729
namespace nav2_behavior_tree
2830
{

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,13 @@ class BtActionServer
9191
*/
9292
bool on_cleanup();
9393

94+
/**
95+
* @brief Enable (or disable) Groot2 monitoring of BT
96+
* @param enable Groot2 monitoring
97+
* @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
98+
*/
99+
void setGrootMonitoring(const bool enable, const unsigned server_port);
100+
94101
/**
95102
* @brief Replace current BT with another one
96103
* @param bt_xml_filename The file containing the new BT, uses default filename if empty
@@ -277,6 +284,10 @@ class BtActionServer
277284
// should the BT be reloaded even if the same xml filename is requested?
278285
bool always_reload_bt_xml_ = false;
279286

287+
// Parameters for Groot2 monitoring
288+
bool enable_groot_monitoring_ = true;
289+
int groot_server_port_ = 1667;
290+
280291
// User-provided callbacks
281292
OnGoalReceivedCallback on_goal_received_callback_;
282293
OnLoopCallback on_loop_callback_;

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -239,10 +239,18 @@ bool BtActionServer<ActionT>::on_cleanup()
239239
current_bt_xml_filename_.clear();
240240
blackboard_.reset();
241241
bt_->haltAllActions(tree_);
242+
bt_->resetGrootMonitor();
242243
bt_.reset();
243244
return true;
244245
}
245246

247+
template<class ActionT>
248+
void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsigned server_port)
249+
{
250+
enable_groot_monitoring_ = enable;
251+
groot_server_port_ = server_port;
252+
}
253+
246254
template<class ActionT>
247255
bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
248256
{
@@ -255,6 +263,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
255263
return true;
256264
}
257265

266+
// if a new tree is created, than the Groot2 Publisher must be destroyed
267+
bt_->resetGrootMonitor();
268+
258269
// Read the input BT XML from the specified file into a string
259270
std::ifstream xml_file(filename);
260271

@@ -285,6 +296,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
285296
topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
286297

287298
current_bt_xml_filename_ = filename;
299+
300+
// Enable monitoring with Groot2
301+
if (enable_groot_monitoring_) {
302+
bt_->addGrootMonitoring(&tree_, groot_server_port_);
303+
RCLCPP_DEBUG(
304+
logger_, "Enabling Groot2 monitoring for %s: %d",
305+
action_name_.c_str(), groot_server_port_);
306+
}
307+
288308
return true;
289309
}
290310

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,11 @@
2020
#include <chrono>
2121

2222
#include "behaviortree_cpp/action_node.h"
23+
#include "behaviortree_cpp/json_export.h"
2324
#include "nav2_util/node_utils.hpp"
2425
#include "rclcpp/rclcpp.hpp"
2526
#include "nav2_behavior_tree/bt_utils.hpp"
27+
#include "nav2_behavior_tree/json_utils.hpp"
2628
#include "nav2_util/service_client.hpp"
2729

2830
namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,13 @@ namespace BT
4343
template<>
4444
inline geometry_msgs::msg::Point convertFromString(const StringView key)
4545
{
46+
// if string starts with "json:{", try to parse it as json
47+
if (StartWith(key, "json:")) {
48+
auto new_key = key;
49+
new_key.remove_prefix(5);
50+
return convertFromJSON<geometry_msgs::msg::Point>(new_key);
51+
}
52+
4653
// three real numbers separated by semicolons
4754
auto parts = BT::splitString(key, ';');
4855
if (parts.size() != 3) {
@@ -64,6 +71,13 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key)
6471
template<>
6572
inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
6673
{
74+
// if string starts with "json:{", try to parse it as json
75+
if (StartWith(key, "json:")) {
76+
auto new_key = key;
77+
new_key.remove_prefix(5);
78+
return convertFromJSON<geometry_msgs::msg::Quaternion>(new_key);
79+
}
80+
6781
// four real numbers separated by semicolons
6882
auto parts = BT::splitString(key, ';');
6983
if (parts.size() != 4) {
@@ -86,6 +100,13 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
86100
template<>
87101
inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
88102
{
103+
// if string starts with "json:{", try to parse it as json
104+
if (StartWith(key, "json:")) {
105+
auto new_key = key;
106+
new_key.remove_prefix(5);
107+
return convertFromJSON<geometry_msgs::msg::PoseStamped>(new_key);
108+
}
109+
89110
// 7 real numbers separated by semicolons
90111
auto parts = BT::splitString(key, ';');
91112
if (parts.size() != 9) {
@@ -113,6 +134,13 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
113134
template<>
114135
inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
115136
{
137+
// if string starts with "json:{", try to parse it as json
138+
if (StartWith(key, "json:")) {
139+
auto new_key = key;
140+
new_key.remove_prefix(5);
141+
return convertFromJSON<std::vector<geometry_msgs::msg::PoseStamped>>(new_key);
142+
}
143+
116144
auto parts = BT::splitString(key, ';');
117145
if (parts.size() % 9 != 0) {
118146
throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
@@ -143,6 +171,13 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
143171
template<>
144172
inline nav_msgs::msg::Goals convertFromString(const StringView key)
145173
{
174+
// if string starts with "json:{", try to parse it as json
175+
if (StartWith(key, "json:")) {
176+
auto new_key = key;
177+
new_key.remove_prefix(5);
178+
return convertFromJSON<nav_msgs::msg::Goals>(new_key);
179+
}
180+
146181
auto parts = BT::splitString(key, ';');
147182
if ((parts.size() - 2) % 9 != 0) {
148183
throw std::runtime_error("invalid number of fields for Goals attribute)");
@@ -175,6 +210,13 @@ inline nav_msgs::msg::Goals convertFromString(const StringView key)
175210
template<>
176211
inline nav_msgs::msg::Path convertFromString(const StringView key)
177212
{
213+
// if string starts with "json:{", try to parse it as json
214+
if (StartWith(key, "json:")) {
215+
auto new_key = key;
216+
new_key.remove_prefix(5);
217+
return convertFromJSON<nav_msgs::msg::Path>(new_key);
218+
}
219+
178220
auto parts = BT::splitString(key, ';');
179221
if ((parts.size() - 2) % 9 != 0) {
180222
throw std::runtime_error("invalid number of fields for Path attribute)");
@@ -207,6 +249,13 @@ inline nav_msgs::msg::Path convertFromString(const StringView key)
207249
template<>
208250
inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
209251
{
252+
// if string starts with "json:{", try to parse it as json
253+
if (StartWith(key, "json:")) {
254+
auto new_key = key;
255+
new_key.remove_prefix(5);
256+
return convertFromJSON<std::chrono::milliseconds>(new_key);
257+
}
258+
210259
return std::chrono::milliseconds(std::stoul(key.data()));
211260
}
212261

‎nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
// Copyright (c) 2025 Alberto J. Tudela Roldán
2+
// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
17+
#define NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
18+
19+
#include <string>
20+
#include <set>
21+
#include <vector>
22+
23+
#include "rclcpp/time.hpp"
24+
#include "rclcpp/node.hpp"
25+
#include "behaviortree_cpp/json_export.h"
26+
#include "geometry_msgs/msg/point.hpp"
27+
#include "geometry_msgs/msg/pose_stamped.hpp"
28+
#include "geometry_msgs/msg/quaternion.hpp"
29+
#include "nav_msgs/msg/goals.hpp"
30+
#include "nav_msgs/msg/path.hpp"
31+
#include "nav2_msgs/msg/waypoint_status.hpp"
32+
33+
// The follow templates are required when using Groot 2 to visualize the BT. They
34+
// convert the data types into JSON format easy for visualization.
35+
36+
namespace builtin_interfaces::msg
37+
{
38+
39+
BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg)
40+
{
41+
add_field("sec", &msg.sec);
42+
add_field("nanosec", &msg.nanosec);
43+
}
44+
45+
} // namespace builtin_interfaces::msg
46+
47+
namespace std_msgs::msg
48+
{
49+
50+
BT_JSON_CONVERTER(std_msgs::msg::Header, msg)
51+
{
52+
add_field("stamp", &msg.stamp);
53+
add_field("frame_id", &msg.frame_id);
54+
}
55+
56+
} // namespace std_msgs::msg
57+
58+
namespace geometry_msgs::msg
59+
{
60+
61+
BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg)
62+
{
63+
add_field("x", &msg.x);
64+
add_field("y", &msg.y);
65+
add_field("z", &msg.z);
66+
}
67+
68+
BT_JSON_CONVERTER(geometry_msgs::msg::Quaternion, msg)
69+
{
70+
add_field("x", &msg.x);
71+
add_field("y", &msg.y);
72+
add_field("z", &msg.z);
73+
add_field("w", &msg.w);
74+
}
75+
76+
BT_JSON_CONVERTER(geometry_msgs::msg::Pose, msg)
77+
{
78+
add_field("position", &msg.position);
79+
add_field("orientation", &msg.orientation);
80+
}
81+
82+
BT_JSON_CONVERTER(geometry_msgs::msg::PoseStamped, msg)
83+
{
84+
add_field("header", &msg.header);
85+
add_field("pose", &msg.pose);
86+
}
87+
88+
} // namespace geometry_msgs::msg
89+
90+
namespace nav_msgs::msg
91+
{
92+
93+
BT_JSON_CONVERTER(nav_msgs::msg::Goals, msg)
94+
{
95+
add_field("header", &msg.header);
96+
add_field("goals", &msg.goals);
97+
}
98+
99+
BT_JSON_CONVERTER(nav_msgs::msg::Path, msg)
100+
{
101+
add_field("header", &msg.header);
102+
add_field("poses", &msg.poses);
103+
}
104+
105+
} // namespace nav_msgs::msg
106+
107+
namespace nav2_msgs::msg
108+
{
109+
110+
BT_JSON_CONVERTER(nav2_msgs::msg::WaypointStatus, msg)
111+
{
112+
add_field("waypoint_status", &msg.waypoint_status);
113+
add_field("waypoint_index", &msg.waypoint_index);
114+
add_field("waypoint_pose", &msg.waypoint_pose);
115+
add_field("error_code", &msg.error_code);
116+
add_field("error_msg", &msg.error_msg);
117+
}
118+
119+
} // namespace nav2_msgs::msg
120+
121+
namespace std
122+
{
123+
124+
inline void from_json(const nlohmann::json & js, std::chrono::milliseconds & dest)
125+
{
126+
if (js.contains("ms")) {
127+
dest = std::chrono::milliseconds(js.at("ms").get<int>());
128+
} else {
129+
throw std::runtime_error("Invalid JSON for std::chrono::milliseconds");
130+
}
131+
}
132+
133+
inline void to_json(nlohmann::json & js, const std::chrono::milliseconds & src)
134+
{
135+
js["__type"] = "std::chrono::milliseconds";
136+
js["ms"] = src.count();
137+
}
138+
139+
} // namespace std
140+
141+
#endif // NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,10 @@ class ComputePathThroughPosesAction
7878
*/
7979
static BT::PortsList providedPorts()
8080
{
81+
// Register JSON definitions for the types used in the ports
82+
BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
83+
BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
84+
8185
return providedBasicPorts(
8286
{
8387
BT::InputPort<nav_msgs::msg::Goals>(

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,10 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
7575
*/
7676
static BT::PortsList providedPorts()
7777
{
78+
// Register JSON definitions for the types used in the ports
79+
BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
80+
BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
81+
7882
return providedBasicPorts(
7983
{
8084
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),

0 commit comments

Comments
 (0)