Program Listing for File smoother_utils.hpp
↰ Return to documentation for file (include/nav2_smoother/smoother_utils.hpp
)
// 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
{
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_