diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 0df85f54bd..0ab7c83628 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -68,8 +68,18 @@ add_library(simple_smoother SHARED ament_target_dependencies(simple_smoother ${dependencies} ) + +# Savitzky Golay Smoother plugin +add_library(savitzky_golay_smoother SHARED + src/savitzky_golay_smoother.cpp +) +ament_target_dependencies(savitzky_golay_smoother + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core plugins.xml) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -82,7 +92,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( - TARGETS ${library_name} simple_smoother + TARGETS ${library_name} simple_smoother savitzky_golay_smoother ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -104,6 +114,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} simple_smoother) +ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index da007a9096..c612dba43c 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -7,3 +7,5 @@ A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface i See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. + +This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp new file mode 100644 index 0000000000..090aa07af8 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::SavitzkyGolaySmoother + * @brief A path smoother implementation using Savitzky Golay filters + */ +class SavitzkyGolaySmoother : public nav2_core::Smoother +{ +public: + /** + * @brief A constructor for nav2_smoother::SavitzkyGolaySmoother + */ + SavitzkyGolaySmoother() = default; + + /** + * @brief A destructor for nav2_smoother::SavitzkyGolaySmoother + */ + ~SavitzkyGolaySmoother() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override; + + /** + * @brief Method to cleanup resources. + */ + void cleanup() override {} + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + void activate() override {} + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + void deactivate() override {} + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + bool do_refinement_; + int refinement_num_; + rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 4b841b3c09..95f19dc54c 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -24,6 +24,7 @@ #include #include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" @@ -35,19 +36,6 @@ namespace nav2_smoother { -/** - * @class nav2_smoother::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - /** * @class nav2_smoother::SimpleSmoother * @brief A path smoother implementation @@ -132,23 +120,6 @@ class SimpleSmoother : public nav2_core::Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_; bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp new file mode 100644 index 0000000000..67d7296353 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace smoother_utils +{ + +/** + * @class nav2_smoother::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +inline std::vector findDirectionalPathSegments( + const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + } +} + +} // namespace smoother_utils + +#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml index c72f7c25c8..90960a51ef 100644 --- a/nav2_smoother/plugins.xml +++ b/nav2_smoother/plugins.xml @@ -4,4 +4,10 @@ Does a simple smoothing process with collision checking + + + + Does Savitzky-Golay smoothing + + diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 27e88f17b3..f8f48c9886 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2019 RoboTech Vision // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..6a61196ab3 --- /dev/null +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include "nav2_smoother/savitzky_golay_smoother.hpp" + +namespace nav2_smoother +{ + +using namespace smoother_utils; // NOLINT +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using nav2_util::declare_parameter_if_not_declared; + +void SavitzkyGolaySmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr/*costmap_sub*/, + std::shared_ptr/*footprint_sub*/) +{ + auto node = parent.lock(); + logger_ = node->get_logger(); + + declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); +} + +bool SavitzkyGolaySmoother::smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) +{ + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time.seconds(); + + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 9) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + + if (time_remaining <= 0.0) { + RCLCPP_WARN( + logger_, + "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); + return false; + } + + // Smooth path segment + success = success && smoothImpl(curr_path_segment, reversing_segment); + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool SavitzkyGolaySmoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + // Must be at least 10 in length to enter function + const unsigned int & path_size = path.poses.size(); + + // 7-point SG filter + const std::array filter = { + -2.0 / 21.0, + 3.0 / 21.0, + 6.0 / 21.0, + 7.0 / 21.0, + 6.0 / 21.0, + 3.0 / 21.0, + -2.0 / 21.0}; + + auto applyFilter = [&](const std::vector & data) + -> geometry_msgs::msg::Point + { + geometry_msgs::msg::Point val; + for (unsigned int i = 0; i != filter.size(); i++) { + val.x += filter[i] * data[i].x; + val.y += filter[i] * data[i].y; + } + return val; + }; + + auto applyFilterOverAxes = + [&](std::vector & plan_pts) -> void + { + // Handle initial boundary conditions, first point is fixed + unsigned int idx = 1; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 2].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + // Apply nominal filter + for (idx = 3; idx < path_size - 4; ++idx) { + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + } + + // Handle terminal boundary conditions, last point is fixed + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 2].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position}); + }; + + applyFilterOverAxes(path.poses); + + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + if (do_refinement_) { + for (int i = 0; i < refinement_num_; i++) { + applyFilterOverAxes(path.poses); + } + } + + updateApproximatePathOrientations(path, reversing_segment); + return true; +} + +} // namespace nav2_smoother + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smoother::SavitzkyGolaySmoother, nav2_core::Smoother) diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 59a81fd3b1..d04d88bc61 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -18,6 +18,7 @@ namespace nav2_smoother { +using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2_util::declare_parameter_if_not_declared; @@ -214,85 +215,6 @@ void SimpleSmoother::setFieldByDim( } } -std::vector SimpleSmoother::findDirectionalPathSegments( - const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existance of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void SimpleSmoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - } // namespace nav2_smoother #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt index 67c98e3e43..623447b5f9 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -21,3 +21,16 @@ target_link_libraries(test_simple_smoother ament_target_dependencies(test_simple_smoother ${dependencies} ) + + +ament_add_gtest(test_savitzky_golay_smoother + test_savitzky_golay_smoother.cpp +) + +target_link_libraries(test_savitzky_golay_smoother + savitzky_golay_smoother +) + +ament_target_dependencies(test_savitzky_golay_smoother + ${dependencies} +) diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..58860d67db --- /dev/null +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -0,0 +1,332 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace smoother_utils; // NOLINT +using namespace nav2_smoother; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_sg_smoother_basics) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + smoother->activate(); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test regular path, should see no effective change + nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + straight_regular_path.header.frame_id = "map"; + straight_regular_path.header.stamp = node->now(); + straight_regular_path.poses.resize(11); + straight_regular_path.poses[0].pose.position.x = 0.5; + straight_regular_path.poses[0].pose.position.y = 0.1; + straight_regular_path.poses[1].pose.position.x = 0.5; + straight_regular_path.poses[1].pose.position.y = 0.2; + straight_regular_path.poses[2].pose.position.x = 0.5; + straight_regular_path.poses[2].pose.position.y = 0.3; + straight_regular_path.poses[3].pose.position.x = 0.5; + straight_regular_path.poses[3].pose.position.y = 0.4; + straight_regular_path.poses[4].pose.position.x = 0.5; + straight_regular_path.poses[4].pose.position.y = 0.5; + straight_regular_path.poses[5].pose.position.x = 0.5; + straight_regular_path.poses[5].pose.position.y = 0.6; + straight_regular_path.poses[6].pose.position.x = 0.5; + straight_regular_path.poses[6].pose.position.y = 0.7; + straight_regular_path.poses[7].pose.position.x = 0.5; + straight_regular_path.poses[7].pose.position.y = 0.8; + straight_regular_path.poses[8].pose.position.x = 0.5; + straight_regular_path.poses[8].pose.position.y = 0.9; + straight_regular_path.poses[9].pose.position.x = 0.5; + straight_regular_path.poses[9].pose.position.y = 1.0; + straight_regular_path.poses[10].pose.position.x = 0.5; + straight_regular_path.poses[10].pose.position.y = 1.1; + straight_regular_path_baseline = straight_regular_path; + + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) { + // Check distances are still the same + EXPECT_NEAR( + fabs( + straight_regular_path.poses[i].pose.position.y - + straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011); + } + + // Attempt smoothing with no time given, should fail + rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds + EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + + smoother->deactivate(); + smoother->cleanup(); +} + +TEST(SmootherTest, test_sg_smoother_noisey_path) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Given nominal irregular/noisey path, test that the output is shorter and smoother + nav_msgs::msg::Path noisey_path, noisey_path_baseline; + noisey_path.header.frame_id = "map"; + noisey_path.header.stamp = node->now(); + noisey_path.poses.resize(11); + noisey_path.poses[0].pose.position.x = 0.5; + noisey_path.poses[0].pose.position.y = 0.1; + noisey_path.poses[1].pose.position.x = 0.5; + noisey_path.poses[1].pose.position.y = 0.2; + noisey_path.poses[2].pose.position.x = 0.5; + noisey_path.poses[2].pose.position.y = 0.3; + noisey_path.poses[3].pose.position.x = 0.5; + noisey_path.poses[3].pose.position.y = 0.4; + noisey_path.poses[4].pose.position.x = 0.5; + noisey_path.poses[4].pose.position.y = 0.5; + noisey_path.poses[5].pose.position.x = 0.5; + noisey_path.poses[5].pose.position.y = 0.6; + noisey_path.poses[6].pose.position.x = 0.5; + noisey_path.poses[6].pose.position.y = 0.7; + noisey_path.poses[7].pose.position.x = 0.5; + noisey_path.poses[7].pose.position.y = 0.8; + noisey_path.poses[8].pose.position.x = 0.5; + noisey_path.poses[8].pose.position.y = 0.9; + noisey_path.poses[9].pose.position.x = 0.5; + noisey_path.poses[9].pose.position.y = 1.0; + noisey_path.poses[10].pose.position.x = 0.5; + noisey_path.poses[10].pose.position.y = 1.1; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != noisey_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + noisey_path.poses[i].pose.position.x += noise; + } + + noisey_path_baseline = noisey_path; + EXPECT_TRUE(smoother->smooth(noisey_path, max_time)); + + // Compute metric, should be shorter if smoother + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y); + base_length += std::hypot( + noisey_path_baseline.poses[i + 1].pose.position.x - + noisey_path_baseline.poses[i].pose.position.x, + noisey_path_baseline.poses[i + 1].pose.position.y - + noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); + + // Test again with refinement, even shorter and smoother + node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); + + length = 0; + double non_refined_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path_refined.poses[i + 1].pose.position.x - + noisey_path_refined.poses[i].pose.position.x, + noisey_path_refined.poses[i + 1].pose.position.y - + noisey_path_refined.poses[i].pose.position.y); + non_refined_length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} + +TEST(SmootherTest, test_sg_smoother_reversing) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test reversing / multiple segments via a cusp + nav_msgs::msg::Path cusp_path, cusp_path_baseline; + cusp_path.header.frame_id = "map"; + cusp_path.header.stamp = node->now(); + cusp_path.poses.resize(22); + cusp_path.poses[0].pose.position.x = 0.5; + cusp_path.poses[0].pose.position.y = 0.1; + cusp_path.poses[1].pose.position.x = 0.5; + cusp_path.poses[1].pose.position.y = 0.2; + cusp_path.poses[2].pose.position.x = 0.5; + cusp_path.poses[2].pose.position.y = 0.3; + cusp_path.poses[3].pose.position.x = 0.5; + cusp_path.poses[3].pose.position.y = 0.4; + cusp_path.poses[4].pose.position.x = 0.5; + cusp_path.poses[4].pose.position.y = 0.5; + cusp_path.poses[5].pose.position.x = 0.5; + cusp_path.poses[5].pose.position.y = 0.6; + cusp_path.poses[6].pose.position.x = 0.5; + cusp_path.poses[6].pose.position.y = 0.7; + cusp_path.poses[7].pose.position.x = 0.5; + cusp_path.poses[7].pose.position.y = 0.8; + cusp_path.poses[8].pose.position.x = 0.5; + cusp_path.poses[8].pose.position.y = 0.9; + cusp_path.poses[9].pose.position.x = 0.5; + cusp_path.poses[9].pose.position.y = 1.0; + cusp_path.poses[10].pose.position.x = 0.5; + cusp_path.poses[10].pose.position.y = 1.1; + cusp_path.poses[11].pose.position.x = 0.5; + cusp_path.poses[11].pose.position.y = 1.0; + cusp_path.poses[12].pose.position.x = 0.5; + cusp_path.poses[12].pose.position.y = 0.9; + cusp_path.poses[13].pose.position.x = 0.5; + cusp_path.poses[13].pose.position.y = 0.8; + cusp_path.poses[14].pose.position.x = 0.5; + cusp_path.poses[14].pose.position.y = 0.7; + cusp_path.poses[15].pose.position.x = 0.5; + cusp_path.poses[15].pose.position.y = 0.6; + cusp_path.poses[16].pose.position.x = 0.5; + cusp_path.poses[16].pose.position.y = 0.5; + cusp_path.poses[17].pose.position.x = 0.5; + cusp_path.poses[17].pose.position.y = 0.4; + cusp_path.poses[18].pose.position.x = 0.5; + cusp_path.poses[18].pose.position.y = 0.3; + cusp_path.poses[19].pose.position.x = 0.5; + cusp_path.poses[19].pose.position.y = 0.2; + cusp_path.poses[20].pose.position.x = 0.5; + cusp_path.poses[20].pose.position.y = 0.1; + cusp_path.poses[21].pose.position.x = 0.5; + cusp_path.poses[21].pose.position.y = 0.0; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != cusp_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + cusp_path.poses[i].pose.position.x += noise; + } + + cusp_path_baseline = cusp_path; + + EXPECT_TRUE(smoother->smooth(cusp_path, max_time)); + + // If it detected the cusp, the cusp point should be fixed + EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x); + EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y); + + // But the path also should be smoother / shorter + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) { + length += std::hypot( + cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x, + cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y); + base_length += std::hypot( + cusp_path_baseline.poses[i + 1].pose.position.x - + cusp_path_baseline.poses[i].pose.position.x, + cusp_path_baseline.poses[i + 1].pose.position.y - + cusp_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fccc2c2a7c..7d9b56d457 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -28,6 +28,7 @@ #include "nav2_smoother/simple_smoother.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT