diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index a0d86649c00..db5f938a9fe 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -22,6 +22,7 @@ #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/loggers/groot2_publisher.h" #include "behaviortree_cpp/xml_parsing.h" #include "rclcpp/rclcpp.hpp" @@ -85,6 +86,18 @@ class BehaviorTreeEngine const std::string & file_path, BT::Blackboard::Ptr blackboard); + /** + * @brief Add Groot2 monitor to publish BT status changes + * @param tree BT to monitor + * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port) + */ + void addGrootMonitoring(BT::Tree * tree, uint16_t server_port); + + /** + * @brief Reset groot monitor + */ + void resetGrootMonitor(); + /** * @brief Function to explicitly reset all BT nodes to initial state * @param tree Tree to halt @@ -97,6 +110,9 @@ class BehaviorTreeEngine // Clock rclcpp::Clock::SharedPtr clock_; + + // Groot2 monitor + std::unique_ptr groot_monitor_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 357903df7dc..a56ae31b8b6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -20,9 +20,11 @@ #include #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/json_export.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 2b323a67985..e61dcefae8b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -87,6 +87,13 @@ class BtActionServer */ bool on_cleanup(); + /** + * @brief Enable (or disable) Groot2 monitoring of BT + * @param enable Groot2 monitoring + * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port) + */ + void setGrootMonitoring(const bool enable, const unsigned server_port); + /** * @brief Replace current BT with another one * @param bt_xml_filename The file containing the new BT, uses default filename if empty @@ -254,6 +261,10 @@ class BtActionServer // should the BT be reloaded even if the same xml filename is requested? bool always_reload_bt_xml_ = false; + // Parameters for Groot2 monitoring + bool enable_groot_monitoring_ = false; + int groot_server_port_ = 1667; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index b19a2f17a96..ac5fc20da41 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -217,10 +217,18 @@ bool BtActionServer::on_cleanup() current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_); + bt_->resetGrootMonitor(); bt_.reset(); return true; } +template +void BtActionServer::setGrootMonitoring(const bool enable, const unsigned server_port) +{ + enable_groot_monitoring_ = enable; + groot_server_port_ = server_port; +} + template bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) { @@ -233,6 +241,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena return true; } + // Reset any existing Groot2 monitoring + bt_->resetGrootMonitor(); + // Read the input BT XML from the specified file into a string std::ifstream xml_file(filename); @@ -261,6 +272,14 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; + + if (enable_groot_monitoring_) { + bt_->addGrootMonitoring(&tree_, groot_server_port_); + RCLCPP_DEBUG( + logger_, "Enabling Groot2 monitoring for %s: %d", + action_name_.c_str(), groot_server_port_); + } + return true; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index d917f711a66..d2149945ad0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -20,9 +20,11 @@ #include #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/json_export.h" #include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 91e3e241b91..9907a955dff 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -42,6 +42,13 @@ namespace BT template<> inline geometry_msgs::msg::Point convertFromString(const StringView key) { + // if string starts with "json:{", try to parse it as json + if (StartWith(key, "json:")) { + auto new_key = key; + new_key.remove_prefix(5); + return convertFromJSON(new_key); + } + // three real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() != 3) { @@ -63,6 +70,13 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key) template<> inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) { + // if string starts with "json:{", try to parse it as json + if (StartWith(key, "json:")) { + auto new_key = key; + new_key.remove_prefix(5); + return convertFromJSON(new_key); + } + // four real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() != 4) { @@ -85,6 +99,13 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) template<> inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) { + // if string starts with "json:{", try to parse it as json + if (StartWith(key, "json:")) { + auto new_key = key; + new_key.remove_prefix(5); + return convertFromJSON(new_key); + } + // 7 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() != 9) { @@ -143,6 +164,13 @@ inline std::vector convertFromString(const Stri template<> inline nav_msgs::msg::Path convertFromString(const StringView key) { + // if string starts with "json:{", try to parse it as json + if (StartWith(key, "json:")) { + auto new_key = key; + new_key.remove_prefix(5); + return convertFromJSON(new_key); + } + // 9 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if ((parts.size() - 2) % 9 != 0) { @@ -176,6 +204,12 @@ inline nav_msgs::msg::Path convertFromString(const StringView key) template<> inline std::chrono::milliseconds convertFromString(const StringView key) { + // if string starts with "json:{", try to parse it as json + if (StartWith(key, "json:")) { + auto new_key = key; + new_key.remove_prefix(5); + return convertFromJSON(new_key); + } return std::chrono::milliseconds(std::stoul(key.data())); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp new file mode 100644 index 00000000000..ed7a0b6bc84 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp @@ -0,0 +1,127 @@ +// Copyright (c) 2025 Alberto J. Tudela Roldán +// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_ +#define NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_ + +#include +#include +#include + +#include "rclcpp/time.hpp" +#include "rclcpp/node.hpp" +#include "behaviortree_cpp/json_export.h" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/path.hpp" + +// The follow templates are required when using Groot 2 to visualize the BT. They +// convert the data types into JSON format easy for visualization. + +namespace builtin_interfaces::msg +{ + +BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg) +{ + add_field("sec", &msg.sec); + add_field("nanosec", &msg.nanosec); +} + +} // namespace builtin_interfaces::msg + +namespace std_msgs::msg +{ + +BT_JSON_CONVERTER(std_msgs::msg::Header, msg) +{ + add_field("stamp", &msg.stamp); + add_field("frame_id", &msg.frame_id); +} + +} // namespace std_msgs::msg + +namespace geometry_msgs::msg +{ + +BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg) +{ + add_field("x", &msg.x); + add_field("y", &msg.y); + add_field("z", &msg.z); +} + +BT_JSON_CONVERTER(geometry_msgs::msg::Quaternion, msg) +{ + add_field("x", &msg.x); + add_field("y", &msg.y); + add_field("z", &msg.z); + add_field("w", &msg.w); +} + +BT_JSON_CONVERTER(geometry_msgs::msg::Pose, msg) +{ + add_field("position", &msg.position); + add_field("orientation", &msg.orientation); +} + +BT_JSON_CONVERTER(geometry_msgs::msg::PoseStamped, msg) +{ + add_field("header", &msg.header); + add_field("pose", &msg.pose); +} + +} // namespace geometry_msgs::msg + +namespace nav_msgs::msg +{ + +BT_JSON_CONVERTER(nav_msgs::msg::Goals, msg) +{ + add_field("header", &msg.header); + add_field("goals", &msg.goals); +} + +BT_JSON_CONVERTER(nav_msgs::msg::Path, msg) +{ + add_field("header", &msg.header); + add_field("poses", &msg.poses); +} + +} // namespace nav_msgs::msg + + +namespace std +{ + +inline void from_json(const nlohmann::json & js, std::chrono::milliseconds & dest) +{ + if (js.contains("ms")) { + dest = std::chrono::milliseconds(js.at("ms").get()); + } else { + throw std::runtime_error("Invalid JSON for std::chrono::milliseconds"); + } +} + +inline void to_json(nlohmann::json & js, const std::chrono::milliseconds & src) +{ + js["__type"] = "std::chrono::milliseconds"; + js["ms"] = src.count(); +} + +} // namespace std + +#endif // NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index fe5d2e9dd25..212d82e2ec3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -18,6 +18,7 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav_msgs/msg/path.h" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -73,6 +74,10 @@ class ComputePathThroughPosesAction */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + return providedBasicPorts( { BT::InputPort>( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 1e58e953954..dc90eded486 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -17,6 +17,7 @@ #include +#include "behaviortree_cpp/json_export.h" #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav_msgs/msg/path.h" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -75,6 +76,10 @@ class ComputePathToPoseAction : public BtActionNode(); + BT::RegisterJsonDefinition(); + return providedBasicPorts( { BT::InputPort("goal", "Destination to plan to"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index c05c2c0d59f..8948a55ee8d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -18,6 +18,7 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -78,6 +79,9 @@ class FollowPathAction : public BtActionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return providedBasicPorts( { BT::InputPort("path", "Path to follow"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp index 091360b4795..96e4606a5a2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -19,10 +19,12 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" +#include "nav2_behavior_tree/json_utils.hpp" #include "nav_msgs/msg/path.h" namespace nav2_behavior_tree @@ -38,6 +40,10 @@ class GetPoseFromPath : public BT::ActionNodeBase static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + return { BT::InputPort("path", "Path to extract pose from"), BT::OutputPort("pose", "Stamped Extracted Pose"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 44ad055ea24..730fc27f238 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,6 +18,7 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" @@ -72,6 +73,9 @@ class NavigateThroughPosesAction : public BtActionNode>(); + return providedBasicPorts( { BT::InputPort>( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 849c8259a48..ff523d28c24 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -17,6 +17,7 @@ #include +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -71,6 +72,9 @@ class NavigateToPoseAction : public BtActionNode(); + return providedBasicPorts( { BT::InputPort("goal", "Destination to plan to"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index fcfade6d17b..78e4e4c0e5c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,11 +19,13 @@ #include #include +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp index d5ec4ced047..93328c793ca 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -18,6 +18,7 @@ #include +#include "behaviortree_cpp/json_export.h" #include "nav2_msgs/action/smooth_path.hpp" #include "nav_msgs/msg/path.h" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -71,6 +72,9 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode(); + return providedBasicPorts( { BT::InputPort("unsmoothed_path", "Path to be smoothed"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index 5501d49cf32..a820f448d17 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -22,6 +22,8 @@ #include "nav_msgs/msg/path.hpp" #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/json_export.h" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -47,6 +49,9 @@ class TruncatePath : public BT::ActionNodeBase */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return { BT::InputPort("input_path", "Original Path"), BT::OutputPort("output_path", "Path truncated to a certain distance"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 94b4b66ca2a..4cd80f9a0db 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -23,6 +23,8 @@ #include "nav_msgs/msg/path.hpp" #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/json_export.h" +#include "nav2_behavior_tree/json_utils.hpp" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree @@ -49,6 +51,10 @@ class TruncatePathLocal : public BT::ActionNodeBase */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + return { BT::InputPort("input_path", "Original Path"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 12344a5d3f7..44194ea85fc 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -20,9 +20,11 @@ #include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/json_export.h" #include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree @@ -58,6 +60,10 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); + return { BT::InputPort>( "goals", "Vector of navigation goals"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index ff65da09a18..aaef3ea12c3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -20,8 +20,10 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" #include "tf2_ros/buffer.h" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -72,6 +74,9 @@ class GoalReachedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return { BT::InputPort("goal", "Destination"), BT::InputPort("global_frame", "Global frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 22893e33ee3..5cc5ddcfff7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,8 +19,10 @@ #include #include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -55,6 +57,10 @@ class GoalUpdatedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); + return { BT::InputPort>( "goals", "Vector of navigation goals"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a22942f9a56..1b6fbe7b2eb 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -20,8 +20,10 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -61,6 +63,10 @@ class IsPathValidCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); + return { BT::InputPort("path", "Path to Check"), BT::InputPort("server_timeout") diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index 5e770b4bbd6..0460cdf3080 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -19,8 +19,10 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/json_export.h" #include "nav_msgs/msg/path.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -55,6 +57,9 @@ class PathExpiringTimerCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return { BT::InputPort("seconds", 1.0, "Seconds"), BT::InputPort("path") diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 63a7f606558..05e77144693 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -20,10 +20,12 @@ #include #include "behaviortree_cpp/decorator_node.h" +#include "behaviortree_cpp/json_export.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -49,6 +51,10 @@ class GoalUpdatedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); + return { BT::InputPort>( "goals", "Vector of navigation goals"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index ddce12cf02e..5ccbfc85d79 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -23,6 +23,8 @@ #include "behaviortree_cpp/decorator_node.h" +#include "behaviortree_cpp/json_export.h" +#include "nav2_behavior_tree/json_utils.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree @@ -50,6 +52,9 @@ class GoalUpdater : public BT::DecoratorNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return { BT::InputPort("input_goal", "Original Goal"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp index bdf4a080d18..ab0f2a05933 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -22,6 +22,8 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" #include "behaviortree_cpp/decorator_node.h" +#include "behaviortree_cpp/json_export.h" +#include "nav2_behavior_tree/json_utils.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree @@ -49,6 +51,9 @@ class PathLongerOnApproach : public BT::DecoratorNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return { BT::InputPort("path", "Planned Path"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 59a9fc55210..f1497bc685b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -25,7 +25,9 @@ #include "nav2_util/odometry_utils.hpp" #include "behaviortree_cpp/decorator_node.h" +#include "behaviortree_cpp/json_export.h" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -53,6 +55,10 @@ class SpeedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition>(); + return { BT::InputPort("min_rate", 0.1, "Minimum rate"), BT::InputPort("max_rate", 1.0, "Maximum rate"), diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 70a192d7196..b18bdef4f46 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -20,7 +20,9 @@ #include #include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/json_export.h" #include "behaviortree_cpp/utils/shared_library.h" +#include "nav2_behavior_tree/json_utils.hpp" namespace nav2_behavior_tree { @@ -98,6 +100,27 @@ BehaviorTreeEngine::createTreeFromFile( return factory_.createTreeFromFile(file_path, blackboard); } +void +BehaviorTreeEngine::addGrootMonitoring( + BT::Tree * tree, + uint16_t server_port) +{ + // This logger publish status changes using Groot2 + groot_monitor_ = std::make_unique(*tree, server_port); + + // Register common types JSON definitions + BT::RegisterJsonDefinition(); + BT::RegisterJsonDefinition(); +} + +void +BehaviorTreeEngine::resetGrootMonitor() +{ + if (groot_monitor_) { + 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::Tree & tree) diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt index eff6e78c5db..82b928526c9 100644 --- a/nav2_behavior_tree/test/CMakeLists.txt +++ b/nav2_behavior_tree/test/CMakeLists.txt @@ -1,6 +1,11 @@ ament_add_gtest(test_bt_utils test_bt_utils.cpp) ament_target_dependencies(test_bt_utils ${dependencies}) +ament_add_gtest(test_json_utils test_json_utils.cpp) +target_link_libraries(test_json_utils + ${library_name} + ${geometry_msgs_TARGETS}) + include_directories(.) add_subdirectory(plugins/condition) diff --git a/nav2_behavior_tree/test/test_json_utils.cpp b/nav2_behavior_tree/test/test_json_utils.cpp new file mode 100644 index 00000000000..7a72e623f82 --- /dev/null +++ b/nav2_behavior_tree/test/test_json_utils.cpp @@ -0,0 +1,832 @@ +// Copyright (c) 2025 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "behaviortree_cpp/json_export.h" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/json_utils.hpp" + + +class JsonTest : public testing::Test +{ +protected: + JsonTest() + { + BT::JsonExporter & exporter = BT::JsonExporter::get(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter(); + exporter.addConverter>(); + exporter.addConverter(); + exporter.addConverter(); + } +}; + +TEST_F(JsonTest, test_milliseconds) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + std::chrono::milliseconds milliseconds_test(1000); + + nlohmann::json json; + exporter.toJson(BT::Any(milliseconds_test), json["ms"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["ms"]["__type"], "std::chrono::milliseconds"); + ASSERT_EQ(json["ms"]["ms"], 1000); + + // Check the two-ways transform, i.e. "from_json" + auto milliseconds_test2 = + exporter.fromJson(json["ms"])->first.cast(); + + ASSERT_EQ(milliseconds_test.count(), milliseconds_test2.count()); + + // Convert from string + std::chrono::milliseconds milliseconds_test3; + auto const test_json = R"(json:{"__type": "std::chrono::milliseconds", "ms": 1000})"; + ASSERT_NO_THROW(milliseconds_test3 = BT::convertFromString(test_json)); + + ASSERT_EQ(milliseconds_test.count(), milliseconds_test3.count()); +} + +TEST_F(JsonTest, test_time) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + builtin_interfaces::msg::Time time_test; + time_test.sec = 1; + time_test.nanosec = 2; + + nlohmann::json json; + exporter.toJson(BT::Any(time_test), json["time"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["time"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["time"]["sec"], 1); + ASSERT_EQ(json["time"]["nanosec"], 2); + + // Check the two-ways transform, i.e. "from_json" + auto time_test2 = exporter.fromJson(json["time"])->first.cast(); + + ASSERT_EQ(time_test.sec, time_test2.sec); + ASSERT_EQ(time_test.nanosec, time_test2.nanosec); + + // Convert from string + builtin_interfaces::msg::Time time_test3; + auto const test_json = + R"(json:{"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2})"; + ASSERT_NO_THROW(time_test3 = BT::convertFromString(test_json)); + + ASSERT_EQ(time_test.sec, time_test3.sec); + ASSERT_EQ(time_test.nanosec, time_test3.nanosec); +} + +TEST_F(JsonTest, test_header) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + std_msgs::msg::Header header_test; + header_test.stamp.sec = 1; + header_test.stamp.nanosec = 2; + header_test.frame_id = "map"; + + nlohmann::json json; + exporter.toJson(BT::Any(header_test), json["header"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["header"]["stamp"]["sec"], 1); + ASSERT_EQ(json["header"]["stamp"]["nanosec"], 2); + ASSERT_EQ(json["header"]["frame_id"], "map"); + + // Check the two-ways transform, i.e. "from_json" + auto header_test2 = exporter.fromJson(json["header"])->first.cast(); + + ASSERT_EQ(header_test.stamp.sec, header_test2.stamp.sec); + ASSERT_EQ(header_test.stamp.nanosec, header_test2.stamp.nanosec); + ASSERT_EQ(header_test.frame_id, header_test2.frame_id); + + // Convert from string + std_msgs::msg::Header header_test3; + auto const test_json = + R"(json: + { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2}, + "frame_id": "map" + } + )"; + ASSERT_NO_THROW(header_test3 = BT::convertFromString(test_json)); + ASSERT_EQ(header_test.stamp.sec, header_test3.stamp.sec); + ASSERT_EQ(header_test.stamp.nanosec, header_test3.stamp.nanosec); + ASSERT_EQ(header_test.frame_id, header_test3.frame_id); +} + +TEST_F(JsonTest, test_point) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + geometry_msgs::msg::Point point_test; + point_test.x = 1.0; + point_test.y = 2.0; + point_test.z = 3.0; + + nlohmann::json json; + exporter.toJson(BT::Any(point_test), json["point"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["point"]["__type"], "geometry_msgs::msg::Point"); + ASSERT_EQ(json["point"]["x"], 1.0); + ASSERT_EQ(json["point"]["y"], 2.0); + ASSERT_EQ(json["point"]["z"], 3.0); + + // Check the two-ways transform, i.e. "from_json" + auto point_test2 = exporter.fromJson(json["point"])->first.cast(); + + ASSERT_EQ(point_test.x, point_test2.x); + ASSERT_EQ(point_test.y, point_test2.y); + ASSERT_EQ(point_test.z, point_test2.z); + + // Convert from string + geometry_msgs::msg::Point point_test3; + auto const test_json = + R"(json: + { + "__type": "geometry_msgs::msg::Point", + "x": 1.0, "y": 2.0, "z": 3.0 + } + )"; + ASSERT_NO_THROW(point_test3 = BT::convertFromString(test_json)); + ASSERT_EQ(point_test.x, point_test3.x); + ASSERT_EQ(point_test.y, point_test3.y); + ASSERT_EQ(point_test.z, point_test3.z); +} + +TEST_F(JsonTest, test_quaternion) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + geometry_msgs::msg::Quaternion quaternion_test; + quaternion_test.x = 1.0; + quaternion_test.y = 2.0; + quaternion_test.z = 3.0; + quaternion_test.w = 4.0; + + nlohmann::json json; + exporter.toJson(BT::Any(quaternion_test), json["quaternion"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["quaternion"]["__type"], "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["quaternion"]["x"], 1.0); + ASSERT_EQ(json["quaternion"]["y"], 2.0); + ASSERT_EQ(json["quaternion"]["z"], 3.0); + ASSERT_EQ(json["quaternion"]["w"], 4.0); + + // Check the two-ways transform, i.e. "from_json" + auto quaternion_test2 = + exporter.fromJson(json["quaternion"])->first.cast(); + + ASSERT_EQ(quaternion_test.x, quaternion_test2.x); + ASSERT_EQ(quaternion_test.y, quaternion_test2.y); + ASSERT_EQ(quaternion_test.z, quaternion_test2.z); + ASSERT_EQ(quaternion_test.w, quaternion_test2.w); + + // Convert from string + geometry_msgs::msg::Quaternion quaternion_test3; + auto const test_json = + R"(json: + { + "__type": "geometry_msgs::msg::Quaternion", + "x": 1.0, "y": 2.0, "z": 3.0, "w": 4.0 + } + )"; + ASSERT_NO_THROW(quaternion_test3 = + BT::convertFromString(test_json)); + ASSERT_EQ(quaternion_test.x, quaternion_test3.x); + ASSERT_EQ(quaternion_test.y, quaternion_test3.y); + ASSERT_EQ(quaternion_test.z, quaternion_test3.z); + ASSERT_EQ(quaternion_test.w, quaternion_test3.w); +} + +TEST_F(JsonTest, test_pose_stamped) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + geometry_msgs::msg::PoseStamped pose_stamped_test; + pose_stamped_test.header.stamp.sec = 1; + pose_stamped_test.header.stamp.nanosec = 2; + pose_stamped_test.header.frame_id = "map"; + pose_stamped_test.pose.position.x = 3.0; + pose_stamped_test.pose.position.y = 4.0; + pose_stamped_test.pose.position.z = 5.0; + pose_stamped_test.pose.orientation.x = 6.0; + pose_stamped_test.pose.orientation.y = 7.0; + pose_stamped_test.pose.orientation.z = 8.0; + pose_stamped_test.pose.orientation.w = 9.0; + + nlohmann::json json; + exporter.toJson(BT::Any(pose_stamped_test), json["pose"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["pose"]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["pose"]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["pose"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["pose"]["header"]["stamp"]["sec"], 1); + ASSERT_EQ(json["pose"]["header"]["stamp"]["nanosec"], 2); + ASSERT_EQ(json["pose"]["header"]["frame_id"], "map"); + ASSERT_EQ(json["pose"]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["pose"]["pose"]["position"]["__type"], "geometry_msgs::msg::Point"); + ASSERT_EQ(json["pose"]["pose"]["position"]["x"], 3.0); + ASSERT_EQ(json["pose"]["pose"]["position"]["y"], 4.0); + ASSERT_EQ(json["pose"]["pose"]["position"]["z"], 5.0); + ASSERT_EQ(json["pose"]["pose"]["orientation"]["__type"], "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["pose"]["pose"]["orientation"]["x"], 6.0); + ASSERT_EQ(json["pose"]["pose"]["orientation"]["y"], 7.0); + ASSERT_EQ(json["pose"]["pose"]["orientation"]["z"], 8.0); + ASSERT_EQ(json["pose"]["pose"]["orientation"]["w"], 9.0); + + // Check the two-ways transform, i.e. "from_json" + auto pose_stamped_test2 = + exporter.fromJson(json["pose"])->first.cast(); + + ASSERT_EQ(pose_stamped_test.header.stamp.sec, pose_stamped_test2.header.stamp.sec); + ASSERT_EQ(pose_stamped_test.header.stamp.nanosec, pose_stamped_test2.header.stamp.nanosec); + ASSERT_EQ(pose_stamped_test.header.frame_id, pose_stamped_test2.header.frame_id); + ASSERT_EQ(pose_stamped_test.pose.position.x, pose_stamped_test2.pose.position.x); + ASSERT_EQ(pose_stamped_test.pose.position.y, pose_stamped_test2.pose.position.y); + ASSERT_EQ(pose_stamped_test.pose.position.z, pose_stamped_test2.pose.position.z); + ASSERT_EQ(pose_stamped_test.pose.orientation.x, pose_stamped_test2.pose.orientation.x); + ASSERT_EQ(pose_stamped_test.pose.orientation.y, pose_stamped_test2.pose.orientation.y); + ASSERT_EQ(pose_stamped_test.pose.orientation.z, pose_stamped_test2.pose.orientation.z); + ASSERT_EQ(pose_stamped_test.pose.orientation.w, pose_stamped_test2.pose.orientation.w); + + // Convert from string + geometry_msgs::msg::PoseStamped pose_stamped_test3; + auto const test_json = + R"(json: + { + "__type": "geometry_msgs::msg::PoseStamped", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2}, + "frame_id": "map" + }, + "pose": { + "__type": "geometry_msgs::msg::Pose", + "position": { + "__type": "geometry_msgs::msg::Point", + "x": 3.0, "y": 4.0, "z": 5.0 + }, + "orientation": { + "__type": "geometry_msgs::msg::Quaternion", + "x": 6.0, "y": 7.0, "z": 8.0, "w": 9.0 + } + } + } + )"; + ASSERT_NO_THROW(pose_stamped_test3 = + BT::convertFromString(test_json)); + ASSERT_EQ(pose_stamped_test.header, pose_stamped_test3.header); + ASSERT_EQ(pose_stamped_test.pose.position, pose_stamped_test3.pose.position); + ASSERT_EQ(pose_stamped_test.pose.orientation, pose_stamped_test3.pose.orientation); +} + +TEST_F(JsonTest, test_pose_stamped_vector) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + std::vector pose_stamped_vector_test; + pose_stamped_vector_test.resize(2); + pose_stamped_vector_test[0].header.stamp.sec = 1; + pose_stamped_vector_test[0].header.stamp.nanosec = 2; + pose_stamped_vector_test[0].header.frame_id = "map"; + pose_stamped_vector_test[0].pose.position.x = 3.0; + pose_stamped_vector_test[0].pose.position.y = 4.0; + pose_stamped_vector_test[0].pose.position.z = 5.0; + pose_stamped_vector_test[0].pose.orientation.x = 6.0; + pose_stamped_vector_test[0].pose.orientation.y = 7.0; + pose_stamped_vector_test[0].pose.orientation.z = 8.0; + pose_stamped_vector_test[0].pose.orientation.w = 9.0; + + pose_stamped_vector_test[1].header.stamp.sec = 10; + pose_stamped_vector_test[1].header.stamp.nanosec = 11; + pose_stamped_vector_test[1].header.frame_id = "odom"; + pose_stamped_vector_test[1].pose.position.x = 12.0; + pose_stamped_vector_test[1].pose.position.y = 13.0; + pose_stamped_vector_test[1].pose.position.z = 14.0; + pose_stamped_vector_test[1].pose.orientation.x = 15.0; + pose_stamped_vector_test[1].pose.orientation.y = 16.0; + pose_stamped_vector_test[1].pose.orientation.z = 17.0; + pose_stamped_vector_test[1].pose.orientation.w = 18.0; + + nlohmann::json json; + exporter.toJson(BT::Any(pose_stamped_vector_test), json["poses"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["poses"][0]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["poses"][0]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["poses"][0]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["poses"][0]["header"]["stamp"]["sec"], 1); + ASSERT_EQ(json["poses"][0]["header"]["stamp"]["nanosec"], 2); + ASSERT_EQ(json["poses"][0]["header"]["frame_id"], "map"); + ASSERT_EQ(json["poses"][0]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["poses"][0]["pose"]["position"]["__type"], "geometry_msgs::msg::Point"); + ASSERT_EQ(json["poses"][0]["pose"]["position"]["x"], 3.0); + ASSERT_EQ(json["poses"][0]["pose"]["position"]["y"], 4.0); + ASSERT_EQ(json["poses"][0]["pose"]["position"]["z"], 5.0); + ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["x"], 6.0); + ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["y"], 7.0); + ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["z"], 8.0); + ASSERT_EQ(json["poses"][0]["pose"]["orientation"]["w"], 9.0); + ASSERT_EQ(json["poses"][1]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["poses"][1]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["poses"][1]["header"]["stamp"]["__type"], + "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["poses"][1]["header"]["stamp"]["sec"], 10); + ASSERT_EQ(json["poses"][1]["header"]["stamp"]["nanosec"], 11); + ASSERT_EQ(json["poses"][1]["header"]["frame_id"], "odom"); + ASSERT_EQ(json["poses"][1]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["poses"][1]["pose"]["position"]["__type"], + "geometry_msgs::msg::Point"); + ASSERT_EQ(json["poses"][1]["pose"]["position"]["x"], 12.0); + ASSERT_EQ(json["poses"][1]["pose"]["position"]["y"], 13.0); + ASSERT_EQ(json["poses"][1]["pose"]["position"]["z"], 14.0); + ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["x"], 15.0); + ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["y"], 16.0); + ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["z"], 17.0); + ASSERT_EQ(json["poses"][1]["pose"]["orientation"]["w"], 18.0); + + // Check the two-ways transform, i.e. "from_json" + // auto pose_stamped_vector_test2 = + // exporter.fromJson(json["poses"])->first.cast>(); + // ASSERT_EQ(pose_stamped_vector_test[0].header.stamp.sec, + // pose_stamped_vector_test2[0].header.stamp.sec); + // ASSERT_EQ(pose_stamped_vector_test[0].header.stamp.nanosec, + // pose_stamped_vector_test2[0].header.stamp.nanosec); + // ASSERT_EQ(pose_stamped_vector_test[0].header.frame_id, + // pose_stamped_vector_test2[0].header.frame_id); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.x, + // pose_stamped_vector_test2[0].pose.position.x); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.y, + // pose_stamped_vector_test2[0].pose.position.y); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.position.z, + // pose_stamped_vector_test2[0].pose.position.z); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.x, + // pose_stamped_vector_test2[0].pose.orientation.x); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.y, + // pose_stamped_vector_test2[0].pose.orientation.y); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.z, + // pose_stamped_vector_test2[0].pose.orientation.z); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation.w, + // pose_stamped_vector_test2[0].pose.orientation.w); + // ASSERT_EQ(pose_stamped_vector_test[1].header.stamp.sec, + // pose_stamped_vector_test2[1].header.stamp.sec); + // ASSERT_EQ(pose_stamped_vector_test[1].header.stamp.nanosec, + // pose_stamped_vector_test2[1].header.stamp.nanosec); + // ASSERT_EQ(pose_stamped_vector_test[1].header.frame_id, + // pose_stamped_vector_test2[1].header.frame_id); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.x, + // pose_stamped_vector_test2[1].pose.position.x); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.y, + // pose_stamped_vector_test2[1].pose.position.y); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.position.z, + // pose_stamped_vector_test2[1].pose.position.z); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.x, + // pose_stamped_vector_test2[1].pose.orientation.x); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.y, + // pose_stamped_vector_test2[1].pose.orientation.y); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.z, + // pose_stamped_vector_test2[1].pose.orientation.z); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation.w, + // pose_stamped_vector_test2[1].pose.orientation.w); + + // // Convert from string + // std::vector pose_stamped_vector_test3; + // auto const test_json = + // R"(json: + // { + // "poses": [ + // { + // "__type": "geometry_msgs::msg::PoseStamped", + // "header": { + // "__type": "std_msgs::msg::Header", + // "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2}, + // "frame_id": "map" + // }, + // "pose": { + // "__type": "geometry_msgs::msg::Pose", + // "position": { + // "__type": "geometry_msgs::msg::Point", + // "x": 3.0, "y": 4.0, "z": 5.0 + // }, + // "orientation": { + // "__type": "geometry_msgs::msg::Quaternion", + // "x": 6.0, "y": 7.0, "z": 8.0, "w": 9.0 + // } + // } + // }, + // { + // "__type": "geometry_msgs::msg::PoseStamped", + // "header": { + // "__type": "std_msgs::msg::Header", + // "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 10, "nanosec": 11}, + // "frame_id": "odom" + // }, + // "pose": { + // "__type": "geometry_msgs::msg::Pose", + // "position": { + // "__type": "geometry_msgs::msg::Point", + // "x": 12.0, "y": 13.0, "z": 14.0 + // }, + // "orientation": { + // "__type": "geometry_msgs::msg::Quaternion", + // "x": 15.0, "y": 16.0, "z": 17.0, "w": 18.0 + // } + // } + // } + // ] + // } + // )"; + // ASSERT_NO_THROW(pose_stamped_vector_test3 = + // BT::convertFromString>(test_json)); + // ASSERT_EQ(pose_stamped_vector_test[0].header, pose_stamped_vector_test3[0].header); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.position, + // pose_stamped_vector_test3[0].pose.position); + // ASSERT_EQ(pose_stamped_vector_test[0].pose.orientation, + // pose_stamped_vector_test3[0].pose.orientation); + // ASSERT_EQ(pose_stamped_vector_test[1].header, pose_stamped_vector_test3[1].header); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.position, + // pose_stamped_vector_test3[1].pose.position); + // ASSERT_EQ(pose_stamped_vector_test[1].pose.orientation, + // pose_stamped_vector_test3[1].pose.orientation); +} + +TEST_F(JsonTest, test_goals) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + nav_msgs::msg::Goals goals_test; + goals_test.header.stamp.sec = 1; + goals_test.header.stamp.nanosec = 2; + goals_test.header.frame_id = "map"; + goals_test.goals.resize(2); + goals_test.goals[0].header.stamp.sec = 3; + goals_test.goals[0].header.stamp.nanosec = 4; + goals_test.goals[0].header.frame_id = "map"; + goals_test.goals[0].pose.position.x = 5.0; + goals_test.goals[0].pose.position.y = 6.0; + goals_test.goals[0].pose.position.z = 7.0; + goals_test.goals[0].pose.orientation.x = 8.0; + goals_test.goals[0].pose.orientation.y = 9.0; + goals_test.goals[0].pose.orientation.z = 10.0; + goals_test.goals[0].pose.orientation.w = 11.0; + + goals_test.goals[1].header.stamp.sec = 12; + goals_test.goals[1].header.stamp.nanosec = 13; + goals_test.goals[1].header.frame_id = "odom"; + goals_test.goals[1].pose.position.x = 14.0; + goals_test.goals[1].pose.position.y = 15.0; + goals_test.goals[1].pose.position.z = 16.0; + goals_test.goals[1].pose.orientation.x = 17.0; + goals_test.goals[1].pose.orientation.y = 18.0; + goals_test.goals[1].pose.orientation.z = 19.0; + goals_test.goals[1].pose.orientation.w = 20.0; + + nlohmann::json json; + exporter.toJson(BT::Any(goals_test), json["goals"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["goals"]["__type"], "nav_msgs::msg::Goals"); + ASSERT_EQ(json["goals"]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["goals"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["goals"]["header"]["stamp"]["sec"], 1); + ASSERT_EQ(json["goals"]["header"]["stamp"]["nanosec"], 2); + ASSERT_EQ(json["goals"]["header"]["frame_id"], "map"); + ASSERT_EQ(json["goals"]["goals"][0]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["goals"]["goals"][0]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["goals"]["goals"][0]["header"]["stamp"]["__type"], + "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["goals"]["goals"][0]["header"]["stamp"]["sec"], 3); + ASSERT_EQ(json["goals"]["goals"][0]["header"]["stamp"]["nanosec"], 4); + ASSERT_EQ(json["goals"]["goals"][0]["header"]["frame_id"], "map"); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["__type"], "geometry_msgs::msg::Point"); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["x"], 5.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["y"], 6.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["position"]["z"], 7.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["x"], 8.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["y"], 9.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["z"], 10.0); + ASSERT_EQ(json["goals"]["goals"][0]["pose"]["orientation"]["w"], 11.0); + ASSERT_EQ(json["goals"]["goals"][1]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["goals"]["goals"][1]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["goals"]["goals"][1]["header"]["stamp"]["__type"], + "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["goals"]["goals"][1]["header"]["stamp"]["sec"], 12); + ASSERT_EQ(json["goals"]["goals"][1]["header"]["stamp"]["nanosec"], 13); + ASSERT_EQ(json["goals"]["goals"][1]["header"]["frame_id"], "odom"); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["__type"], "geometry_msgs::msg::Point"); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["x"], 14.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["y"], 15.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["position"]["z"], 16.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["x"], 17.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["y"], 18.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["z"], 19.0); + ASSERT_EQ(json["goals"]["goals"][1]["pose"]["orientation"]["w"], 20.0); + + // Check the two-ways transform, i.e. "from_json" + auto goals_test2 = exporter.fromJson(json["goals"])->first.cast(); + + ASSERT_EQ(goals_test.goals[0].header.stamp.sec, goals_test2.goals[0].header.stamp.sec); + ASSERT_EQ(goals_test.goals[0].header.stamp.nanosec, goals_test2.goals[0].header.stamp.nanosec); + ASSERT_EQ(goals_test.goals[0].header.frame_id, goals_test2.goals[0].header.frame_id); + ASSERT_EQ(goals_test.goals[0].pose.position.x, goals_test2.goals[0].pose.position.x); + ASSERT_EQ(goals_test.goals[0].pose.position.y, goals_test2.goals[0].pose.position.y); + ASSERT_EQ(goals_test.goals[0].pose.position.z, goals_test2.goals[0].pose.position.z); + ASSERT_EQ(goals_test.goals[0].pose.orientation.x, goals_test2.goals[0].pose.orientation.x); + ASSERT_EQ(goals_test.goals[0].pose.orientation.y, goals_test2.goals[0].pose.orientation.y); + ASSERT_EQ(goals_test.goals[0].pose.orientation.z, goals_test2.goals[0].pose.orientation.z); + ASSERT_EQ(goals_test.goals[0].pose.orientation.w, goals_test2.goals[0].pose.orientation.w); + ASSERT_EQ(goals_test.goals[1].header.stamp.sec, goals_test2.goals[1].header.stamp.sec); + ASSERT_EQ(goals_test.goals[1].header.stamp.nanosec, goals_test2.goals[1].header.stamp.nanosec); + ASSERT_EQ(goals_test.goals[1].header.frame_id, goals_test2.goals[1].header.frame_id); + ASSERT_EQ(goals_test.goals[1].pose.position.x, goals_test2.goals[1].pose.position.x); + ASSERT_EQ(goals_test.goals[1].pose.position.y, goals_test2.goals[1].pose.position.y); + ASSERT_EQ(goals_test.goals[1].pose.position.z, goals_test2.goals[1].pose.position.z); + ASSERT_EQ(goals_test.goals[1].pose.orientation.x, goals_test2.goals[1].pose.orientation.x); + ASSERT_EQ(goals_test.goals[1].pose.orientation.y, goals_test2.goals[1].pose.orientation.y); + ASSERT_EQ(goals_test.goals[1].pose.orientation.z, goals_test2.goals[1].pose.orientation.z); + ASSERT_EQ(goals_test.goals[1].pose.orientation.w, goals_test2.goals[1].pose.orientation.w); + + // Convert from string + nav_msgs::msg::Goals goals_test3; + auto const test_json = + R"(json: + { + "__type": "nav_msgs::msg::Goals", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2}, + "frame_id": "map" + }, + "goals": [ + { + "__type": "geometry_msgs::msg::PoseStamped", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 3, "nanosec": 4}, + "frame_id": "map" + }, + "pose": { + "__type": "geometry_msgs::msg::Pose", + "position": { + "__type": "geometry_msgs::msg::Point", + "x": 5.0, "y": 6.0, "z": 7.0 + }, + "orientation": { + "__type": "geometry_msgs::msg::Quaternion", + "x": 8.0, "y": 9.0, "z": 10.0, "w": 11.0 + } + } + }, + { + "__type": "geometry_msgs::msg::PoseStamped", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 12, "nanosec": 13}, + "frame_id": "odom" + }, + "pose": { + "__type": "geometry_msgs::msg::Pose", + "position": { + "__type": "geometry_msgs::msg::Point", + "x": 14.0, "y": 15.0, "z": 16.0 + }, + "orientation": { + "__type": "geometry_msgs::msg::Quaternion", + "x": 17.0, "y": 18.0, "z": 19.0, "w": 20.0 + } + } + } + ] + } + )"; + ASSERT_NO_THROW(goals_test3 = + BT::convertFromString(test_json)); + ASSERT_EQ(goals_test.goals[0].header, goals_test3.goals[0].header); + ASSERT_EQ(goals_test.goals[0].pose.position, goals_test3.goals[0].pose.position); + ASSERT_EQ(goals_test.goals[0].pose.orientation, goals_test3.goals[0].pose.orientation); + ASSERT_EQ(goals_test.goals[1].header, goals_test3.goals[1].header); + ASSERT_EQ(goals_test.goals[1].pose.position, goals_test3.goals[1].pose.position); + ASSERT_EQ(goals_test.goals[1].pose.orientation, goals_test3.goals[1].pose.orientation); +} + +TEST_F(JsonTest, test_path) +{ + BT::JsonExporter & exporter = BT::JsonExporter::get(); + + nav_msgs::msg::Path path_test; + path_test.header.stamp.sec = 1; + path_test.header.stamp.nanosec = 2; + path_test.header.frame_id = "map"; + path_test.poses.resize(2); + path_test.poses[0].header.stamp.sec = 3; + path_test.poses[0].header.stamp.nanosec = 4; + path_test.poses[0].header.frame_id = "map"; + path_test.poses[0].pose.position.x = 5.0; + path_test.poses[0].pose.position.y = 6.0; + path_test.poses[0].pose.position.z = 7.0; + path_test.poses[0].pose.orientation.x = 8.0; + path_test.poses[0].pose.orientation.y = 9.0; + path_test.poses[0].pose.orientation.z = 10.0; + path_test.poses[0].pose.orientation.w = 11.0; + + path_test.poses[1].header.stamp.sec = 12; + path_test.poses[1].header.stamp.nanosec = 13; + path_test.poses[1].header.frame_id = "odom"; + path_test.poses[1].pose.position.x = 14.0; + path_test.poses[1].pose.position.y = 15.0; + path_test.poses[1].pose.position.z = 16.0; + path_test.poses[1].pose.orientation.x = 17.0; + path_test.poses[1].pose.orientation.y = 18.0; + path_test.poses[1].pose.orientation.z = 19.0; + path_test.poses[1].pose.orientation.w = 20.0; + + nlohmann::json json; + exporter.toJson(BT::Any(path_test), json["path"]); + + std::cout << json.dump(2) << std::endl; + + ASSERT_EQ(json["path"]["__type"], "nav_msgs::msg::Path"); + ASSERT_EQ(json["path"]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["path"]["header"]["stamp"]["__type"], "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["path"]["header"]["stamp"]["sec"], 1); + ASSERT_EQ(json["path"]["header"]["stamp"]["nanosec"], 2); + ASSERT_EQ(json["path"]["header"]["frame_id"], "map"); + ASSERT_EQ(json["path"]["poses"][0]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["path"]["poses"][0]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["path"]["poses"][0]["header"]["stamp"]["__type"], + "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["path"]["poses"][0]["header"]["stamp"]["sec"], 3); + ASSERT_EQ(json["path"]["poses"][0]["header"]["stamp"]["nanosec"], 4); + ASSERT_EQ(json["path"]["poses"][0]["header"]["frame_id"], "map"); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["__type"], + "geometry_msgs::msg::Point"); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["x"], 5.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["y"], 6.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["position"]["z"], 7.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["x"], 8.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["y"], 9.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["z"], 10.0); + ASSERT_EQ(json["path"]["poses"][0]["pose"]["orientation"]["w"], 11.0); + ASSERT_EQ(json["path"]["poses"][1]["__type"], "geometry_msgs::msg::PoseStamped"); + ASSERT_EQ(json["path"]["poses"][1]["header"]["__type"], "std_msgs::msg::Header"); + ASSERT_EQ(json["path"]["poses"][1]["header"]["stamp"]["__type"], + "builtin_interfaces::msg::Time"); + ASSERT_EQ(json["path"]["poses"][1]["header"]["stamp"]["sec"], 12); + ASSERT_EQ(json["path"]["poses"][1]["header"]["stamp"]["nanosec"], 13); + ASSERT_EQ(json["path"]["poses"][1]["header"]["frame_id"], "odom"); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["__type"], "geometry_msgs::msg::Pose"); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["__type"], + "geometry_msgs::msg::Point"); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["x"], 14.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["y"], 15.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["position"]["z"], 16.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["__type"], + "geometry_msgs::msg::Quaternion"); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["x"], 17.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["y"], 18.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["z"], 19.0); + ASSERT_EQ(json["path"]["poses"][1]["pose"]["orientation"]["w"], 20.0); + + // Check the two-ways transform, i.e. "from_json" + auto path_test2 = exporter.fromJson(json["path"])->first.cast(); + ASSERT_EQ(path_test.header.stamp.sec, path_test2.header.stamp.sec); + ASSERT_EQ(path_test.header.stamp.nanosec, path_test2.header.stamp.nanosec); + ASSERT_EQ(path_test.header.frame_id, path_test2.header.frame_id); + ASSERT_EQ(path_test.poses[0].header.stamp.sec, path_test2.poses[0].header.stamp.sec); + ASSERT_EQ(path_test.poses[0].header.stamp.nanosec, path_test2.poses[0].header.stamp.nanosec); + ASSERT_EQ(path_test.poses[0].header.frame_id, path_test2.poses[0].header.frame_id); + ASSERT_EQ(path_test.poses[0].pose.position.x, path_test2.poses[0].pose.position.x); + ASSERT_EQ(path_test.poses[0].pose.position.y, path_test2.poses[0].pose.position.y); + ASSERT_EQ(path_test.poses[0].pose.position.z, path_test2.poses[0].pose.position.z); + ASSERT_EQ(path_test.poses[0].pose.orientation.x, path_test2.poses[0].pose.orientation.x); + ASSERT_EQ(path_test.poses[0].pose.orientation.y, path_test2.poses[0].pose.orientation.y); + ASSERT_EQ(path_test.poses[0].pose.orientation.z, path_test2.poses[0].pose.orientation.z); + ASSERT_EQ(path_test.poses[0].pose.orientation.w, path_test2.poses[0].pose.orientation.w); + ASSERT_EQ(path_test.poses[1].header.stamp.sec, path_test2.poses[1].header.stamp.sec); + ASSERT_EQ(path_test.poses[1].header.stamp.nanosec, path_test2.poses[1].header.stamp.nanosec); + ASSERT_EQ(path_test.poses[1].header.frame_id, path_test2.poses[1].header.frame_id); + ASSERT_EQ(path_test.poses[1].pose.position.x, path_test2.poses[1].pose.position.x); + ASSERT_EQ(path_test.poses[1].pose.position.y, path_test2.poses[1].pose.position.y); + ASSERT_EQ(path_test.poses[1].pose.position.z, path_test2.poses[1].pose.position.z); + ASSERT_EQ(path_test.poses[1].pose.orientation.x, path_test2.poses[1].pose.orientation.x); + ASSERT_EQ(path_test.poses[1].pose.orientation.y, path_test2.poses[1].pose.orientation.y); + ASSERT_EQ(path_test.poses[1].pose.orientation.z, path_test2.poses[1].pose.orientation.z); + ASSERT_EQ(path_test.poses[1].pose.orientation.w, path_test2.poses[1].pose.orientation.w); + + // Convert from string + nav_msgs::msg::Path path_test3; + auto const test_json = + R"(json: + { + "__type": "nav_msgs::msg::Path", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 1, "nanosec": 2}, + "frame_id": "map" + }, + "poses": [ + { + "__type": "geometry_msgs::msg::PoseStamped", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 3, "nanosec": 4}, + "frame_id": "map" + }, + "pose": { + "__type": "geometry_msgs::msg::Pose", + "position": { + "__type": "geometry_msgs::msg::Point", + "x": 5.0, "y": 6.0, "z": 7.0 + }, + "orientation": { + "__type": "geometry_msgs::msg::Quaternion", + "x": 8.0, "y": 9.0, "z": 10.0, "w": 11.0 + } + } + }, + { + "__type": "geometry_msgs::msg::PoseStamped", + "header": { + "__type": "std_msgs::msg::Header", + "stamp": {"__type": "builtin_interfaces::msg::Time", "sec": 12, "nanosec": 13}, + "frame_id": "odom" + }, + "pose": { + "__type": "geometry_msgs::msg::Pose", + "position": { + "__type": "geometry_msgs::msg::Point", + "x": 14.0, "y": 15.0, "z": 16.0 + }, + "orientation": { + "__type": "geometry_msgs::msg::Quaternion", + "x": 17.0, "y": 18.0, "z": 19.0, "w": 20.0 + } + } + } + ] + } + )"; + ASSERT_NO_THROW(path_test3 = + BT::convertFromString(test_json)); + ASSERT_EQ(path_test.header, path_test3.header); + ASSERT_EQ(path_test.poses[0].header, path_test3.poses[0].header); + ASSERT_EQ(path_test.poses[0].pose.position, path_test3.poses[0].pose.position); + ASSERT_EQ(path_test.poses[0].pose.orientation, path_test3.poses[0].pose.orientation); + ASSERT_EQ(path_test.poses[1].header, path_test3.poses[1].header); + ASSERT_EQ(path_test.poses[1].pose.position, path_test3.poses[1].pose.position); + ASSERT_EQ(path_test.poses[1].pose.orientation, path_test3.poses[1].pose.orientation); +} diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 98d86499800..d75911b7d1c 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -45,6 +45,18 @@ NavigateThroughPosesNavigator::configure( // Odometry smoother object for getting current speed odom_smoother_ = odom_smoother; + if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { + node->declare_parameter(getName() + ".enable_groot_monitoring", false); + } + + if (!node->has_parameter(getName() + ".groot_server_port")) { + node->declare_parameter(getName() + ".groot_server_port", 1669); + } + + bt_action_server_->setGrootMonitoring( + node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), + node->get_parameter(getName() + ".groot_server_port").as_int()); + return true; } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index be6a2948358..2f7ed68aa3d 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -50,6 +50,19 @@ NavigateToPoseNavigator::configure( "goal_pose", rclcpp::SystemDefaultsQoS(), std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1)); + + if (!node->has_parameter(getName() + ".enable_groot_monitoring")) { + node->declare_parameter(getName() + ".enable_groot_monitoring", false); + } + + if (!node->has_parameter(getName() + ".groot_server_port")) { + node->declare_parameter(getName() + ".groot_server_port", 1667); + } + + bt_action_server_->setGrootMonitoring( + node->get_parameter(getName() + ".enable_groot_monitoring").as_bool(), + node->get_parameter(getName() + ".groot_server_port").as_int()); + return true; } diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 34ebf0e30e5..6953bd4bddc 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -40,7 +40,7 @@ ament_add_test(test_bt_navigator_with_dijkstra PLANNER=nav2_navfn_planner::NavfnPlanner ) -ament_add_test(test_bt_navigator_2 +ament_add_test(test_bt_navigator_with_groot_monitoring GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" @@ -50,6 +50,7 @@ ament_add_test(test_bt_navigator_2 BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml TESTER=nav_to_pose_tester_node.py ASTAR=False + GROOT_MONITORING=True CONTROLLER=dwb_core::DWBLocalPlanner PLANNER=nav2_navfn_planner::NavfnPlanner ) diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 5ee29e6536e..e0d901907fb 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -49,6 +49,8 @@ bt_navigator: navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + enable_groot_monitoring: true + groot_server_port: 1667 navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" error_code_names: @@ -362,4 +364,4 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 - \ No newline at end of file + diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 0eca0680395..478336eb083 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -15,7 +15,10 @@ # limitations under the License. import argparse +import json import math +import os +import struct import sys import time @@ -35,6 +38,7 @@ from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile +import zmq class NavTester(Node): @@ -95,6 +99,12 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") + if os.getenv('GROOT_MONITORING') == 'True': + if not self.grootMonitoringGetStatus(): + self.error_msg('Behavior Tree must not be running already!') + self.error_msg('Are you running multiple goals/bts..?') + return False + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) @@ -113,6 +123,17 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): get_result_future = goal_handle.get_result_async() future_return = True + if os.getenv('GROOT_MONITORING') == 'True': + try: + if not self.grootMonitoringReloadTree(): + self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') + future_return = False + if not self.grootMonitoringSetBreakpoint(): + self.error_msg('Failed GROOT_BT - Set Breakpoint from ZMQ Publisher') + future_return = False + except Exception as e: # noqa: B902 + self.error_msg(f'Failed GROOT_BT - ZMQ Tests: {e}') + future_return = False self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) @@ -127,7 +148,138 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal succeeded!') return True - def poseCallback(self, msg): + def grootMonitoringReloadTree(self) -> bool: + # ZeroMQ Context + context = zmq.Context() + + sock = context.socket(zmq.REQ) + port = 1667 # default server port for groot monitoring + # # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 1000) + # sock.setsockopt(zmq.LINGER, 0) + + sock.connect(f'tcp://localhost:{port}') + self.info_msg(f'ZMQ Server Port:{port}') + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') + # Only works when ZMQ server receives a request first + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 1/3: Check') + try: + # request tree from server + request_header = struct.pack('!BBI', 2, ord('T'), 12345) + sock.send(request_header) + # receive tree from server as flat_buffer + sock.recv_multipart() + self.info_msg('ZMQ Reload Tree Test 2/3: Check') + except zmq.error.Again: + self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') + sock.close() + return False + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') + # Tree should only be loadable ONCE after ZMQ server received a request + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 3/3: Check') + + return True + + def grootMonitoringSetBreakpoint(self) -> bool: + # ZeroMQ Context + context = zmq.Context() + # Define the socket using the 'Context' + sock = context.socket(zmq.REQ) + # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 2000) + # sock.setsockopt(zmq.LINGER, 0) + + port = 1667 # default publishing port for groot monitoring + sock.connect(f'tcp://127.0.0.1:{port}') + self.info_msg(f'ZMQ Publisher Port:{port}') + + # Create header for the request + request_header = struct.pack('!BBI', 2, ord('I'), 12345) # HOOK_INSERT + # Create JSON for the hook + hook_data = { + 'enabled': True, + 'uid': 9, # Node ID + 'mode': 0, # 0 = BREAKPOINT, 1 = REPLACE + 'once': False, + 'desired_status': 'SUCCESS', # Desired status + 'position': 0, # 0 = PRE, 1 = POST + } + hook_json = json.dumps(hook_data) + + # Send the request + try: + sock.send_multipart([request_header, hook_json.encode('utf-8')]) + reply = sock.recv_multipart() + if len(reply[0]) < 2: + self.error_msg('ZMQ - Incomplete reply received') + sock.close() + return False + except Exception as e: + self.error_msg(f'ZMQ - Error during request: {e}') + sock.close() + return False + self.info_msg('ZMQ - HOOK_INSERT request sent') + return True + + def grootMonitoringGetStatus(self) -> bool: + # ZeroMQ Context + context = zmq.Context() + + sock = context.socket(zmq.REQ) + port = 1667 # default server port for groot monitoring + # # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 1000) + # sock.setsockopt(zmq.LINGER, 0) + + sock.connect(f'tcp://localhost:{port}') + self.info_msg(f'ZMQ Server Port:{port}') + + for request in range(3): + try: + # request tree from server + request_header = struct.pack('!BBI', 2, ord('S'), 12345) + sock.send(request_header) + # receive tree from server as flat_buffer + reply = sock.recv_multipart() + if len(reply[0]) < 6: + self.error_msg('ZMQ - Incomplete reply received') + sock.close() + return False + # Decoding payload + payload = reply[1] + node_states = [] + offset = 0 + while offset < len(payload): + node_uid, node_status = struct.unpack_from('!HB', payload, offset) + offset += 3 # 2 bytes for UID, 1 byte for status + node_states.append((node_uid, node_status)) + # Get the status of the first node + node_uid, node_status = node_states[0] + if node_status != 0: + self.error_msg('ZMQ - BT Not running') + return False + except zmq.error.Again: + self.error_msg('ZMQ - Did not receive any status') + sock.close() + return False + self.info_msg('ZMQ - Did receive status') + return True + + def poseCallback(self, msg: PoseWithCovarianceStamped) -> None: self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose self.initial_pose_received = True diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 439a82e8962..db2ec1a584d 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -68,6 +68,9 @@ def generate_launch_description(): if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) + if (os.getenv('GROOT_MONITORING') == 'True'): + param_substitutions.update({'enable_groot_monitoring': 'True'}) + param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} ) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index c6fbe3f9548..05520a57e7f 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -67,6 +67,9 @@ def generate_launch_description(): if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) + if (os.getenv('GROOT_MONITORING') == 'True'): + param_substitutions.update({'enable_groot_monitoring': 'True'}) + param_substitutions.update( {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} )