diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index e9a6861f054..5f437ce546a 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,7 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - viz_expansions: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index f944e3dac1f..fae139aa2f8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -117,10 +117,12 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner double _max_planning_time; double _lookup_table_size; double _minimum_turning_radius_global_coords; - bool _viz_expansions; + bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index fcb267e980a..277edb60834 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2021, Samsung Research America +// Copyright (c) 2023, 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. @@ -17,13 +18,18 @@ #include #include +#include +#include "nlohmann/json.hpp" #include "Eigen/Core" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tf2/utils.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "nav2_smac_planner/types.hpp" +#include namespace nav2_smac_planner { @@ -154,6 +160,76 @@ inline void fromJsonToMotionPrimitive( } } +/** + * @brief transform footprint into edges + * @param[in] robot position , orientation and footprint + * @param[out] robot footprint edges + */ +inline std::vector transformFootprintToEdges( + const geometry_msgs::msg::Pose & pose, + const std::vector & footprint) +{ + const double & x = pose.position.x; + const double & y = pose.position.y; + const double & yaw = tf2::getYaw(pose.orientation); + + std::vector out_footprint; + out_footprint.resize(2 * footprint.size()); + for (unsigned int i = 0; i < footprint.size(); i++) { + out_footprint[2 * i].x = x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y; + out_footprint[2 * i].y = y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y; + if (i == 0) { + out_footprint.back().x = out_footprint[i].x; + out_footprint.back().y = out_footprint[i].y; + } else { + out_footprint[2 * i - 1].x = out_footprint[2 * i].x; + out_footprint[2 * i - 1].y = out_footprint[2 * i].y; + } + } + return out_footprint; +} + +/** + * @brief initializes marker to visualize shape of linestring + * @param edge edge to mark of footprint + * @param i marker ID + * @param frame_id frame of the marker + * @param timestamp timestamp of the marker + * @return marker populated + */ +inline visualization_msgs::msg::Marker createMarker( + const std::vector edge, + unsigned int i, const std::string & frame_id, const rclcpp::Time & timestamp) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.frame_locked = false; + marker.ns = "planned_footprint"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.lifetime = rclcpp::Duration(0, 0); + + marker.id = i; + for (auto & point : edge) { + marker.points.push_back(point); + } + + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + marker.color.a = 1.3f; + return marker; +} + + } // namespace nav2_smac_planner #endif // NAV2_SMAC_PLANNER__UTILS_HPP_ diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 949b3cd73bb..1cbeac1e3f2 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2023, 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. @@ -134,8 +135,8 @@ void SmacPlannerHybrid::configure( node->get_parameter(name + ".lookup_table_size", _lookup_table_size); nav2_util::declare_parameter_if_not_declared( - node, name + ".viz_expansions", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".viz_expansions", _viz_expansions); + node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".debug_visualizations", _debug_visualizations); nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); @@ -219,8 +220,11 @@ void SmacPlannerHybrid::configure( } _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); - if (_viz_expansions) { + + if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions", 1); + _planned_footprints_publisher = node->create_publisher( + "planned_footprints", 1); } RCLCPP_INFO( @@ -238,8 +242,9 @@ void SmacPlannerHybrid::activate() _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); - if (_viz_expansions) { + if (_debug_visualizations) { _expansions_publisher->on_activate(); + _planned_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_activate(); @@ -256,8 +261,9 @@ void SmacPlannerHybrid::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); - if (_viz_expansions) { + if (_debug_visualizations) { _expansions_publisher->on_deactivate(); + _planned_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); @@ -278,6 +284,7 @@ void SmacPlannerHybrid::cleanup() } _raw_plan_publisher.reset(); _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( @@ -352,7 +359,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; std::unique_ptr>> expansions = nullptr; - if (_viz_expansions) { + if (_debug_visualizations) { expansions = std::make_unique>>(); } // Note: All exceptions thrown are handled by the planner server and returned to the action @@ -364,8 +371,21 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } } - // Publish expansions for debug - if (_viz_expansions) { + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + if (_debug_visualizations) { + // Publish expansions for debug geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; msg.header.stamp = _clock->now(); @@ -376,19 +396,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( msg.poses.push_back(msg_pose); } _expansions_publisher->publish(msg); - } - // Convert to world coordinates - plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); - pose.pose.orientation = getWorldOrientation(path[i].theta); - plan.poses.push_back(pose); - } + // plot footprint path planned for debug + if (_planned_footprints_publisher->get_subscription_count() > 0) { + auto marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); + } - // Publish raw path for debug - if (_raw_plan_publisher->get_subscription_count() > 0) { - _raw_plan_publisher->publish(plan); + if (marker_array->markers.empty()) { + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } } // Find how much time we have left to do smoothing diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 76befe954cf..8039cf9c51e 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -1,3 +1,14 @@ +# Test utils +ament_add_gtest(test_utils + test_utils.cpp +) +ament_target_dependencies(test_utils + ${dependencies} +) +target_link_libraries(test_utils + ${library_name} +) + # Test costmap downsampler ament_add_gtest(test_costmap_downsampler test_costmap_downsampler.cpp diff --git a/nav2_smac_planner/test/test_utils.cpp b/nav2_smac_planner/test/test_utils.cpp new file mode 100644 index 00000000000..8a6b2440ef5 --- /dev/null +++ b/nav2_smac_planner/test/test_utils.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2023 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 "gtest/gtest.h" +#include "nav2_smac_planner/utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace nav2_smac_planner; // NOLINT + +TEST(transform_footprint_to_edges, test_basic) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + std::vector footprint{p1, p2, p3, p4}; + std::vector footprint_edges{p1, p2, p2, p3, p3, p4, p4, p1}; + + // Identity pose + geometry_msgs::msg::Pose pose0; + auto result = transformFootprintToEdges(pose0, footprint); + EXPECT_EQ(result.size(), 8u); + + for (size_t i = 0; i < result.size(); i++) { + auto & p = result[i]; + auto & q = footprint_edges[i]; + EXPECT_EQ(p.x, q.x); + EXPECT_EQ(p.y, q.y); + } +} + +TEST(transform_footprint_to_edges, test_transition_rotation) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + + geometry_msgs::msg::Pose pose0; + pose0.position.x = 1.0; + pose0.position.y = 1.0; + pose0.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI / 4.0); + + std::vector footprint{p1, p2, p3, p4}; + + // q1 + geometry_msgs::msg::Point q1; + q1.x = 0.0 + pose0.position.x; + q1.y = sqrt(2) + pose0.position.y; + + // q2 + geometry_msgs::msg::Point q2; + q2.x = sqrt(2.0) + pose0.position.x; + q2.y = 0.0 + pose0.position.y; + + // q3 + geometry_msgs::msg::Point q3; + q3.x = 0.0 + pose0.position.x; + q3.y = -sqrt(2) + pose0.position.y; + + // q4 + geometry_msgs::msg::Point q4; + q4.x = -sqrt(2.0) + pose0.position.x; + q4.y = 0.0 + pose0.position.y; + + std::vector footprint_edges{q1, q2, q2, q3, q3, q4, q4, q1}; + auto result = transformFootprintToEdges(pose0, footprint); + EXPECT_EQ(result.size(), 8u); + + for (size_t i = 0; i < result.size(); i++) { + auto & p = result[i]; + auto & q = footprint_edges[i]; + EXPECT_NEAR(p.x, q.x, 1e-3); + EXPECT_NEAR(p.y, q.y, 1e-3); + } +} + +TEST(create_marker, test_createMarker) +{ + geometry_msgs::msg::Point p1; + p1.x = 1.0; + p1.y = 1.0; + + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = -1.0; + + geometry_msgs::msg::Point p3; + p3.x = -1.0; + p3.y = -1.0; + + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = 1.0; + std::vector edges{p1, p2, p3, p4}; + + auto marker1 = createMarker(edges, 10u, "test_frame", rclcpp::Time(0.)); + EXPECT_EQ(marker1.header.frame_id, "test_frame"); + EXPECT_EQ(rclcpp::Time(marker1.header.stamp).nanoseconds(), 0); + EXPECT_EQ(marker1.ns, "planned_footprint"); + EXPECT_EQ(marker1.id, 10u); + EXPECT_EQ(marker1.points.size(), 4u); + + edges.clear(); + auto marker2 = createMarker(edges, 8u, "test_frame2", rclcpp::Time(1.0, 0.0)); + EXPECT_EQ(marker2.header.frame_id, "test_frame2"); + EXPECT_EQ(rclcpp::Time(marker2.header.stamp).nanoseconds(), 1e9); + EXPECT_EQ(marker2.id, 8u); + EXPECT_EQ(marker2.points.size(), 0u); +}