diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index fb5c07c6248..3a01902c3b9 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.10 + 1.3.11

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index e3faad2f7e6..8f6711dac1d 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -59,6 +59,9 @@ list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node) add_library(nav2_compute_path_through_poses_action_bt_node SHARED plugins/action/compute_path_through_poses_action.cpp) list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node) +add_library(nav2_concatenate_paths_action_bt_node SHARED plugins/action/concatenate_paths_action.cpp) +list(APPEND plugin_libs nav2_concatenate_paths_action_bt_node) + add_library(nav2_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp) list(APPEND plugin_libs nav2_controller_cancel_bt_node) @@ -140,6 +143,9 @@ list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node) add_library(nav2_are_error_codes_active_condition_bt_node SHARED plugins/condition/are_error_codes_present_condition.cpp) list(APPEND plugin_libs nav2_are_error_codes_active_condition_bt_node) +add_library(nav2_are_poses_near_condition_bt_node SHARED plugins/condition/are_poses_near_condition.cpp) +list(APPEND plugin_libs nav2_are_poses_near_condition_bt_node) + add_library(nav2_would_a_controller_recovery_help_condition_bt_node SHARED plugins/condition/would_a_controller_recovery_help_condition.cpp) list(APPEND plugin_libs nav2_would_a_controller_recovery_help_condition_bt_node) @@ -185,6 +191,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) +add_library(nav2_get_current_pose_action_bt_node SHARED plugins/action/get_current_pose_action.cpp) +list(APPEND plugin_libs nav2_get_current_pose_action_bt_node) + add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) 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 211fb70df4c..c0afc5bd7cb 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 { @@ -59,6 +61,7 @@ class BtActionNode : public BT::ActionNodeBase auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); getInputOrBlackboard("server_timeout", server_timeout_); + getInputOrBlackboard("cancel_timeout", cancel_timeout_); wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); @@ -321,7 +324,7 @@ class BtActionNode : public BT::ActionNodeBase "Failed to cancel action server for %s", action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + if (callback_group_executor_.spin_until_future_complete(future_result, cancel_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( @@ -474,6 +477,9 @@ class BtActionNode : public BT::ActionNodeBase // new action goal is sent or canceled std::chrono::milliseconds server_timeout_; + // The timeout value when cancelling actions during halt + std::chrono::milliseconds cancel_timeout_; + // The timeout value for BT loop execution std::chrono::milliseconds max_timeout_; 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..60771bb697e 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 @@ -248,12 +255,19 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // Default timeout value when cancelling actions during node halt + std::chrono::milliseconds default_cancel_timeout_; + // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; // 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..23f77970500 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 @@ -61,6 +61,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("default_cancel_timeout")) { + node->declare_parameter("default_cancel_timeout", 20); + } if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } @@ -164,6 +167,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int default_cancel_timeout; + node->get_parameter("default_cancel_timeout", default_cancel_timeout); + default_cancel_timeout_ = std::chrono::milliseconds(default_cancel_timeout); int wait_for_service_timeout; node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); @@ -181,6 +187,7 @@ bool BtActionServer::on_configure() // Put items on the blackboard blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT + blackboard_->set("cancel_timeout", default_cancel_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT blackboard_->set( "wait_for_service_timeout", @@ -217,10 +224,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 +248,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); @@ -248,6 +266,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena auto & blackboard = subtree->blackboard; blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); + blackboard->set("cancel_timeout", default_cancel_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( "wait_for_service_timeout", @@ -261,6 +280,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/concatenate_paths_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp new file mode 100644 index 00000000000..53433c00215 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp @@ -0,0 +1,74 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp/action_node.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ActionNodeBase to shorten path by some distance + */ +class ConcatenatePaths : public BT::ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::ConcatenatePaths constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ConcatenatePaths( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_path1", "Input Path 1 to cancatenate"), + BT::InputPort("input_path2", "Input Path 2 to cancatenate"), + BT::OutputPort("output_path", "Paths concatenated"), + }; + } + +private: + /** + * @brief The other (optional) override required by a BT action. + */ + void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONCATENATE_PATHS_ACTION_HPP_ 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 b8740d975e5..510540812ac 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_current_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_current_pose_action.hpp new file mode 100644 index 00000000000..378972c902b --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_current_pose_action.hpp @@ -0,0 +1,80 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/action_node.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief Action Node to get the current robot pose from TF + */ +class GetCurrentPoseAction : public BT::ActionNodeBase +{ +public: + /** + * @brief Constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + GetCurrentPoseAction( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("global_frame", "Global reference frame"), + BT::InputPort("robot_base_frame", "Robot base frame"), + BT::OutputPort("current_pose", "Current pose output"), + }; + } + +private: + /** + * @brief The other (optional) override required by a BT action. + */ + void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + std::string global_frame_, robot_base_frame_; + std::shared_ptr tf_; + double transform_tolerance_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_CURRENT_POSE_ACTION_HPP_ 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/are_poses_near_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp new file mode 100644 index 00000000000..ff1741f4ec7 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "behaviortree_cpp/condition_node.h" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when a specified goal + * is reached and FAILURE otherwise + */ +class ArePosesNearCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ArePosesNearCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ArePosesNearCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + ArePosesNearCondition() = delete; + + /** + * @brief A destructor for nav2_behavior_tree::ArePosesNearCondition + */ + ~ArePosesNearCondition() override = default; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + + /** + * @brief Checks if the current robot pose lies within a given distance from the goal + * @return bool true when goal is reached, false otherwise + */ + bool arePosesNearby(); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("ref_pose", "Destination"), + BT::InputPort("target_pose", "Destination"), + BT::InputPort("global_frame", "Global frame"), + BT::InputPort("tolerance", 0.5, "Tolerance") + }; + } + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_; + double transform_tolerance_; + std::string global_frame_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_POSES_NEAR_CONDITION_HPP_ 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/package.xml b/nav2_behavior_tree/package.xml index 835dcc2c12b..c129951eeea 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.10 + 1.3.11 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp b/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp new file mode 100644 index 00000000000..94da8d431b1 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/concatenate_paths_action.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp" + +namespace nav2_behavior_tree +{ + +ConcatenatePaths::ConcatenatePaths( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus ConcatenatePaths::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path1, input_path2; + getInput("input_path1", input_path1); + getInput("input_path2", input_path2); + + if (input_path1.poses.empty() && input_path2.poses.empty()) { + RCLCPP_ERROR( + config().blackboard->get("node")->get_logger(), + "No input paths provided to concatenate. Both paths are empty."); + return BT::NodeStatus::FAILURE; + } + + nav_msgs::msg::Path output_path; + output_path = input_path1; + if (input_path1.header != std_msgs::msg::Header()) { + output_path.header = input_path1.header; + } else if (input_path2.header != std_msgs::msg::Header()) { + output_path.header = input_path2.header; + } + + output_path.poses.insert( + output_path.poses.end(), + input_path2.poses.begin(), + input_path2.poses.end()); + + setOutput("output_path", output_path); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ConcatenatePaths"); +} diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 21111fe6629..4d663405094 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -47,7 +47,7 @@ void DriveOnHeadingAction::initialize() void DriveOnHeadingAction::on_tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { initialize(); } } diff --git a/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp b/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp new file mode 100644 index 00000000000..8d5a730a444 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_current_pose_action.cpp @@ -0,0 +1,58 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_behavior_tree/plugins/action/get_current_pose_action.hpp" + +namespace nav2_behavior_tree +{ + +GetCurrentPoseAction::GetCurrentPoseAction( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(xml_tag_name, conf) +{ + auto node = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node->get_parameter("transform_tolerance", transform_tolerance_); + global_frame_ = BT::deconflictPortAndParamFrame( + node, "global_frame", this); + robot_base_frame_ = BT::deconflictPortAndParamFrame( + node, "robot_base_frame", this); +} + +BT::NodeStatus GetCurrentPoseAction::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + geometry_msgs::msg::PoseStamped current_pose; + + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) + { + RCLCPP_WARN( + config().blackboard->get("node")->get_logger(), + "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + + setOutput("current_pose", current_pose); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetCurrentPose"); +} diff --git a/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp new file mode 100644 index 00000000000..1d2327526b9 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp" + +namespace nav2_behavior_tree +{ + +ArePosesNearCondition::ArePosesNearCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf) +{ + auto node = config().blackboard->get("node"); + global_frame_ = BT::deconflictPortAndParamFrame( + node, "global_frame", this); +} + +void ArePosesNearCondition::initialize() +{ + node_ = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node_->get_parameter("transform_tolerance", transform_tolerance_); +} + +BT::NodeStatus ArePosesNearCondition::tick() +{ + if (!BT::isStatusActive(status())) { + initialize(); + } + + if (arePosesNearby()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +bool ArePosesNearCondition::arePosesNearby() +{ + geometry_msgs::msg::PoseStamped pose1, pose2; + double tol; + getInput("ref_pose", pose1); + getInput("target_pose", pose2); + getInput("tolerance", tol); + + if (pose1.header.frame_id != pose2.header.frame_id) { + if (!nav2_util::transformPoseInTargetFrame( + pose1, pose1, *tf_, global_frame_, transform_tolerance_) || + !nav2_util::transformPoseInTargetFrame( + pose2, pose2, *tf_, global_frame_, transform_tolerance_)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to transform poses to the same frame"); + return false; + } + } + + double dx = pose1.pose.position.x - pose2.pose.position.x; + double dy = pose1.pose.position.y - pose2.pose.position.y; + return (dx * dx + dy * dy) <= (tol * tol); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ArePosesNear"); +} diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 70a192d7196..a9b43c7044e 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 { @@ -62,7 +64,9 @@ BehaviorTreeEngine::run( result = tree->tickOnce(); - onLoop(); + if (result == BT::NodeStatus::RUNNING || result == BT::NodeStatus::IDLE) { + onLoop(); + } if (!loopRate.sleep()) { RCLCPP_DEBUG_THROTTLE( @@ -98,6 +102,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/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 94e81cfaecf..119b7c6550a 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -96,6 +96,10 @@ ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cp target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node) ament_target_dependencies(test_get_pose_from_path_action ${dependencies}) +ament_add_gtest(test_get_current_pose_action test_get_current_pose_action.cpp) +target_link_libraries(test_get_current_pose_action nav2_get_current_pose_action_bt_node) +ament_target_dependencies(test_get_current_pose_action ${dependencies}) + ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp) target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node) ament_target_dependencies(test_planner_selector_node ${dependencies}) @@ -124,6 +128,10 @@ ament_add_gtest(test_compute_route test_compute_route_action.cpp) target_link_libraries(test_compute_route nav2_compute_route_bt_node) ament_target_dependencies(test_compute_route ${dependencies}) +ament_add_gtest(test_concatenate_paths_action test_concatenate_paths_action.cpp) +target_link_libraries(test_concatenate_paths_action nav2_concatenate_paths_action_bt_node) +ament_target_dependencies(test_concatenate_paths_action ${dependencies}) + ament_add_gtest(test_compute_and_track_route_cancel test_compute_and_track_route_cancel_node.cpp) target_link_libraries(test_compute_and_track_route_cancel nav2_compute_and_track_route_cancel_bt_node) ament_target_dependencies(test_compute_and_track_route_cancel ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d22ed27c7a2..b90be0d8a9b 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -146,7 +146,9 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset("sequence", result_.result->sequence); + if (result_.result) { + config().blackboard->set("sequence", result_.result->sequence); + } config().blackboard->set("on_cancelled_triggered", true); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp b/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp new file mode 100644 index 00000000000..85935465483 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_concatenate_paths_action.cpp @@ -0,0 +1,152 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include "nav2_behavior_tree/plugins/action/concatenate_paths_action.hpp" + +class ConcatenatePathsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("concatenate_paths_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "ConcatenatePaths", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ConcatenatePathsTestFixture::node_ = nullptr; +BT::NodeConfiguration * ConcatenatePathsTestFixture::config_ = nullptr; +std::shared_ptr ConcatenatePathsTestFixture::factory_ = nullptr; +std::shared_ptr ConcatenatePathsTestFixture::tree_ = nullptr; + +TEST_F(ConcatenatePathsTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + nav_msgs::msg::Path path1, path2; + path1.header.stamp = node_->now(); + path2.header.stamp = node_->now(); + + int i = 0; + int j = 3; + for (int x = 0; x != 3; x++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path1.poses.push_back(pose); + pose.pose.position.x = j; + path2.poses.push_back(pose); + i++; + j++; + } + + config_->blackboard->set("path1", path1); + config_->blackboard->set("path2", path2); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + nav_msgs::msg::Path concat_path; + EXPECT_TRUE(config_->blackboard->get("concat_path", concat_path)); + + EXPECT_EQ(concat_path.poses.size(), 6u); + for (size_t x = 0; x < concat_path.poses.size(); ++x) { + EXPECT_EQ(concat_path.poses[x].pose.position.x, static_cast(x)); + } + + tree_->haltTree(); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + config_->blackboard->set("path1", nav_msgs::msg::Path()); + config_->blackboard->set("path2", nav_msgs::msg::Path()); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp new file mode 100644 index 00000000000..166e54f1fb2 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_get_current_pose_action.cpp @@ -0,0 +1,162 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "behaviortree_cpp/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/get_current_pose_action.hpp" + +using namespace std::chrono_literals; + +class GetCurrentPoseTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("get_current_pose_test_fixture"); + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_shared(node_); + + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("node", node_); + config_->blackboard->set("tf_buffer", tf_buffer_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GetCurrentPose", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + tf_listener_.reset(); + tf_buffer_.reset(); + tf_broadcaster_.reset(); + } + + void SetUp() override + { + if (!node_->has_parameter("robot_base_frame")) { + node_->declare_parameter("robot_base_frame", rclcpp::ParameterValue("base_link")); + } + if (!node_->has_parameter("global_frame")) { + node_->declare_parameter("global_frame", rclcpp::ParameterValue("map")); + } + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + + static std::shared_ptr tf_buffer_; + static std::shared_ptr tf_listener_; + static std::shared_ptr tf_broadcaster_; +}; + +rclcpp::Node::SharedPtr GetCurrentPoseTestFixture::node_ = nullptr; +BT::NodeConfiguration * GetCurrentPoseTestFixture::config_ = nullptr; +std::shared_ptr GetCurrentPoseTestFixture::factory_ = nullptr; +std::shared_ptr GetCurrentPoseTestFixture::tree_ = nullptr; +std::shared_ptr GetCurrentPoseTestFixture::tf_buffer_ = nullptr; +std::shared_ptr GetCurrentPoseTestFixture::tf_listener_ = nullptr; +std::shared_ptr GetCurrentPoseTestFixture::tf_broadcaster_ = nullptr; + +TEST_F(GetCurrentPoseTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = node_->now(); + t.header.frame_id = "map"; + t.child_frame_id = "base_link"; + t.transform.translation.x = 1.0; + t.transform.translation.y = 2.0; + t.transform.translation.z = 0.0; + t.transform.rotation.w = 1.0; + t.transform.rotation.x = 0.0; + t.transform.rotation.y = 0.0; + t.transform.rotation.z = 0.0; + + tf_broadcaster_->sendTransform(t); + + rclcpp::spin_some(node_); + std::this_thread::sleep_for(100ms); + + int ticks = 0; + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && ticks < 10) { + rclcpp::spin_some(node_); + tree_->rootNode()->executeTick(); + std::this_thread::sleep_for(10ms); + ticks++; + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + geometry_msgs::msg::PoseStamped current_pose; + EXPECT_TRUE(config_->blackboard->get("current_pose", current_pose)); + EXPECT_DOUBLE_EQ(current_pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(current_pose.pose.position.y, 2.0); + EXPECT_EQ(current_pose.header.frame_id, "map"); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index d2ba3c1179d..4321f94f854 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -50,6 +50,10 @@ ament_add_gtest(test_are_error_codes_present test_are_error_codes_present.cpp) target_link_libraries(test_are_error_codes_present nav2_would_a_controller_recovery_help_condition_bt_node) ament_target_dependencies(test_are_error_codes_present ${dependencies}) +ament_add_gtest(test_are_poses_near test_are_poses_near.cpp) +target_link_libraries(test_are_poses_near nav2_are_poses_near_condition_bt_node) +ament_target_dependencies(test_are_poses_near ${dependencies}) + ament_add_gtest(test_would_a_controller_recovery_help test_would_a_controller_recovery_help.cpp) target_link_libraries(test_would_a_controller_recovery_help nav2_would_a_controller_recovery_help_condition_bt_node) ament_target_dependencies(test_would_a_controller_recovery_help ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp new file mode 100644 index 00000000000..e028c838787 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_are_poses_near.cpp @@ -0,0 +1,133 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "behaviortree_cpp/bt_factory.h" +#include "nav2_behavior_tree/plugins/condition/are_poses_near_condition.hpp" + +class ArePosesNearTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_fixture"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, node_, false); + + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + + config_->blackboard = BT::Blackboard::create(); + + config_->blackboard->set("node", node_); + config_->blackboard->set>("tf_buffer", tf_buffer_); + + factory_->registerNodeType("ArePosesNear"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + + tf_listener_.reset(); + tf_buffer_.reset(); + node_.reset(); + factory_.reset(); + } + + void SetUp() override + { + if (!node_->has_parameter("transform_tolerance")) { + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + } + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + config_->blackboard->set("p1", goal); + + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr tf_buffer_; + static std::shared_ptr tf_listener_; + static std::shared_ptr factory_; + static BT::NodeConfiguration * config_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ArePosesNearTestFixture::node_ = nullptr; +std::shared_ptr ArePosesNearTestFixture::tf_buffer_ = nullptr; +std::shared_ptr ArePosesNearTestFixture::tf_listener_ = nullptr; +std::shared_ptr ArePosesNearTestFixture::factory_ = nullptr; +BT::NodeConfiguration * ArePosesNearTestFixture::config_ = nullptr; +std::shared_ptr ArePosesNearTestFixture::tree_ = nullptr; + +TEST_F(ArePosesNearTestFixture, test_behavior) +{ + geometry_msgs::msg::PoseStamped p2; + p2.header.stamp = node_->now(); + p2.header.frame_id = "map"; + p2.pose.position.x = 0.0; + p2.pose.position.y = 0.0; + + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); + + p2.pose.position.x = 1.0; + p2.pose.position.y = 1.0; + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); + + p2.pose.position.x = 1.1; + p2.pose.position.y = 1.1; + config_->blackboard->set("p2", p2); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int all_successful = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return all_successful; +} 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_behaviors/package.xml b/nav2_behaviors/package.xml index 779848b5cc5..df077db5f11 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.10 + 1.3.11 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7c59a8806af..74d9ce1ad6e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.10 + 1.3.11 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index b6b27887a38..d5ce8f300a3 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -289,6 +289,7 @@ waypoint_follower: velocity_smoother: ros__parameters: smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" max_velocity: [0.26, 0.0, 1.0] diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 7ec070ece3e..ac52c787f58 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -359,6 +359,7 @@ route_server: velocity_smoother: ros__parameters: smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" max_velocity: [0.5, 0.0, 2.0] diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index e4aefe795d7..4592df49089 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.10 + 1.3.11 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 98d86499800..7228c5b573d 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp" namespace nav2_bt_navigator @@ -45,6 +46,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; } @@ -191,11 +204,9 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) { - RCLCPP_WARN( - logger_, + throw std::runtime_error( "Preemption request was rejected since the goal poses could not be " - "transformed. For now, continuing to track the last goal until completion."); - bt_action_server_->terminatePendingGoal(); + "transformed."); } } else { RCLCPP_WARN( 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_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 76fd8ce1fce..74cc85da1c5 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.10 + 1.3.11 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index dfe0f6cd63d..83db39b500e 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -125,6 +125,11 @@ bool PointCloud::getData( tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + // Still need to transfer height from "z" field if not using global height + if (!use_global_height_) { + data_height = p_v3_b.z(); + } + // Refill data array if (data_height >= min_height_ && data_height <= max_height_) { data.push_back({p_v3_b.x(), p_v3_b.y()}); diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 3235bf8308b..60251cd33d1 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.10 + 1.3.11 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 51b76ef06fa..9e2f9b3796b 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.10 + 1.3.11 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index e9091db9ae8..a6df71b1490 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -74,6 +74,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker protected: double xy_goal_tolerance_, yaw_goal_tolerance_; bool stateful_, check_xy_; + bool symmetric_yaw_tolerance_; // Cached squared xy_goal_tolerance_ double xy_goal_tolerance_sq_; // Dynamic parameters handler diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index d829540d9c2..5e9b930f062 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.10 + 1.3.11 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 2dd6afde449..834df44a2a0 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -57,6 +57,7 @@ SimpleGoalChecker::SimpleGoalChecker() yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), + symmetric_yaw_tolerance_(false), xy_goal_tolerance_sq_(0.0625) { } @@ -78,10 +79,14 @@ void SimpleGoalChecker::initialize( nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".stateful", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".symmetric_yaw_tolerance", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); node->get_parameter(plugin_name + ".stateful", stateful_); + node->get_parameter(plugin_name + ".symmetric_yaw_tolerance", symmetric_yaw_tolerance_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -111,10 +116,23 @@ bool SimpleGoalChecker::isGoalReached( check_xy_ = false; } } - double dyaw = angles::shortest_angular_distance( - tf2::getYaw(query_pose.orientation), - tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) <= yaw_goal_tolerance_; + + double query_yaw = tf2::getYaw(query_pose.orientation); + double goal_yaw = tf2::getYaw(goal_pose.orientation); + if (symmetric_yaw_tolerance_) { + // For symmetric robots: accept either goal orientation or goal + 180° + double dyaw_forward = angles::shortest_angular_distance(query_yaw, goal_yaw); + double dyaw_backward = angles::shortest_angular_distance( + query_yaw, angles::normalize_angle(goal_yaw + M_PI)); + + bool forward_match = fabs(dyaw_forward) <= yaw_goal_tolerance_; + bool backward_match = fabs(dyaw_backward) <= yaw_goal_tolerance_; + + return forward_match || backward_match; + } else { + double dyaw = angles::shortest_angular_distance(query_yaw, goal_yaw); + return fabs(dyaw) <= yaw_goal_tolerance_; + } } bool SimpleGoalChecker::getTolerances( @@ -158,6 +176,8 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".symmetric_yaw_tolerance") { + symmetric_yaw_tolerance_ = parameter.as_bool(); } } } diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index d1d337a5b5f..546355068f3 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -39,6 +39,7 @@ #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "eigen3/Eigen/Geometry" @@ -217,7 +218,8 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), - rclcpp::Parameter("test2.stateful", true)}); + rclcpp::Parameter("test2.stateful", true), + rclcpp::Parameter("test2.symmetric_yaw_tolerance", true)}); rclcpp::spin_until_future_complete( x->get_node_base_interface(), @@ -226,6 +228,7 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); + EXPECT_EQ(x->get_parameter("test2.symmetric_yaw_tolerance").as_bool(), true); // Test the dynamic parameters impacted the tolerances EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); @@ -338,6 +341,24 @@ TEST(StoppedGoalChecker, is_reached) velocity.angular.z = 0.25; EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25 + M_PI); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test2.symmetric_yaw_tolerance", true), + rclcpp::Parameter("test.symmetric_yaw_tolerance", true)}); + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); } int main(int argc, char ** argv) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 1f4d31e982e..71e7d7de039 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -167,6 +167,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) for (size_t i = 0; i != progress_checker_ids_.size(); i++) { progress_checker_ids_concat_ += progress_checker_ids_[i] + std::string(" "); } + if (progress_checker_ids_concat_.empty()) { + progress_checker_ids_concat_ = "(none)"; + } RCLCPP_INFO( get_logger(), @@ -431,6 +434,22 @@ bool ControllerServer::findProgressCheckerId( const std::string & c_name, std::string & current_progress_checker) { + if (progress_checkers_.size() == 0) { + if (c_name.empty()) { + RCLCPP_DEBUG( + get_logger(), + "No progress checker configured and none requested. Progress checking will be bypassed."); + current_progress_checker = ""; + return true; + } else { + RCLCPP_ERROR( + get_logger(), "FollowPath called with progress_checker name %s in parameter" + " 'current_progress_checker', but no progress checkers are configured.", + c_name.c_str()); + return false; + } + } + if (progress_checkers_.find(c_name) == progress_checkers_.end()) { if (progress_checkers_.size() == 1 && c_name.empty()) { RCLCPP_WARN_ONCE( @@ -490,7 +509,9 @@ void ControllerServer::computeControl() } setPlannerPath(goal->path, goal->goal); - progress_checkers_[current_progress_checker_]->reset(); + if (!current_progress_checker_.empty()) { + progress_checkers_[current_progress_checker_]->reset(); + } last_valid_cmd_time_ = now(); rclcpp::WallRate loop_rate(controller_frequency_); @@ -539,6 +560,7 @@ void ControllerServer::computeControl() get_logger(), "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", controller_frequency_, 1 / cycle_duration.seconds()); + loop_rate.reset(); } } } catch (nav2_core::InvalidController & e) { @@ -644,8 +666,10 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::ControllerTFError("Failed to obtain robot pose"); } - if (!progress_checkers_[current_progress_checker_]->check(pose)) { - throw nav2_core::FailedToMakeProgress("Failed to make progress"); + if (!current_progress_checker_.empty()) { + if (!progress_checkers_[current_progress_checker_]->check(pose)) { + throw nav2_core::FailedToMakeProgress("Failed to make progress"); + } } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -744,7 +768,9 @@ void ControllerServer::updateGlobalPath() get_logger(), "Change of progress checker %s requested, resetting it", goal->progress_checker_id.c_str()); current_progress_checker_ = current_progress_checker; - progress_checkers_[current_progress_checker_]->reset(); + if (!current_progress_checker_.empty()) { + progress_checkers_[current_progress_checker_]->reset(); + } } } else { RCLCPP_INFO( diff --git a/nav2_core/package.xml b/nav2_core/package.xml index cde04619d3e..5a4a75cfa0e 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.10 + 1.3.11 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 97a9c0a4ff8..37ca608dd3b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -122,19 +122,11 @@ class Costmap2DPublisher } /** - * @brief Publishes the visualization data over ROS + * @brief Publishes the visualization data over ROS + * @note Only publishes when the associated layer is enabled */ void publishCostmap(); - /** - * @brief Check if the publisher is active - * @return True if the frequency for the publisher is non-zero, false otherwise - */ - bool active() - { - return active_; - } - private: /** @brief Prepare grid_ message for publication. */ void prepareGrid(); @@ -165,7 +157,6 @@ class Costmap2DPublisher unsigned int x0_, xn_, y0_, yn_; double saved_origin_x_; double saved_origin_y_; - bool active_; bool always_send_full_costmap_; double map_vis_z_; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index eef7e78e0cd..03f8131b851 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.10 + 1.3.11 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 6321305dd79..c5fa8abcc94 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -457,8 +457,14 @@ ObstacleLayer::updateBounds( const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); - double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; - double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; + const unsigned int max_range_cells = cellDistance(obs.obstacle_max_range_); + const unsigned int min_range_cells = cellDistance(obs.obstacle_min_range_); + + unsigned int x0, y0; + if (!worldToMap(obs.origin_.x, obs.origin_.y, x0, y0)) { + RCLCPP_DEBUG(logger_, "Sensor origin is out of map bounds"); + continue; + } sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); @@ -479,31 +485,33 @@ ObstacleLayer::updateBounds( continue; } - // compute the squared distance from the hitpoint to the pointcloud's origin - double sq_dist = - (px - - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + - (pz - obs.origin_.z) * (pz - obs.origin_.z); + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + + // compute the distance from the hitpoint to the pointcloud's origin + // Calculate the distance in cell space to match the ray trace algorithm + // used for clearing obstacles (see Costmap2D::raytraceLine). + const int dx = static_cast(mx) - static_cast(x0); + const int dy = static_cast(my) - static_cast(y0); + const unsigned int dist = static_cast( + std::hypot(static_cast(dx), static_cast(dy))); // if the point is far enough away... we won't consider it - if (sq_dist >= sq_obstacle_max_range) { + if (dist > max_range_cells) { RCLCPP_DEBUG(logger_, "The point is too far away"); continue; } - // if the point is too close, do not conisder it - if (sq_dist < sq_obstacle_min_range) { + // if the point is too close, do not consider it + if (dist < min_range_cells) { RCLCPP_DEBUG(logger_, "The point is too close"); continue; } - // now we need to compute the map coordinates for the observation - unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) { - RCLCPP_DEBUG(logger_, "Computing map coords failed"); - continue; - } - unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index d586319783e..7240a23bf06 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -37,6 +37,7 @@ * David V. Lu!! *********************************************************************/ #include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_layer.hpp" #include #include @@ -59,7 +60,6 @@ Costmap2DPublisher::Costmap2DPublisher( : costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), - active_(false), always_send_full_costmap_(always_send_full_costmap), map_vis_z_(map_vis_z) { @@ -232,6 +232,11 @@ std::unique_ptr Costmap2DPublisher::createCostmap void Costmap2DPublisher::publishCostmap() { + auto const costmap_layer = dynamic_cast(costmap_); + if (costmap_layer != nullptr && !costmap_layer->isEnabled()) { + return; + } + float resolution = costmap_->getResolution(); if (always_send_full_costmap_ || grid_resolution_ != resolution || grid_width_ != costmap_->getSizeInCellsX() || diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 8dbce5017f0..cacb1a9f394 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -57,6 +57,12 @@ void CostmapLayer::matchSize() { std::lock_guard guard(*getMutex()); Costmap2D * master = layered_costmap_->getCostmap(); + if (!master) { + RCLCPP_WARN( + rclcpp::get_logger("nav2_costmap_2d"), + "Cannot match size for layer, master costmap is not initialized yet."); + return; + } resizeMap( master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), master->getOriginX(), master->getOriginY()); diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 33e9fc22aa5..2d099a22370 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -44,6 +44,8 @@ #include #include +#include + #include "nav2_costmap_2d/footprint.hpp" @@ -115,7 +117,9 @@ void LayeredCostmap::resizeMap( for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { - (*plugin)->matchSize(); + if (*plugin) { + (*plugin)->matchSize(); + } } for (vector>::iterator filter = filters_.begin(); filter != filters_.end(); ++filter) @@ -146,9 +150,11 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) } if (isOutofBounds(robot_x, robot_y)) { - RCLCPP_WARN( + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_WARN_THROTTLE( rclcpp::get_logger("nav2_costmap_2d"), - "Robot is out of bounds of the costmap!"); + clock, 5000, + "Robot is out of bounds of the costmap"); } if (plugins_.size() == 0 && filters_.size() == 0) { diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 0d332a44630..3b37976a7c8 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -60,4 +60,10 @@ target_link_libraries(denoise_layer_test ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test ${PROJECT_NAME}::nav2_costmap_2d_core -) \ No newline at end of file +) + +ament_add_gtest(obstacle_layer_test obstacle_layer_test.cpp) +target_link_libraries(obstacle_layer_test + ${PROJECT_NAME}::nav2_costmap_2d_core + layers +) diff --git a/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp new file mode 100644 index 00000000000..6075034e190 --- /dev/null +++ b/nav2_costmap_2d/test/unit/obstacle_layer_test.cpp @@ -0,0 +1,396 @@ +// Copyright 2025 Kudan Ltd + +// 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 +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/obstacle_layer.hpp" +#include "../testing_helper.hpp" +#include "tf2_ros/buffer.hpp" + + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +class ObstacleLayerTest : public ::testing::Test +{ +public: + explicit ObstacleLayerTest(double resolution = 0.1) + : layers_("frame", false, false) + { + node_ = std::make_shared("obstacle_cell_distance_test_node"); + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter( + "unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + + // 20x20 cells with origin at (0, 0) + layers_.resizeMap(20, 20, resolution, 0, 0); + tf2_ros::Buffer tf(node_->get_clock()); + addObstacleLayer(layers_, tf, node_, obstacle_layer_); + } + + ~ObstacleLayerTest() {} + + void update() + { + double min_x, min_y, max_x, max_y; + obstacle_layer_->updateBounds(0.0, 0.0, 0.0, &min_x, &min_y, &max_x, &max_y); + } + + void printMap() + { + printf("Printing map:\n"); + for (unsigned int y = 0; y < obstacle_layer_->getSizeInCellsY(); y++) { + for (unsigned int x = 0; x < obstacle_layer_->getSizeInCellsX(); x++) { + printf("%4d", static_cast(obstacle_layer_->getCost(x, y))); + } + printf("\n"); + } + } + +protected: + std::shared_ptr obstacle_layer_; + +private: + std::shared_ptr node_; + nav2_costmap_2d::LayeredCostmap layers_; +}; + +class ObstacleLayerFineResolutionTest : public ObstacleLayerTest +{ +public: + ObstacleLayerFineResolutionTest() + : ObstacleLayerTest(0.05) {} +}; + +/** + * Test that a point within cell_max_range is marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointWithinCellMaxRange) +{ + // Add observation: point at (0.55, 0.0) + // obstacle_max_range = 1.0m + addObservation(obstacle_layer_, 0.55, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.55, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that a point beyond cell_max_range is NOT marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointBeyondCellMaxRange) +{ + // Add observation: point at (1.55, 0.0) + // obstacle_max_range = 1.0m + addObservation(obstacle_layer_, 1.55, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.55, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that a point below cell_min_range is NOT marked as obstacle + */ +TEST_F(ObstacleLayerTest, testPointWithinCellMinRange) +{ + // Add observation: point at (0.35, 0.0) + // obstacle_min_range = 0.5m, obstacle_max_range = 10.0m + addObservation(obstacle_layer_, 0.35, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 10.0, 0.5); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.35, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that points within diagonal distance are marked as obstacles + */ +TEST_F(ObstacleLayerTest, testDiagonalDistanceWithinRange) +{ + // Add observation: point at (0.701, 0.701) + // obstacle_max_range = 1.0m + // obstacle range = sqrt(7^2 + 7^2) = 9.9 cells + addObservation(obstacle_layer_, 0.701, 0.701, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + unsigned int mx, my; + obstacle_layer_->worldToMap(0.701, 0.701, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that point beyond obstacle max range but in same cell as max range is + * marked as obstacle. + */ +TEST_F(ObstacleLayerTest, testPointBeyondRangeButInSameCellAsMaxRange) { + // Observation at (0.79, 0.0) + // obstacle_max_range = 0.72m + // obstacle range = 0.79m, > 0.72m but at same cell distance as max range so + // should be marked as obstacle, since it could be cleared by raytracing with + // the same max range. + addObservation(obstacle_layer_, 0.79, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 0.72, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.79, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test diagonal distance beyond range + */ +TEST_F(ObstacleLayerTest, testDiagonalDistanceBeyondRange) +{ + // Add observation: point at (1.0, 1.0) + // obstacle_max_range = 1.2m + // Distance: sqrt(10^2 + 10^2) = 14.14 cells, 14 cells > 12 cells, so should + // NOT be marked + addObservation(obstacle_layer_, 1.0, 1.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.2, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.0, 1.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_NE(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test edge case: point exactly at cell_max_range boundary + */ +TEST_F(ObstacleLayerTest, testPointAtCellMaxRangeBoundary) +{ + // Add observation: point at (1.0, 0.0) + // obstacle_max_range = 1.0m + addObservation(obstacle_layer_, 1.0, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + unsigned int mx, my; + obstacle_layer_->worldToMap(1.0, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test edge case: point exactly at cell_min_range boundary + */ +TEST_F(ObstacleLayerTest, testPointAtCellMinRangeBoundary) +{ + // Add observation: point at (0.5, 0.0) + // obstacle_min_range = 0.5m + addObservation(obstacle_layer_, 0.5, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 10.0, 0.5); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(0.5, 0.0, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test with different resolution + */ +TEST_F(ObstacleLayerFineResolutionTest, testDifferentResolution) +{ + // Add observation: point at (0.52, 0.28) + // Resolution: 0.05m/cell + // Cell index: (10, 5) + addObservation(obstacle_layer_, 0.52, 0.28, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 100.0, 0.0, 1.0, 0.0); + update(); + + unsigned char cost = obstacle_layer_->getCost(10, 5); + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test with origin offset + */ +TEST_F(ObstacleLayerTest, testOriginOffset) +{ + // Add observation: point at (1.75, 1.85) with origin at (0.5, 1.5) + // obstacle_max_range = 2.0m + addObservation(obstacle_layer_, 1.75, 1.85, MAX_Z / 2, 0.5, 1.5, MAX_Z / 2, + true, true, 100.0, 0.0, 2.0, 0.0); + update(); + + unsigned int mx, my; + obstacle_layer_->worldToMap(1.75, 1.85, mx, my); + unsigned char cost = obstacle_layer_->getCost(mx, my); + ASSERT_EQ(cost, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test that raytracing clears cells along the path and the endpoint is marked + * as obstacle. + */ +TEST_F(ObstacleLayerTest, testRaytraceClearsPathAndMarksEndpoint) +{ + // Mark a few cells along the path as obstacles, excluding the endpoint + for (unsigned int x = 0; x < 4; ++x) { + obstacle_layer_->setCost(x, 0, nav2_costmap_2d::LETHAL_OBSTACLE); + } + + // Add observation at (0.85, 0.0) + addObservation(obstacle_layer_, 0.85, 0.0, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 2.0, 0.0, 2.0, 0.0); + update(); + + unsigned int mx_end; + unsigned int my_end; + obstacle_layer_->worldToMap(0.85, 0.0, mx_end, my_end); + + // Check that cells before endpoint are cleared (FREE_SPACE) + for (unsigned int x = 0; x < mx_end; ++x) { + unsigned char cost = obstacle_layer_->getCost(x, 0); + ASSERT_EQ(cost, nav2_costmap_2d::FREE_SPACE); + } + + // Check that the endpoint cell is LETHAL_OBSTACLE + unsigned char cost_endpoint = obstacle_layer_->getCost(mx_end, my_end); + ASSERT_EQ(cost_endpoint, nav2_costmap_2d::LETHAL_OBSTACLE); +} + +/** + * Test diagonal raytracing does not clear endpoint + */ +TEST_F(ObstacleLayerTest, testDiagonalRaytraceDoesNotClearEndpoint) +{ + // Mark the endpoint as obstacle + unsigned int mx_end, my_end; + obstacle_layer_->worldToMap(0.75, 0.75, mx_end, my_end); + obstacle_layer_->setCost(mx_end, my_end, nav2_costmap_2d::LETHAL_OBSTACLE); + // Mark a cell along the diagonal path + unsigned int mx_mid, my_mid; + obstacle_layer_->worldToMap(0.45, 0.45, mx_mid, my_mid); + obstacle_layer_->setCost(mx_mid, my_mid, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Add observation at (0.75, 0.75) + addObservation(obstacle_layer_, 0.75, 0.75, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 2.0, 0.0, 100.0, 0.0); + update(); + + // The endpoint cell should still be LETHAL_OBSTACLE + unsigned char cost_endpoint = obstacle_layer_->getCost(mx_end, my_end); + ASSERT_EQ(cost_endpoint, nav2_costmap_2d::LETHAL_OBSTACLE); + + // The intermediate cell should be cleared + unsigned char cost_mid = obstacle_layer_->getCost(mx_mid, my_mid); + ASSERT_EQ(cost_mid, nav2_costmap_2d::FREE_SPACE); +} + +/** + * Test diagonal clearing up to max range + */ +TEST_F(ObstacleLayerTest, testClearDiagonalDistance) { + // Mark all points as obstacles + for (unsigned int x = 0; x < obstacle_layer_->getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < obstacle_layer_->getSizeInCellsY(); y++) { + obstacle_layer_->setCost(x, y, nav2_costmap_2d::LETHAL_OBSTACLE); + } + } + // Add observation at (1.55, 1.55) with max clearing range of 1.0m + // This should clear the diagonal distance up to range of 1.0m, cells (0, 0) + // to (7, 7) + addObservation(obstacle_layer_, 1.55, 1.55, MAX_Z / 2, 0.0, 0.0, MAX_Z / 2, + true, true, 1.0, 0.0, 100.0, 0.0); + update(); + + for (unsigned int i = 0; i < 8; i++) { + ASSERT_EQ(obstacle_layer_->getCost(i, i), nav2_costmap_2d::FREE_SPACE); + } + ASSERT_EQ(countValues(*obstacle_layer_, nav2_costmap_2d::FREE_SPACE), 8); + ASSERT_EQ(countValues(*obstacle_layer_, nav2_costmap_2d::LETHAL_OBSTACLE), + 20 * 20 - 8); +} diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index 9278234d996..462273eacfb 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -134,7 +134,7 @@ install(DIRECTORY include/ DESTINATION include/ ) -install(FILES test/test_dock_file.yaml +install(DIRECTORY test/dock_files DESTINATION share/${PROJECT_NAME}/ ) diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index 995e1c2b5e1..21f7d7096d4 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.10 + 1.3.11 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index b6a1f888817..f1d73fc6da1 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -199,7 +199,7 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share node->get_logger(), "Loading dock from database file %s.", dock_filepath.c_str()); try { return utils::parseDockFile(dock_filepath, node, dock_instances_); - } catch (YAML::ParserException & e) { + } catch (YAML::BadConversion & e) { RCLCPP_ERROR( node->get_logger(), "Dock database (%s) is malformed: %s.", dock_filepath.c_str(), e.what()); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 169c7491156..b099a339113 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -431,13 +431,6 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & geometry_msgs::msg::PoseStamped target_pose = dock_pose; target_pose.header.stamp = rclcpp::Time(0); - // Make sure that the target pose is pointing at the robot when moving backwards - // This is to ensure that the robot doesn't try to dock from the wrong side - if (dock_backwards_) { - target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( - tf2::getYaw(target_pose.pose.orientation) + M_PI); - } - // The control law can get jittery when close to the end when atan2's can explode. // Thus, we backward project the controller's target pose a little bit after the // dock so that the robot never gets to the end of the spiral before its in contact @@ -449,6 +442,13 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & target_pose.pose.position.y += sin(yaw) * backward_projection; tf2_buffer_->transform(target_pose, target_pose, base_frame_); + // Make sure that the target pose is pointing at the robot when moving backwards + // This is to ensure that the robot doesn't try to dock from the wrong side + if (dock_backwards_) { + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + } + // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now(); @@ -616,6 +616,12 @@ void DockingServer::undockRobot() // Get "dock pose" by finding the robot pose geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_); + // Make sure that the staging pose is pointing in the same direction when moving backwards + if (dock_backwards_) { + dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(dock_pose.pose.orientation) + M_PI); + } + // Get staging pose (in fixed frame) geometry_msgs::msg::PoseStamped staging_pose = dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id); diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index a8c95ea40fc..667e419661f 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -100,4 +100,20 @@ target_link_libraries(test_docking_server_unit ) # Test docking server (smoke) -ament_add_pytest_test("test_docking_server" test_docking_server.py) +ament_add_pytest_test(test_docking_server_with_charging_dock test_docking_server.py + ENV + NON_CHARGING_DOCK=False + BACKWARD=False +) + +ament_add_pytest_test(test_docking_server_with_non_charging_dock test_docking_server.py + ENV + NON_CHARGING_DOCK=True + BACKWARD=False +) + +ament_add_pytest_test(test_docking_server_backward test_docking_server.py + ENV + NON_CHARGING_DOCK=False + BACKWARD=True +) diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml new file mode 100644 index 00000000000..99efe11c30d --- /dev/null +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml @@ -0,0 +1,5 @@ +docks: + dock1: + type: "dockv3" + frame: "map" + pose: [0.3, map, 0.0] diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml new file mode 100644 index 00000000000..de412e00121 --- /dev/null +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml @@ -0,0 +1,5 @@ +docks: + dock1: + type: "dockv3" + frame: "mapA" + pose: [0.3, 0.3] diff --git a/nav2_docking/opennav_docking/test/test_dock_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml similarity index 100% rename from nav2_docking/opennav_docking/test/test_dock_file.yaml rename to nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml new file mode 100644 index 00000000000..4d88a4da695 --- /dev/null +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml @@ -0,0 +1,4 @@ +docks: + dock1: + type: "dockv3" + frame: "mapA" diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml new file mode 100644 index 00000000000..197e691c7af --- /dev/null +++ b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml @@ -0,0 +1,4 @@ +docks: + dock1: + frame: "mapA" + pose: [0.3, 0.3, 0.0] diff --git a/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml new file mode 100644 index 00000000000..49195f2cb4c --- /dev/null +++ b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml @@ -0,0 +1,5 @@ +no_docks: + dock1: + type: "dockv3" + frame: "mapA" + pose: [0.3, 0.3, 0.0] diff --git a/nav2_docking/opennav_docking/test/docking_params.yaml b/nav2_docking/opennav_docking/test/docking_params.yaml new file mode 100644 index 00000000000..79bc784b292 --- /dev/null +++ b/nav2_docking/opennav_docking/test/docking_params.yaml @@ -0,0 +1,18 @@ +docking_server: + ros__parameters: + wait_charge_timeout: 1.0 + controller: + use_collision_detection: False + transform_tolerance: 0.5 + dock_plugins: [test_dock_plugin] + test_dock_plugin: + plugin: opennav_docking::SimpleChargingDock + use_battery_status: True + dock_direction: forward + staging_yaw_offset: 0.0 + docks: [test_dock] + test_dock: + type: test_dock_plugin + frame: odom + pose: [10.0, 0.0, 0.0] + tolerance: 0.5 diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp index 0285e30f4a1..2181aedede5 100644 --- a/nav2_docking/opennav_docking/test/test_dock_database.cpp +++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp @@ -99,6 +99,47 @@ TEST(DatabaseTests, findTests) db.findDockPlugin(""); } +TEST(DatabaseTests, getDockInstancesBadConversionFile) +{ + auto node = std::make_shared("test"); + std::vector plugins{"dockv1"}; + node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); + + // Set a valid path with a malformed file + node->declare_parameter( + "dock_database", + rclcpp::ParameterValue(ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_dock_bad_conversion_file.yaml")); + + opennav_docking::DockDatabase db; + db.initialize(node, nullptr); + + EXPECT_EQ(db.plugin_size(), 1u); + EXPECT_EQ(db.instance_size(), 0u); +} + +TEST(DatabaseTests, getDockInstancesWrongPath) +{ + auto node = std::make_shared("test"); + std::vector plugins{"dockv1"}; + node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); + + // Set a wrong path + node->declare_parameter("dock_database", rclcpp::ParameterValue("file_does_not_exist.yaml")); + + opennav_docking::DockDatabase db; + db.initialize(node, nullptr); + + EXPECT_EQ(db.plugin_size(), 1u); + EXPECT_EQ(db.instance_size(), 0u); +} + TEST(DatabaseTests, reloadDbService) { auto node = std::make_shared("test"); @@ -116,7 +157,7 @@ TEST(DatabaseTests, reloadDbService) auto request = std::make_shared(); request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + - "/test_dock_file.yaml"; + "/dock_files/test_dock_file.yaml"; EXPECT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); EXPECT_EQ( @@ -136,4 +177,36 @@ TEST(DatabaseTests, reloadDbService) EXPECT_FALSE(result2.get()->success); } +TEST(DatabaseTests, reloadDbMutexLocked) +{ + auto node = std::make_shared("test"); + std::vector plugins{"dockv1"}; + node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); + + // This mutex is locked when dock / undock is called + auto mutex = std::make_shared(); + mutex->lock(); + opennav_docking::DockDatabase db(mutex); + db.initialize(node, nullptr); + + // Call service with a filepath + auto client = + node->create_client("test/reload_database"); + + auto request = std::make_shared(); + request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_dock_file.yaml"; + EXPECT_TRUE(client->wait_for_service(1s)); + auto result = client->async_send_request(request); + EXPECT_EQ( + rclcpp::spin_until_future_complete(node, result, 2s), + rclcpp::FutureReturnCode::SUCCESS); + EXPECT_FALSE(result.get()->success); + + mutex->unlock(); +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 4874d771201..8b59486a499 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -13,6 +13,7 @@ # limitations under the License. from math import acos, cos, sin +import os import time import unittest @@ -20,12 +21,14 @@ from geometry_msgs.msg import TransformStamped, Twist from launch import LaunchDescription # from launch.actions import SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch_ros.actions import Node import launch_testing import launch_testing.actions import launch_testing.asserts import launch_testing.markers import launch_testing.util +from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot import pytest import rclpy @@ -45,7 +48,32 @@ @pytest.mark.rostest # @pytest.mark.flaky # @pytest.mark.flaky(max_runs=5, min_passes=3) -def generate_test_description(): +def generate_test_description() -> LaunchDescription: + + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'docking_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('NON_CHARGING_DOCK') == 'True': + param_substitutions.update({'plugin': 'opennav_docking::SimpleNonChargingDock'}) + + if os.getenv('BACKWARD') == 'True': + param_substitutions.update({'dock_backwards': 'True'}) + param_substitutions.update({'staging_yaw_offset': '3.14'}) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) return LaunchDescription([ # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), @@ -54,21 +82,7 @@ def generate_test_description(): package='opennav_docking', executable='opennav_docking', name='docking_server', - parameters=[{'wait_charge_timeout': 1.0, - 'controller': { - 'use_collision_detection': False, - 'transform_tolerance': 0.5, - }, - 'dock_plugins': ['test_dock_plugin'], - 'test_dock_plugin': { - 'plugin': 'opennav_docking::SimpleChargingDock', - 'use_battery_status': True}, - 'docks': ['test_dock'], - 'test_dock': { - 'type': 'test_dock_plugin', - 'frame': 'odom', - 'pose': [10.0, 0.0, 0.0] - }}], + parameters=[new_yaml], output='screen', ), Node( @@ -99,6 +113,9 @@ def setUp(self): self.x = 0.0 self.y = 0.0 self.theta = 0.0 + # If BACKWARD is set, start facing backward + if os.getenv('BACKWARD') == 'True': + self.theta = 3.14 # Track charge state self.is_charging = False # Latest command velocity @@ -132,13 +149,14 @@ def publish(self): t.transform.rotation.z = sin(self.theta / 2.0) t.transform.rotation.w = cos(self.theta / 2.0) self.tf_broadcaster.sendTransform(t) - # Publish battery state - b = BatteryState() - if self.is_charging: - b.current = 1.0 - else: - b.current = -1.0 - self.battery_state_pub.publish(b) + # Publish the battery state if we are using a charging dock + if os.getenv('NON_CHARGING_DOCK') == 'False': + b = BatteryState() + if self.is_charging: + b.current = 1.0 + else: + b.current = -1.0 + self.battery_state_pub.publish(b) def action_feedback_callback(self, msg): # Force the docking action to run a full recovery loop and then @@ -180,12 +198,13 @@ def test_docking_server(self): 10 ) - # Create publisher for battery state message - self.battery_state_pub = self.node.create_publisher( - BatteryState, - 'battery_state', - 10 - ) + # Create publisher for battery state message if we are using a charging dock + if os.getenv('NON_CHARGING_DOCK') == 'False': + self.battery_state_pub = self.node.create_publisher( + BatteryState, + 'battery_state', + 10 + ) # Mock out navigation server (just teleport the robot) self.action_server = ActionServer( @@ -264,9 +283,14 @@ def test_docking_server(self): rclpy.spin_until_future_complete(self.node, result_future) self.action_result.append(result_future.result()) - self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED) - self.assertTrue(self.action_result[2].result.success) - self.assertEqual(self.action_result[2].result.num_retries, 1) + self.assertIsNotNone(self.action_result[2]) + if self.action_result[2] is not None: + self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED) + self.assertTrue(self.action_result[2].result.success) + if os.getenv('NON_CHARGING_DOCK') == 'False': + self.assertEqual(self.action_result[2].result.num_retries, 1) + else: + self.assertEqual(self.action_result[2].result.num_retries, 0) # Test undocking action self.is_charging = False diff --git a/nav2_docking/opennav_docking/test/test_pose_filter.cpp b/nav2_docking/opennav_docking/test/test_pose_filter.cpp index 2997a21c5aa..79797730f56 100644 --- a/nav2_docking/opennav_docking/test/test_pose_filter.cpp +++ b/nav2_docking/opennav_docking/test/test_pose_filter.cpp @@ -84,6 +84,16 @@ TEST(PoseFilterTests, FilterTests) EXPECT_NEAR(pose.pose.position.x, 2.0, 0.0001); EXPECT_NEAR(pose.pose.position.y, 4.0, 0.0001); EXPECT_NEAR(pose.pose.position.z, 6.0, 0.0001); + + // Create a non-filtering pose filter + PoseFilter no_filter(0.0, 1.0); + // Update with the first measurement + pose = no_filter.update(meas1); + // Check that the pose is the same as the measurement + EXPECT_NEAR(pose.pose.position.x, 1.0, 0.0001); + EXPECT_NEAR(pose.pose.position.y, 3.0, 0.0001); + EXPECT_NEAR(pose.pose.position.z, 5.0, 0.0001); + EXPECT_EQ(pose.pose.orientation.w, 1.0); } } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 7193bb188b0..283c1ec077b 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -35,6 +35,18 @@ RosLockGuard g_rclcpp; namespace opennav_docking { +class SimpleChargingDockShim : public opennav_docking::SimpleChargingDock +{ +public: + SimpleChargingDockShim() + : opennav_docking::SimpleChargingDock() {} + + std::vector getStallJointNames() + { + return stall_joint_names_; + } +}; + TEST(SimpleChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -104,15 +116,23 @@ TEST(SimpleChargingDockTests, StallDetection) "joint_states", rclcpp::QoS(1)); pub->on_activate(); node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); - std::vector names = {"left_motor", "right_motor"}; - node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY); node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); - auto dock = std::make_unique(); + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + // Check that the joint names are empty, showing the error + EXPECT_TRUE(dock->getStallJointNames().empty()); + dock->cleanup(); + // Now set the joint names + std::vector names = {"left_motor", "right_motor"}; + node->set_parameter( + rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); dock->activate(); + EXPECT_EQ(dock->getStallJointNames(), names); // Stopped, but below effort threshold sensor_msgs::msg::JointState msg; @@ -242,4 +262,85 @@ TEST(SimpleChargingDockTests, RefinedPoseTest) dock.reset(); } +TEST(SimpleChargingDockTests, RefinedPoseNotTransform) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + tf_buffer->setUsingDedicatedThread(true); + + dock->configure(node, "my_dock", tf_buffer); + dock->activate(); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Create a pose with a different frame_id + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "other_frame"; + + // It can not find a transform between the two frames + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); + tf_buffer.reset(); +} + +TEST(SimpleChargingDockTests, IsDockedTransformException) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + tf_buffer->setUsingDedicatedThread(true); + + dock->configure(node, "my_dock", tf_buffer); + dock->activate(); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Create a pose with a different frame_id + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "other_frame"; + + // Set a transform between the two frames + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node->now(); + transform.header.frame_id = "my_frame"; + transform.child_frame_id = "other_frame"; + tf_buffer->setTransform(transform, "test", true); + + // It can find a transform between the two frames but it throws an exception in isDocked + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_FALSE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 89c423f5ff3..4c9e4365733 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -35,6 +35,18 @@ RosLockGuard g_rclcpp; namespace opennav_docking { +class SimpleNonChargingDockShim : public opennav_docking::SimpleNonChargingDock +{ +public: + SimpleNonChargingDockShim() + : opennav_docking::SimpleNonChargingDock() {} + + std::vector getStallJointNames() + { + return stall_joint_names_; + } +}; + TEST(SimpleNonChargingDockTests, ObjectLifecycle) { auto node = std::make_shared("test"); @@ -62,14 +74,23 @@ TEST(SimpleNonChargingDockTests, StallDetection) "joint_states", rclcpp::QoS(1)); pub->on_activate(); node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); - std::vector names = {"left_motor", "right_motor"}; - node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY); node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); - auto dock = std::make_unique(); + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + // Check that the joint names are empty, showing the error + EXPECT_TRUE(dock->getStallJointNames().empty()); + dock->cleanup(); + // Now set the joint names + std::vector names = {"left_motor", "right_motor"}; + node->set_parameter( + rclcpp::Parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names))); dock->configure(node, "my_dock", nullptr); + EXPECT_EQ(dock->getStallJointNames(), names); + dock->activate(); geometry_msgs::msg::PoseStamped pose; EXPECT_TRUE(dock->getRefinedPose(pose, "")); @@ -202,4 +223,85 @@ TEST(SimpleNonChargingDockTests, RefinedPoseTest) dock.reset(); } +TEST(SimpleNonChargingDockTests, RefinedPoseNotTransform) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + tf_buffer->setUsingDedicatedThread(true); + + dock->configure(node, "my_dock", tf_buffer); + dock->activate(); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Create a pose with a different frame_id + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "other_frame"; + + // It can not find a transform between the two frames + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); + tf_buffer.reset(); +} + +TEST(SimpleNonChargingDockTests, IsDockedTransformException) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + // Create the TF + auto tf_buffer = std::make_shared(node->get_clock()); + tf_buffer->setUsingDedicatedThread(true); + + dock->configure(node, "my_dock", tf_buffer); + dock->activate(); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 1.0; + detected_pose.pose.position.y = 1.0; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + // Create a pose with a different frame_id + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "other_frame"; + + // Set a transform between the two frames + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node->now(); + transform.header.frame_id = "my_frame"; + transform.child_frame_id = "other_frame"; + tf_buffer->setTransform(transform, "test", true); + + // It can find a transform between the two frames but it throws an exception in isDocked + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_FALSE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_utils.cpp b/nav2_docking/opennav_docking/test/test_utils.cpp index f8474e93740..6e637b07684 100644 --- a/nav2_docking/opennav_docking/test/test_utils.cpp +++ b/nav2_docking/opennav_docking/test/test_utils.cpp @@ -100,7 +100,7 @@ TEST(UtilsTests, parseDockFile) auto node = std::make_shared("test4"); DockMap db; std::string filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + - "/test_dock_file.yaml"; + "/dock_files/test_dock_file.yaml"; EXPECT_TRUE(utils::parseDockFile(filepath, node, db)); EXPECT_EQ(db.size(), 2u); EXPECT_EQ(db["dock1"].frame, std::string("mapA")); @@ -117,6 +117,32 @@ TEST(UtilsTests, parseDockFile) EXPECT_EQ(db["dock2"].id, std::string("2")); } +TEST(UtilsTests, parseDockFile2) +{ + auto node = std::make_shared("test4"); + DockMap db; + + // Test with a file that has no docks + std::string filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_no_docks_file.yaml"; + EXPECT_FALSE(utils::parseDockFile(filepath, node, db)); + + // Test with a file that has no type + filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_dock_no_type_file.yaml"; + EXPECT_FALSE(utils::parseDockFile(filepath, node, db)); + + // Test with a file that has no pose + filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_dock_no_pose_file.yaml"; + EXPECT_FALSE(utils::parseDockFile(filepath, node, db)); + + // Test with a file that has wring pose array size + filepath = ament_index_cpp::get_package_share_directory("opennav_docking") + + "/dock_files/test_dock_bad_pose_file.yaml"; + EXPECT_FALSE(utils::parseDockFile(filepath, node, db)); +} + TEST(UtilsTests, testgetDockPoseStamped) { Dock d; diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 50b2cd7676d..b73a0fb6f55 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.10 + 1.3.11 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 5e03f57a7fb..d486f9bda49 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.10 + 1.3.11 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 2cb7bfcfa85..488eab1756e 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.10 + 1.3.11 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 542d273bc11..a14708dd5d4 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.10 + 1.3.11 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index b8393bd27c6..855b15811a0 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.10 + 1.3.11 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7e643a5f190..4fcd56d9b41 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.10 + 1.3.11 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 2f442fe957c..caebb2f23c5 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.10 + 1.3.11 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 3e149004476..a797e350b37 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.10 + 1.3.11 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 85f4a7b0703..9a223249299 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.10 + 1.3.11 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 8eed8fe0aa2..b1c4a32f1af 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.10 + 1.3.11 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index 2ed28d9cb20..3de1189e530 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.10 + 1.3.11 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 1c0868038ea..7e47bdb5b6e 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.10 + 1.3.11 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index b3ce0480200..9163dea345c 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.10 + 1.3.11 A loopback simulator to replace physics simulation steve macenski Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 728f0a7549a..a375a2e3605 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.10 + 1.3.11 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index d061958135c..bdee6abc1de 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -88,6 +88,7 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | + | symmetric_yaw_tolerance | bool | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty if either is valid and wish the controller to select the easiest one itself. | #### Goal Critic | Parameter | Type | Definition | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 08ec354cf3b..0eeb98063a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -43,6 +43,7 @@ class GoalAngleCritic : public CriticFunction void score(CriticData & data) override; protected: + bool symmetric_yaw_tolerance_{false}; float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index ec8a745407a..493e50f724c 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.10 + 1.3.11 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index c8636e8b78c..31885245d95 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" - +#include "angles/angles.h" namespace mppi::critics { @@ -25,12 +25,13 @@ void GoalAngleCritic::initialize() getParam(weight_, "cost_weight", 3.0f); getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); + getParam(symmetric_yaw_tolerance_, "symmetric_yaw_tolerance", false); RCLCPP_INFO( logger_, - "GoalAngleCritic instantiated with %d power, %f weight, and %f " - "angular threshold.", - power_, weight_, threshold_to_consider_); + "GoalAngleCritic instantiated with %d power, %f weight, %f " + "angular threshold and symmetric_yaw_tolerance %s", + power_, weight_, threshold_to_consider_, symmetric_yaw_tolerance_ ? "enabled" : "disabled"); } void GoalAngleCritic::score(CriticData & data) @@ -44,13 +45,23 @@ void GoalAngleCritic::score(CriticData & data) const auto goal_idx = data.path.x.shape(0) - 1; const float goal_yaw = data.path.yaws(goal_idx); + auto angular_distances = + xt::eval(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw))); + + if (symmetric_yaw_tolerance_) { + // For symmetric robots: use minimum distance to either goal orientation or goal + 180° + const float symmetric_goal_yaw = angles::normalize_angle(goal_yaw + M_PI); + auto symmetric_distances = + xt::eval(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, + symmetric_goal_yaw))); + angular_distances = xt::eval(xt::minimum(angular_distances, symmetric_distances)); + } + if (power_ > 1u) { data.costs += xt::pow( - xt::mean(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * - weight_, power_); + xt::mean(angular_distances, {1}) * weight_, power_); } else { - data.costs += xt::mean( - xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_; + data.costs += xt::mean(angular_distances, {1}) * weight_; } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 21c0987991e..8fa53f03c45 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -202,7 +202,7 @@ bool Optimizer::fallback(bool fail) return false; } - reset(); + reset(false /*Don't reset zone-based speed limits after fallback*/); if (++counter > settings_.retry_attempt_limit) { counter = 0; @@ -503,6 +503,7 @@ void Optimizer::setSpeedLimit(double speed_limit, bool percentage) s.constraints.wz = s.base_constraints.wz * ratio; } } + motion_model_->initialize(settings_.constraints, settings_.model_dt); } models::Trajectories & Optimizer::getGeneratedTrajectories() diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 7cb07c68588..c638efe757b 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -65,6 +65,12 @@ void TrajectoryVisualizer::add( const std::string & marker_namespace, const builtin_interfaces::msg::Time & cmd_stamp) { + if (optimal_path_pub_->get_subscription_count() == 0 && + trajectories_publisher_->get_subscription_count() == 0) + { + return; + } + auto & size = trajectory.shape()[0]; if (!size) { return; @@ -105,6 +111,10 @@ void TrajectoryVisualizer::add( void TrajectoryVisualizer::add( const models::Trajectories & trajectories, const std::string & marker_namespace) { + if (trajectories_publisher_->get_subscription_count() == 0) { + return; + } + auto & shape = trajectories.x.shape(); const float shape_1 = static_cast(shape[1]); points_->markers.reserve(floor(shape[0] / trajectory_step_) * floor(shape[1] * time_step_)); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 114755a5220..c6f54cdb351 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -258,6 +258,57 @@ TEST(CriticTests, GoalAngleCritic) EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight } +TEST(CriticTests, GoalAngleSymmetricCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + geometry_msgs::msg::Pose goal; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, goal, costs, model_dt, + false, nullptr, nullptr, std::nullopt, std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly + auto getParam = param_handler.getParamGetter("critic"); + bool symmetric_yaw_tolerance = true; + getParam(symmetric_yaw_tolerance, "symmetric_yaw_tolerance", true); + GoalAngleCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path too far from `threshold_to_consider` to consider + state.pose.pose.position.x = 9.7; + path.reset(10); + path.x(9) = 10.0; + path.y(9) = 0.0; + path.yaws(9) = 3.14; + goal.position.x = 10.0; + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0); + EXPECT_NEAR(costs(0), 0, 0.02); // Should be zero cost due to symmetry + + path.yaws(9) = 0.0; + critic.score(data); + EXPECT_NEAR(costs(0), 0, 0.02); // (0.0 - 0.0) * 3.0 weight +} + TEST(CriticTests, GoalCritic) { // Standard preamble diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 6170db1dc66..a27fee44f10 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.10 + 1.3.11 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 52da4f30fb0..52d6c8021eb 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.10 + 1.3.11 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index b9385fd205f..0724fe2a79e 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.10 + 1.3.11 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 688823657fb..25a9fa8fbdd 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.10 + 1.3.11 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index d906b97d65f..29665fc4233 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.10 + 1.3.11 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_route/package.xml b/nav2_route/package.xml index 4421dbd1c89..2fa074ed509 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -2,7 +2,7 @@ nav2_route - 1.3.10 + 1.3.11 A Route Graph planner to compliment the Planner Server Steve Macenski Apache-2.0 diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp index 30b56bc2f0d..4c93ae30e75 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -34,19 +34,19 @@ void CostmapScorer::configure( // Find whether to use average or maximum cost values nav2_util::declare_parameter_if_not_declared( node, getName() + ".use_maximum", rclcpp::ParameterValue(true)); - use_max_ = static_cast(node->get_parameter(getName() + ".use_maximum").as_bool()); + use_max_ = node->get_parameter(getName() + ".use_maximum").as_bool(); // Edge is invalid if its in collision nav2_util::declare_parameter_if_not_declared( node, getName() + ".invalid_on_collision", rclcpp::ParameterValue(true)); invalid_on_collision_ = - static_cast(node->get_parameter(getName() + ".invalid_on_collision").as_bool()); + node->get_parameter(getName() + ".invalid_on_collision").as_bool(); // Edge is invalid if edge is off the costmap nav2_util::declare_parameter_if_not_declared( node, getName() + ".invalid_off_map", rclcpp::ParameterValue(true)); invalid_off_map_ = - static_cast(node->get_parameter(getName() + ".invalid_off_map").as_bool()); + node->get_parameter(getName() + ".invalid_off_map").as_bool(); // Max cost to be considered valid nav2_util::declare_parameter_if_not_declared( diff --git a/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp index 257b4298954..5290c9b05c7 100644 --- a/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp +++ b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp @@ -36,7 +36,7 @@ void AdjustSpeedLimit::configure( nav2_util::declare_parameter_if_not_declared( node, getName() + ".speed_limit_topic", rclcpp::ParameterValue("speed_limit")); - std::string topic = node->get_parameter(getName() + ".speed_tag").as_string(); + std::string topic = node->get_parameter(getName() + ".speed_limit_topic").as_string(); speed_limit_pub_ = node->create_publisher(topic, 10); speed_limit_pub_->on_activate(); diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index c5f5c788aa0..3f6f619219a 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -16,6 +16,7 @@ set(CMAKE_AUTOMOC ON) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav2_route REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_msgs REQUIRED) @@ -40,12 +41,15 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_common.hpp include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp + include/nav2_rviz_plugins/route_tool.hpp include/nav2_rviz_plugins/selector.hpp include/nav2_rviz_plugins/utils.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) +qt_wrap_ui(route_tool_UIS_H resource/route_tool.ui) + include_directories( include ) @@ -57,15 +61,18 @@ add_library(${library_name} SHARED src/docking_panel.cpp src/goal_tool.cpp src/nav2_panel.cpp + src/route_tool.cpp src/selector.cpp src/utils.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_headers_to_moc} + ${route_tool_UIS_H} ) set(dependencies geometry_msgs + nav2_route nav2_util nav2_lifecycle_manager nav2_msgs @@ -90,10 +97,12 @@ ament_target_dependencies(${library_name} target_include_directories(${library_name} PUBLIC ${Qt5Widgets_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} + ${nav2_route_INCLUDE_DIRS} ) target_link_libraries(${library_name} rviz_common::rviz_common + nav2_route::route_server_core ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -119,9 +128,13 @@ install( install( DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + "${CMAKE_CURRENT_SOURCE_DIR}/resource" + "${CMAKE_CURRENT_SOURCE_DIR}/rviz" + "${CMAKE_CURRENT_SOURCE_DIR}/launch" DESTINATION "share/${PROJECT_NAME}" ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -136,6 +149,7 @@ ament_export_dependencies( map_msgs nav_msgs rclcpp + nav2_route ) ament_package() diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp new file mode 100644 index 00000000000..50beca8d670 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -0,0 +1,120 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ +#define NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ + +#include +#include +#include +#include +#include +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/graph_saver.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rviz_common/panel.hpp" +#include "std_msgs/msg/int16.hpp" +#include "std_msgs/msg/string.hpp" + + +namespace nav2_rviz_plugins +{ +/** + * Here we declare our new subclass of rviz::Panel. Every panel which + * can be added via the Panels/Add_New_Panel menu is a subclass of + * rviz::Panel. + */ + +class RouteTool : public rviz_common::Panel +{ + /** + * This class uses Qt slots and is a subclass of QObject, so it needs + * the Q_OBJECT macro. + */ + Q_OBJECT + +public: + /** + * QWidget subclass constructors usually take a parent widget + * parameter (which usually defaults to 0). At the same time, + * pluginlib::ClassLoader creates instances by calling the default + * constructor (with no arguments). Taking the parameter and giving + * a default of 0 let's the default constructor work and also let's + * someone using the class for something else to pass in a parent + * widget as they normally would with Qt. + */ + explicit RouteTool(QWidget * parent = nullptr); + + void onInitialize() override; + + /** + * Now we declare overrides of rviz_common::Panel functions for saving and + * loading data from the config file. Here the data is the topic name. + */ + virtual void save(rviz_common::Config config) const; + virtual void load(const rviz_common::Config & config); + + + /** + * Here we declare some internal slots. + */ + +private Q_SLOTS: + void on_load_button_clicked(void); + + void on_save_button_clicked(void); + + void on_create_button_clicked(void); + + void on_confirm_button_clicked(void); + + void on_delete_button_clicked(void); + + void on_add_node_button_toggled(void); + + void on_edit_node_button_toggled(void); + + /** + * Finally, we close up with protected member variables + */ + +protected: + // UI pointer + std::unique_ptr ui_; + +private: + void update_route_graph(void); + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr graph_loader_; + std::shared_ptr graph_saver_; + std::shared_ptr tf_; + nav2_route::Graph graph_; + nav2_route::GraphToIDMap graph_to_id_map_; + nav2_route::GraphToIDMap edge_to_node_map_; + nav2_route::GraphToIncomingEdgesMap graph_to_incoming_edges_map_; + + rclcpp::Publisher::SharedPtr + graph_vis_publisher_; + rclcpp::Subscription::SharedPtr + clicked_point_subscription_; + + unsigned int next_node_id_ = 0; +}; +} // namespace nav2_rviz_plugins +#endif // NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ diff --git a/nav2_rviz_plugins/launch/route_tool.launch.py b/nav2_rviz_plugins/launch/route_tool.launch.py new file mode 100644 index 00000000000..e0914b9d861 --- /dev/null +++ b/nav2_rviz_plugins/launch/route_tool.launch.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025 John Chrosniak +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +import launch_ros.actions + + +def generate_launch_description() -> LaunchDescription: + + # Nodes launching commands + map_file = LaunchConfiguration('yaml_filename') + + declare_map_file_cmd = DeclareLaunchArgument( + 'yaml_filename', + default_value='', + description='Full path to an occupancy grid map yaml file', + ) + + start_route_tool_cmd = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=[ + '-d' + + os.path.join( + get_package_share_directory('nav2_rviz_plugins'), + 'rviz', + 'route_tool.rviz', + ) + ], + ) + + start_map_server = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[{'yaml_filename': map_file}], + ) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': False}, + {'autostart': True}, + {'node_names': ['map_server']}, + ], + ) + + return LaunchDescription( + [ + declare_map_file_cmd, + start_route_tool_cmd, + start_map_server, + start_lifecycle_manager_cmd, + ] + ) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 7b21372dcc0..b33b814821d 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.10 + 1.3.11 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 @@ -28,6 +28,7 @@ urdf visualization_msgs yaml_cpp_vendor + nav2_route libqt5-core libqt5-gui diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index a28b1d9992a..188e94f94e2 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -35,5 +35,11 @@ base_class_type="rviz_common::Display"> The Particle Cloud rviz display. + + + A tool used to create route graphs. + diff --git a/nav2_rviz_plugins/resource/route_tool.ui b/nav2_rviz_plugins/resource/route_tool.ui new file mode 100644 index 00000000000..ba166a46cfe --- /dev/null +++ b/nav2_rviz_plugins/resource/route_tool.ui @@ -0,0 +1,386 @@ + + + + + + route_tool + + + + 0 + 0 + 394 + 461 + + + + MainWindow + + + + + + + + + + 0 + + + + + 0 + 0 + + + + Add + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + Text: + + + + + + + + + + + + + + 16777215 + 50 + + + + Field 1: + + + + + + + + 500 + 30 + + + + + + + + + + + + + 16777215 + 50 + + + + Field 2: + + + + + + + + 16777215 + 30 + + + + + + + + + + + + Create + + + + + + + + + + Edit + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Text: + + + + + + + + + + + + + Field 1: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Field 2: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Confirm + + + + + + + + + + + + + Remove + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + Delete + + + + + + + + + + + + + + + + + + + + Load + + + + + + + Save + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nav2_rviz_plugins/rviz/route_tool.rviz b/nav2_rviz_plugins/rviz/route_tool.rviz new file mode 100644 index 00000000000..521b90a1b19 --- /dev/null +++ b/nav2_rviz_plugins/rviz/route_tool.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Route Graph1/Topic1 + - /Map1 + - /Map1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 305 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: nav2_rviz_plugins/Route Tool + Name: Route Tool +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Route Graph + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /route_graph + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 22.106815338134766 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1003 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000034dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120072006f0075007400650054006f006f006c00000001a4000001e60000006e00fffffffb000000140052006f00750074006500200054006f006f006c01000001ff0000018b0000018b00ffffff000000010000010f0000034dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000034d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000034d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Route Tool: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 27 + routeTool: + collapsed: false \ No newline at end of file diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp new file mode 100644 index 00000000000..7befe4c864a --- /dev/null +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -0,0 +1,268 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 "nav2_rviz_plugins/route_tool.hpp" +#include +#include +#include +#include +#include "rviz_common/display_context.hpp" + + +namespace nav2_rviz_plugins +{ +RouteTool::RouteTool(QWidget * parent) +: rviz_common::Panel(parent), + ui_(std::make_unique()) +{ + // Extend the widget with all attributes and children from UI file + ui_->setupUi(this); + node_ = std::make_shared("route_tool_node", "", rclcpp::NodeOptions()); + node_->configure(); + graph_vis_publisher_ = node_->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + node_->activate(); + tf_ = std::make_shared(node_->get_clock()); + graph_loader_ = std::make_shared(node_, tf_, "map"); + graph_saver_ = std::make_shared(node_, tf_, "map"); + ui_->add_node_button->setChecked(true); + ui_->edit_node_button->setChecked(true); + ui_->remove_node_button->setChecked(true); + // Needed to prevent memory addresses moving from resizing + // when adding nodes and edges + graph_.reserve(1000); +} + +void RouteTool::onInitialize(void) +{ + auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_ERROR( + node_->get_logger(), "Unable to get ROS node abstraction"); + return; + } + auto node = ros_node_abstraction->get_raw_node(); + + clicked_point_subscription_ = node->create_subscription( + "clicked_point", 1, [this](const geometry_msgs::msg::PointStamped::SharedPtr msg) { + ui_->add_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->add_field_2->setText(std::to_string(msg->point.y).c_str()); + ui_->edit_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->edit_field_2->setText(std::to_string(msg->point.y).c_str()); + }); +} + +void RouteTool::on_load_button_clicked(void) +{ + graph_to_id_map_.clear(); + edge_to_node_map_.clear(); + graph_to_incoming_edges_map_.clear(); + graph_.clear(); + QString filename = QFileDialog::getOpenFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + graph_loader_->loadGraphFromFile(graph_, graph_to_id_map_, filename.toStdString()); + unsigned int max_node_id = 0; + for (const auto & node : graph_) { + max_node_id = std::max(node.nodeid, max_node_id); + for (const auto & edge : node.neighbors) { + max_node_id = std::max(edge.edgeid, max_node_id); + edge_to_node_map_[edge.edgeid] = node.nodeid; + if (graph_to_incoming_edges_map_.find(edge.end->nodeid) != + graph_to_incoming_edges_map_.end()) + { + graph_to_incoming_edges_map_[edge.end->nodeid].push_back(edge.edgeid); + } else { + graph_to_incoming_edges_map_[edge.end->nodeid] = std::vector {edge.edgeid}; + } + } + } + next_node_id_ = max_node_id + 1; + update_route_graph(); +} + +void RouteTool::on_save_button_clicked(void) +{ + QString filename = QFileDialog::getSaveFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + RCLCPP_INFO(node_->get_logger(), "Save graph to: %s", filename.toStdString().c_str()); + graph_saver_->saveGraphToFile(graph_, filename.toStdString()); +} + +void RouteTool::on_create_button_clicked(void) +{ + if (ui_->add_field_1->toPlainText() == "" || ui_->add_field_2->toPlainText() == "") {return;} + if (ui_->add_node_button->isChecked()) { + auto longitude = ui_->add_field_1->toPlainText().toFloat(); + auto latitude = ui_->add_field_2->toPlainText().toFloat(); + nav2_route::Node new_node; + new_node.nodeid = next_node_id_; + new_node.coords.x = longitude; + new_node.coords.y = latitude; + graph_.push_back(new_node); + graph_to_id_map_[next_node_id_++] = graph_.size() - 1; + RCLCPP_INFO(node_->get_logger(), "Adding node at: (%f, %f)", longitude, latitude); + update_route_graph(); + } else if (ui_->add_edge_button->isChecked()) { + auto start_node = ui_->add_field_1->toPlainText().toInt(); + auto end_node = ui_->add_field_2->toPlainText().toInt(); + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[start_node]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[end_node]]), + next_node_id_); + if (graph_to_incoming_edges_map_.find(end_node) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[end_node].push_back(next_node_id_); + } else { + graph_to_incoming_edges_map_[end_node] = std::vector {next_node_id_}; + } + edge_to_node_map_[next_node_id_++] = start_node; + RCLCPP_INFO(node_->get_logger(), "Adding edge from %d to %d", start_node, end_node); + update_route_graph(); + } + ui_->add_field_1->setText(""); + ui_->add_field_2->setText(""); +} + +void RouteTool::on_confirm_button_clicked(void) +{ + if (ui_->edit_id->toPlainText() == "" || ui_->edit_field_1->toPlainText() == "" || + ui_->edit_field_2->toPlainText() == "") {return;} + if (ui_->edit_node_button->isChecked()) { + auto node_id = ui_->edit_id->toPlainText().toInt(); + auto new_longitude = ui_->edit_field_1->toPlainText().toFloat(); + auto new_latitude = ui_->edit_field_2->toPlainText().toFloat(); + if (graph_to_id_map_.find(node_id) != graph_to_id_map_.end()) { + graph_[graph_to_id_map_[node_id]].coords.x = new_longitude; + graph_[graph_to_id_map_[node_id]].coords.y = new_latitude; + update_route_graph(); + } + } else if (ui_->edit_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->edit_id->toPlainText().toInt(); + auto new_start = ui_->edit_field_1->toPlainText().toInt(); + auto new_end = ui_->edit_field_2->toPlainText().toInt(); + // Find and remove current edge + auto current_start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = current_start_node->neighbors.begin(); + itr != current_start_node->neighbors.end(); itr++) + { + if (itr->edgeid == edge_id) { + current_start_node->neighbors.erase(itr); + break; + } + } + // Create new edge with same ID using new start and stop nodes + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[new_start]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[new_end]]), + edge_id); + edge_to_node_map_[edge_id] = new_start; + if (graph_to_incoming_edges_map_.find(new_end) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[new_end].push_back(edge_id); + } else { + graph_to_incoming_edges_map_[new_end] = std::vector {edge_id}; + } + update_route_graph(); + } + ui_->edit_id->setText(""); + ui_->edit_field_1->setText(""); + ui_->edit_field_2->setText(""); +} + +void RouteTool::on_delete_button_clicked(void) +{ + if (ui_->remove_id->toPlainText() == "") {return;} + if (ui_->remove_node_button->isChecked()) { + unsigned int node_id = ui_->remove_id->toPlainText().toInt(); + // Remove edges pointing to the removed node + for (auto edge_id : graph_to_incoming_edges_map_[node_id]) { + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + } + if (graph_[graph_to_id_map_[node_id]].nodeid == node_id) { + // Use max int to mark the node as deleted + graph_[graph_to_id_map_[node_id]].nodeid = std::numeric_limits::max(); + graph_to_id_map_.erase(node_id); + graph_to_incoming_edges_map_.erase(node_id); + RCLCPP_INFO(node_->get_logger(), "Removed node %d", node_id); + } + update_route_graph(); + } else if (ui_->remove_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->remove_id->toPlainText().toInt(); + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + RCLCPP_INFO(node_->get_logger(), "Removed edge %d", edge_id); + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + update_route_graph(); + } + ui_->remove_id->setText(""); +} + +void RouteTool::on_add_node_button_toggled(void) +{ + if (ui_->add_node_button->isChecked()) { + ui_->add_text->setText("Position:"); + ui_->add_label_1->setText("X:"); + ui_->add_label_2->setText("Y:"); + } else { + ui_->add_text->setText("Connections:"); + ui_->add_label_1->setText("Start Node ID:"); + ui_->add_label_2->setText("End Node ID:"); + } +} + +void RouteTool::on_edit_node_button_toggled(void) +{ + if (ui_->edit_node_button->isChecked()) { + ui_->edit_text->setText("Position:"); + ui_->edit_label_1->setText("X:"); + ui_->edit_label_2->setText("Y:"); + } else { + ui_->edit_text->setText("Connections:"); + ui_->edit_label_1->setText("Start Node ID:"); + ui_->edit_label_2->setText("End Node ID:"); + } +} + +void RouteTool::update_route_graph(void) +{ + graph_vis_publisher_->publish(nav2_route::utils::toMsg(graph_, "map", node_->now())); +} + +void RouteTool::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); +} + +void RouteTool::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); +} +} // namespace nav2_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::RouteTool, rviz_common::Panel) diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 52463c9991b..ff67b70d27d 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -313,7 +313,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): self.debug("Waiting for 'DriveOnHeading' action server") - while not self.backup_client.wait_for_server(timeout_sec=1.0): + while not self.drive_on_heading_client.wait_for_server(timeout_sec=1.0): self.info("'DriveOnHeading' action server not available, waiting...") goal_msg = DriveOnHeading.Goal() goal_msg.target = Point(x=float(dist)) diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 9d381b375c7..a659b3a5eba 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.10 + 1.3.11 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 927e973dfe8..db2e52a3fe8 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.10 + 1.3.11 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 46668395b85..915cb3ff6a2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.10 + 1.3.11 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index a9da4f43fb9..8b263818ff4 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.10 + 1.3.11 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 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')} ) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 0ac1fa9b7aa..02b991ec0dc 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -25,9 +25,6 @@ The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the **f(a)** - total cost (g(a) + h(a)) for the node 'a' -**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with -respect to a function, value = 253 - **curr** - represents the node whose neighbours are being added to the list **neigh** - one of the neighboring nodes of curr @@ -36,7 +33,7 @@ respect to a function, value = 253 **euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' -**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 255 = unknown value) between the node type 'a' and type 'b' ### Cost function ``` diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 1c819a882ee..1b6f298314c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -25,8 +25,8 @@ const double INF_COST = DBL_MAX; const int UNKNOWN_COST = 255; -const int OBS_COST = 254; -const int LETHAL_COST = 252; +const int OCCUPIED_COST = 254; +const int MAX_NON_OBSTACLE_COST = 252; struct coordsM { @@ -92,14 +92,13 @@ class ThetaStar bool generatePath(std::vector & raw_path, std::function cancel_checker); /** - * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST + * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than or equal to the MAX_NON_OBSTACLE_COST * @return the result of the check */ inline bool isSafe(const int & cx, const int & cy) const { - return (costmap_->getCost( - cx, - cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || + costmap_->getCost(cx, cy) <= MAX_NON_OBSTACLE_COST; } /** @@ -110,8 +109,8 @@ class ThetaStar const geometry_msgs::msg::PoseStamped & goal); /** - * @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST - * @return true if the cost of any one of the points is greater than LETHAL_COST + * @brief checks whether the start and goal points have costmap costs greater than MAX_NON_OBSTACLE_COST + * @return true if the cost of any one of the points is greater than MAX_NON_OBSTACLE_COST */ bool isUnsafeToPlan() const { @@ -193,16 +192,19 @@ class ThetaStar /** * @brief it is an overloaded function to ease the cost calculations while performing the LOS check * @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned - * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise + * @return false if the traversal cost is greater than the MAX_NON_OBSTACLE_COST and true otherwise */ bool isSafe(const int & cx, const int & cy, double & cost) const { double curr_cost = getCost(cx, cy); - if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { + if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || + curr_cost <= MAX_NON_OBSTACLE_COST) + { if (costmap_->getCost(cx, cy) == UNKNOWN_COST) { - curr_cost = OBS_COST - 1; + curr_cost = OCCUPIED_COST - 1; } - cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + cost += w_traversal_cost_ * curr_cost * curr_cost / MAX_NON_OBSTACLE_COST / + MAX_NON_OBSTACLE_COST; return true; } else { return false; @@ -226,7 +228,8 @@ class ThetaStar inline double getTraversalCost(const int & cx, const int & cy) { double curr_cost = getCost(cx, cy); - return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return w_traversal_cost_ * curr_cost * curr_cost / MAX_NON_OBSTACLE_COST / + MAX_NON_OBSTACLE_COST; } /** diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index d05f1d82034..7dee7230b7a 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.10 + 1.3.11 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 66575ed42d1..5f51b7831b4 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.10 + 1.3.11 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index 579b3a6737a..4238f9fed5d 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -60,6 +60,7 @@ velocity_smoother: odom_topic: "odom" # Topic of odometry to use for estimating current velocities odom_duration: 0.1 # Period of time (s) to sample odometry information in for velocity estimation enable_stamped_cmd_vel: false # Whether to stamp the velocity. True uses TwistStamped. False uses Twist + stamp_smoothed_velocity_with_smoothing_time: false # Whether to smooth the timestamp of the header message of TwistStamped output instead of keeping the timestamp of the last received command ``` ## Topics @@ -87,4 +88,4 @@ The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current. -The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file +The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index f69a827a7db..bc757888589 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -149,6 +149,8 @@ class VelocitySmoother : public nav2_util::LifecycleNode bool open_loop_; bool stopped_{true}; bool scale_velocities_; + // whether to overwite the timestamp of the smoothed message or to keep last command's timestamp + bool stamp_smoothed_velocity_with_smoothing_time_; std::vector max_velocities_; std::vector min_velocities_; std::vector max_accels_; diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index abdd180a9f3..c67f0e13879 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.10 + 1.3.11 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 3e2fb782c81..9ba546dc9f7 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -145,6 +145,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) return nav2_util::CallbackReturn::FAILURE; } + // Define option to overwrite the timestamp of the message containing the smoothed velocity + declare_parameter_if_not_declared( + node, "stamp_smoothed_velocity_with_smoothing_time", rclcpp::ParameterValue(false)); + node->get_parameter( + "stamp_smoothed_velocity_with_smoothing_time", stamp_smoothed_velocity_with_smoothing_time_); + // Setup inputs / outputs smoothed_cmd_pub_ = std::make_unique(node, "cmd_vel_smoothed", 1); cmd_sub_ = std::make_unique( @@ -309,11 +315,21 @@ void VelocitySmoother::smootherTimer() return; } + auto const delta_time_since_last_command = now() - last_command_time_; + auto cmd_vel = std::make_unique(); - cmd_vel->header = command_->header; + cmd_vel->header.frame_id = command_->header.frame_id; + if (stamp_smoothed_velocity_with_smoothing_time_) { + // Smooth the timestamp of the smoothed message + // Do not keep the same timestamp of the last command; this causes jerky behavior + // See https://github.com/ros-navigation/navigation2/issues/5857 + cmd_vel->header.stamp = command_->header.stamp + delta_time_since_last_command; + } else { + cmd_vel->header.stamp = command_->header.stamp; + } // Check for velocity timeout. If nothing received, publish zeros to apply deceleration - if (now() - last_command_time_ > velocity_timeout_) { + if (delta_time_since_last_command > velocity_timeout_) { if (last_cmd_.twist == geometry_msgs::msg::Twist() || stopped_) { stopped_ = true; return; diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index b452f639020..a717bda55ce 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.10 + 1.3.11 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index fbba3b2d159..155cfc83fe7 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.10 + 1.3.11 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 4892d47ac59..651db4579ba 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.10 + 1.3.11 ROS2 Navigation Stack