-
Notifications
You must be signed in to change notification settings - Fork 12
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
improve the orientation filter, add tests (#20)
- Loading branch information
1 parent
ec54f94
commit e581463
Showing
5 changed files
with
448 additions
and
32 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
68 changes: 68 additions & 0 deletions
68
graceful_controller_ros/include/graceful_controller_ros/orientation_tools.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,68 @@ | ||
| /********************************************************************* | ||
| * | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2021, Michael Ferguson | ||
| * Copyright (c) 2009, Willow Garage, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of Willow Garage, Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * Author: Michael Ferguson | ||
| *********************************************************************/ | ||
|
|
||
| #ifndef GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_HPP | ||
| #define GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_HPP | ||
|
|
||
| #include <vector> | ||
| #include <geometry_msgs/PoseStamped.h> | ||
|
|
||
| namespace graceful_controller | ||
| { | ||
|
|
||
| /** | ||
| * @brief Add orientation to each pose in a path. | ||
| * @param path The path to have orientations added. | ||
| * @returns The oriented path. | ||
| */ | ||
| std::vector<geometry_msgs::PoseStamped> | ||
| addOrientations(const std::vector<geometry_msgs::PoseStamped>& path); | ||
|
|
||
| /** | ||
| * @brief Filter a path for orientation noise. | ||
| * @param path The path to be filtered. | ||
| * @param yaw_tolerance Maximum deviation allowed before a pose is filtered. | ||
| * @returns The filtered path. | ||
| */ | ||
| std::vector<geometry_msgs::PoseStamped> | ||
| applyOrientationFilter(const std::vector<geometry_msgs::PoseStamped>& path, | ||
| double yaw_tolerance); | ||
|
|
||
| } // namespace graceful_controller | ||
|
|
||
| #endif // GRACEFUL_CONTROLLER_ROS_ORIENTATION_TOOLS_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,131 @@ | ||
| /********************************************************************* | ||
| * | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2021, Michael Ferguson | ||
| * Copyright (c) 2009, Willow Garage, Inc. | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of Willow Garage, Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * Author: Michael Ferguson | ||
| *********************************************************************/ | ||
|
|
||
| #include <ros/ros.h> | ||
| #include <angles/angles.h> | ||
| #include <tf2/utils.h> // getYaw | ||
| #include "graceful_controller_ros/orientation_tools.hpp" | ||
|
|
||
| namespace graceful_controller | ||
| { | ||
|
|
||
| // Helper function to get yaw if "from" were to point towards "to" | ||
| double getRelativeYaw(const geometry_msgs::PoseStamped& from, | ||
| const geometry_msgs::PoseStamped& to) | ||
| { | ||
| double dx = to.pose.position.x - from.pose.position.x; | ||
| double dy = to.pose.position.y - from.pose.position.y; | ||
| return std::atan2(dy, dx); | ||
| } | ||
|
|
||
| void setYaw(geometry_msgs::PoseStamped& pose, double yaw) | ||
| { | ||
| pose.pose.orientation.z = sin(yaw / 2.0); | ||
| pose.pose.orientation.w = cos(yaw / 2.0); | ||
| } | ||
|
|
||
| std::vector<geometry_msgs::PoseStamped> | ||
| addOrientations(const std::vector<geometry_msgs::PoseStamped>& path) | ||
| { | ||
| std::vector<geometry_msgs::PoseStamped> oriented_path; | ||
| oriented_path.resize(path.size()); | ||
|
|
||
| // The last pose will already be oriented since it is our goal | ||
| oriented_path.back() = path.back(); | ||
|
|
||
| // For each pose, point at the next one | ||
| for (size_t i = 0; i < oriented_path.size() - 1; ++i) | ||
| { | ||
| oriented_path[i] = path[i]; | ||
| double yaw = getRelativeYaw(path[i], path[i + 1]); | ||
| setYaw(oriented_path[i], yaw); | ||
| } | ||
|
|
||
| return oriented_path; | ||
| } | ||
|
|
||
| std::vector<geometry_msgs::PoseStamped> | ||
| applyOrientationFilter(const std::vector<geometry_msgs::PoseStamped>& path, | ||
| double yaw_tolerance) | ||
| { | ||
| std::vector<geometry_msgs::PoseStamped> filtered_path; | ||
| filtered_path.reserve(path.size()); | ||
|
|
||
| // Always keep the first pose | ||
| filtered_path.push_back(path.front()); | ||
|
|
||
| // Possibly filter some intermediate poses | ||
| for (size_t i = 1; i < path.size() - 1; ++i) | ||
| { | ||
| // Get the yaw angle if the previous pose were to be pointing at this pose | ||
| // We need to recompute because we might have dropped poses | ||
| double yaw_previous = getRelativeYaw(filtered_path.back(), path[i]); | ||
|
|
||
| // Get the yaw angle of this pose pointing at next pose | ||
| double yaw_this = tf2::getYaw(path[i].pose.orientation); | ||
|
|
||
| // Get the yaw angle if previous pose were to be pointing at next pose, filtering this pose | ||
| double yaw_without = getRelativeYaw(filtered_path.back(), path[i+1]); | ||
|
|
||
| // Determine if this pose is far off a direct line between previous and next pose | ||
| if (fabs(angles::shortest_angular_distance(yaw_previous, yaw_without)) < yaw_tolerance && | ||
| fabs(angles::shortest_angular_distance(yaw_this, yaw_without)) < yaw_tolerance) | ||
| { | ||
| // Update previous heading in case we dropped some poses | ||
| setYaw(filtered_path.back(), yaw_previous); | ||
| // Add this pose to the filtered plan | ||
| filtered_path.push_back(path[i]); | ||
| } | ||
| else | ||
| { | ||
| // Sorry pose, the plan is better without you :( | ||
| ROS_DEBUG_NAMED("orientation_filter", "Filtering pose %lu", i); | ||
| } | ||
| } | ||
|
|
||
| // Reset heading of what will be penultimate pose, in case we dropped some poses | ||
| setYaw(filtered_path.back(), getRelativeYaw(filtered_path.back(), path.back())); | ||
|
|
||
| // Always add the last pose, since this is our goal | ||
| filtered_path.push_back(path.back()); | ||
|
|
||
| ROS_DEBUG_NAMED("orientation_filter", "Filtered %lu points from plan", path.size() - filtered_path.size()); | ||
| return filtered_path; | ||
| } | ||
|
|
||
| } // namespace graceful_controller |
Oops, something went wrong.