forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)
* initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions
- Loading branch information
1 parent
7340017
commit 15f5800
Showing
12 changed files
with
807 additions
and
111 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
108 changes: 108 additions & 0 deletions
108
nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <cmath> | ||
| #include <vector> | ||
| #include <string> | ||
| #include <iostream> | ||
| #include <memory> | ||
| #include <queue> | ||
| #include <utility> | ||
|
|
||
| #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<tf2_ros::Buffer>, | ||
| std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>, | ||
| std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) 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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <cmath> | ||
| #include <vector> | ||
| #include <string> | ||
| #include <iostream> | ||
| #include <memory> | ||
| #include <queue> | ||
| #include <utility> | ||
|
|
||
| #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<geometry_msgs::msg::PoseStamped>::iterator PathIterator; | ||
| typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator; | ||
|
|
||
| inline std::vector<PathSegment> findDirectionalPathSegments( | ||
| const nav_msgs::msg::Path & path) | ||
| { | ||
| std::vector<PathSegment> 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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.