X-lanni
131 lines
3.5 KiB
C++
131 lines
3.5 KiB
C++
// Copyright (c) 2018 Intel Corporation
|
|
// Copyright (c) 2020 Sarthak Mittal
|
|
//
|
|
// 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.
|
|
|
|
#include "nav2_util/geometry_utils.hpp"
|
|
#include "geometry_msgs/msg/point.hpp"
|
|
#include "geometry_msgs/msg/pose.hpp"
|
|
#include "nav_msgs/msg/path.hpp"
|
|
#include "gtest/gtest.h"
|
|
|
|
using nav2_util::geometry_utils::euclidean_distance;
|
|
using nav2_util::geometry_utils::calculate_path_length;
|
|
|
|
TEST(GeometryUtils, euclidean_distance_point_3d)
|
|
{
|
|
geometry_msgs::msg::Point point1;
|
|
point1.x = 3.0;
|
|
point1.y = 2.0;
|
|
point1.z = 1.0;
|
|
|
|
geometry_msgs::msg::Point point2;
|
|
point2.x = 1.0;
|
|
point2.y = 2.0;
|
|
point2.z = 3.0;
|
|
|
|
ASSERT_NEAR(euclidean_distance(point1, point2, true), 2.82843, 1e-5);
|
|
}
|
|
|
|
TEST(GeometryUtils, euclidean_distance_point_2d)
|
|
{
|
|
geometry_msgs::msg::Point point1;
|
|
point1.x = 3.0;
|
|
point1.y = 2.0;
|
|
point1.z = 1.0;
|
|
|
|
geometry_msgs::msg::Point point2;
|
|
point2.x = 1.0;
|
|
point2.y = 2.0;
|
|
point2.z = 3.0;
|
|
|
|
ASSERT_NEAR(euclidean_distance(point1, point2), 2.0, 1e-5);
|
|
}
|
|
|
|
TEST(GeometryUtils, euclidean_distance_pose_3d)
|
|
{
|
|
geometry_msgs::msg::Pose pose1;
|
|
pose1.position.x = 7.0;
|
|
pose1.position.y = 4.0;
|
|
pose1.position.z = 3.0;
|
|
|
|
geometry_msgs::msg::Pose pose2;
|
|
pose2.position.x = 17.0;
|
|
pose2.position.y = 6.0;
|
|
pose2.position.z = 2.0;
|
|
|
|
ASSERT_NEAR(euclidean_distance(pose1, pose2, true), 10.24695, 1e-5);
|
|
}
|
|
|
|
TEST(GeometryUtils, euclidean_distance_pose_2d)
|
|
{
|
|
geometry_msgs::msg::Pose pose1;
|
|
pose1.position.x = 7.0;
|
|
pose1.position.y = 4.0;
|
|
pose1.position.z = 3.0;
|
|
|
|
geometry_msgs::msg::Pose pose2;
|
|
pose2.position.x = 17.0;
|
|
pose2.position.y = 6.0;
|
|
pose2.position.z = 2.0;
|
|
|
|
ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.19804, 1e-5);
|
|
}
|
|
|
|
TEST(GeometryUtils, calculate_path_length)
|
|
{
|
|
nav_msgs::msg::Path straight_line_path;
|
|
size_t nb_path_points = 10;
|
|
float distance_between_poses = 2.0;
|
|
float current_x_loc = 0.0;
|
|
|
|
for (size_t i = 0; i < nb_path_points; ++i) {
|
|
geometry_msgs::msg::PoseStamped pose_stamped_msg;
|
|
pose_stamped_msg.pose.position.x = current_x_loc;
|
|
|
|
straight_line_path.poses.push_back(pose_stamped_msg);
|
|
|
|
current_x_loc += distance_between_poses;
|
|
}
|
|
|
|
ASSERT_NEAR(
|
|
calculate_path_length(straight_line_path),
|
|
(nb_path_points - 1) * distance_between_poses, 1e-5);
|
|
|
|
ASSERT_NEAR(
|
|
calculate_path_length(straight_line_path, straight_line_path.poses.size()),
|
|
0.0, 1e-5);
|
|
|
|
nav_msgs::msg::Path circle_path;
|
|
float polar_distance = 2.0;
|
|
uint32_t current_polar_angle_deg = 0;
|
|
constexpr float pi = 3.14159265358979;
|
|
|
|
while (current_polar_angle_deg != 360) {
|
|
float x_loc = polar_distance * std::cos(current_polar_angle_deg * (pi / 180.0));
|
|
float y_loc = polar_distance * std::sin(current_polar_angle_deg * (pi / 180.0));
|
|
|
|
geometry_msgs::msg::PoseStamped pose_stamped_msg;
|
|
pose_stamped_msg.pose.position.x = x_loc;
|
|
pose_stamped_msg.pose.position.y = y_loc;
|
|
|
|
circle_path.poses.push_back(pose_stamped_msg);
|
|
|
|
current_polar_angle_deg += 1;
|
|
}
|
|
|
|
ASSERT_NEAR(
|
|
calculate_path_length(circle_path),
|
|
2 * pi * polar_distance, 1e-1);
|
|
}
|