add humble-navigation2

navigation2/nav2_graceful_controller/test/test_egopolar.cpp

@@ -0,0 +1,95 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// 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 "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_graceful_controller/ego_polar_coords.hpp"
+
+TEST(EgocentricPolarCoordinatesTest, constructorDefault) {
+  nav2_graceful_controller::EgocentricPolarCoordinates coords;
+
+  EXPECT_FLOAT_EQ(0.0, coords.r);
+  EXPECT_FLOAT_EQ(0.0, coords.phi);
+  EXPECT_FLOAT_EQ(0.0, coords.delta);
+}
+
+TEST(EgocentricPolarCoordinatesTest, constructorWithValues) {
+  float r_value = 5.0;
+  float phi_value = 1.2;
+  float delta_value = -0.5;
+
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value);
+
+  EXPECT_FLOAT_EQ(r_value, coords.r);
+  EXPECT_FLOAT_EQ(phi_value, coords.phi);
+  EXPECT_FLOAT_EQ(delta_value, coords.delta);
+}
+
+TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) {
+  geometry_msgs::msg::Pose target;
+  target.position.x = 3.0;
+  target.position.y = 4.0;
+  target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.8));
+
+  geometry_msgs::msg::Pose current;
+  current.position.x = 1.0;
+  current.position.y = 1.0;
+  current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2));
+
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current);
+
+  EXPECT_FLOAT_EQ(3.6055512428283691, coords.r);
+  EXPECT_FLOAT_EQ(-0.18279374837875384, coords.phi);
+  EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta);
+}
+
+TEST(EgocentricPolarCoordinatesTest, constructorFromPose) {
+  geometry_msgs::msg::Pose target;
+  target.position.x = 3.0;
+  target.position.y = 3.0;
+  target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI));
+
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(target);
+
+  EXPECT_FLOAT_EQ(4.2426405, coords.r);
+  EXPECT_FLOAT_EQ(2.3561945, coords.phi);
+  EXPECT_FLOAT_EQ(-M_PI / 4, coords.delta);
+}
+
+TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) {
+  geometry_msgs::msg::Pose target;
+  target.position.x = -3.0;
+  target.position.y = -4.0;
+  target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.8));
+
+  geometry_msgs::msg::Pose current;
+  current.position.x = 1.0;
+  current.position.y = 1.0;
+  current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2));
+
+  nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current, true);
+
+  EXPECT_FLOAT_EQ(6.4031243, coords.r);
+  EXPECT_FLOAT_EQ(-0.096055523, coords.phi);
+  EXPECT_FLOAT_EQ(-1.0960555, coords.delta);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool success = RUN_ALL_TESTS();
+  return success;
+}