Program Listing for File geometry_utils.hpp
↰ Return to documentation for file (include/nav2_util/geometry_utils.hpp
)
// Copyright (c) 2019 Intel Corporation
//
// 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.
#ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_
#define NAV2_UTIL__GEOMETRY_UTILS_HPP_
#include <cmath>
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav_msgs/msg/path.hpp"
namespace nav2_util
{
namespace geometry_utils
{
inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
{
tf2::Quaternion q;
q.setRPY(0, 0, angle); // void returning function
return tf2::toMsg(q);
}
inline double euclidean_distance(
const geometry_msgs::msg::Point & pos1,
const geometry_msgs::msg::Point & pos2,
const bool is_3d = false)
{
double dx = pos1.x - pos2.x;
double dy = pos1.y - pos2.y;
if (is_3d) {
double dz = pos1.z - pos2.z;
return std::hypot(dx, dy, dz);
}
return std::hypot(dx, dy);
}
inline double euclidean_distance(
const geometry_msgs::msg::Pose & pos1,
const geometry_msgs::msg::Pose & pos2,
const bool is_3d = false)
{
double dx = pos1.position.x - pos2.position.x;
double dy = pos1.position.y - pos2.position.y;
if (is_3d) {
double dz = pos1.position.z - pos2.position.z;
return std::hypot(dx, dy, dz);
}
return std::hypot(dx, dy);
}
inline double euclidean_distance(
const geometry_msgs::msg::PoseStamped & pos1,
const geometry_msgs::msg::PoseStamped & pos2,
const bool is_3d = false)
{
return euclidean_distance(pos1.pose, pos2.pose, is_3d);
}
inline double euclidean_distance(
const geometry_msgs::msg::Pose2D & pos1,
const geometry_msgs::msg::Pose2D & pos2)
{
double dx = pos1.x - pos2.x;
double dy = pos1.y - pos2.y;
return std::hypot(dx, dy);
}
template<typename Iter, typename Getter>
inline Iter min_by(Iter begin, Iter end, Getter getCompareVal)
{
if (begin == end) {
return end;
}
auto lowest = getCompareVal(*begin);
Iter lowest_it = begin;
for (Iter it = ++begin; it != end; ++it) {
auto comp = getCompareVal(*it);
if (comp <= lowest) {
lowest = comp;
lowest_it = it;
}
}
return lowest_it;
}
template<typename Iter, typename Getter>
inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getCompareVal)
{
if (begin == end) {
return end;
}
Getter dist = 0.0;
for (Iter it = begin; it != end - 1; it++) {
dist += euclidean_distance(*it, *(it + 1));
if (dist > getCompareVal) {
return it + 1;
}
}
return end;
}
inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0)
{
if (start_index + 1 >= path.poses.size()) {
return 0.0;
}
double path_length = 0.0;
for (size_t idx = start_index; idx < path.poses.size() - 1; ++idx) {
path_length += euclidean_distance(path.poses[idx].pose, path.poses[idx + 1].pose);
}
return path_length;
}
} // namespace geometry_utils
} // namespace nav2_util
#endif // NAV2_UTIL__GEOMETRY_UTILS_HPP_