Add dwb tests (#2022)

* Add conversion tests.

* Add path_obs test. Resolution test is not completed. What is the expected behavior?

* Added more details and tests to messages.

* Added rotation checks.

* Update the test for the length of the segments.

* Changed back variable name to min_. Removed couts. Use EXPECT_TRUE.

* Also set the frame_id and check for the results.

* Update the copyright statement.

nav2_dwb_controller/nav_2d_utils/CMakeLists.txt

@@ -4,6 +4,7 @@ project(nav_2d_utils)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(nav_2d_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -18,6 +19,7 @@ set(dependencies
   geometry_msgs
   nav_2d_msgs
   nav_msgs
+  std_msgs
   rclcpp
   tf2
   tf2_geometry_msgs
@@ -65,6 +67,9 @@ if(BUILD_TESTING)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include)

nav2_dwb_controller/nav_2d_utils/package.xml

@@ -13,6 +13,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav_2d_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>nav2_msgs</depend>
@@ -20,6 +21,7 @@
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Wilco Bonestroo
+ *  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 the copyright holder 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 HOLDER 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.
+ */
+
+#include <math.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav_2d_utils/conversions.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+
+using nav_2d_utils::posesToPath;
+using nav_2d_utils::pathToPath;
+
+TEST(nav_2d_utils, PosesToPathEmpty)
+{
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  nav_msgs::msg::Path path = posesToPath(poses);
+
+  EXPECT_EQ(path.poses.size(), 0ul);
+}
+
+TEST(nav_2d_utils, PosesToPathNonEmpty)
+{
+  std::vector<geometry_msgs::msg::PoseStamped> poses;
+  geometry_msgs::msg::PoseStamped pose1;
+  rclcpp::Time time1, time2;
+  auto node = rclcpp::Node::make_shared("twod_utils_test_node");
+  time1 = node->now();
+
+  tf2::Quaternion quat1, quat2;
+  quat1.setRPY(0, 0, 0.123);
+  pose1.pose.position.x = 1.0;
+  pose1.pose.position.y = 2.0;
+  pose1.pose.orientation.w = quat1.w();
+  pose1.pose.orientation.x = quat1.x();
+  pose1.pose.orientation.y = quat1.y();
+  pose1.pose.orientation.z = quat1.z();
+  pose1.header.stamp = time1;
+  pose1.header.frame_id = "frame1_id";
+
+  geometry_msgs::msg::PoseStamped pose2;
+  pose2.pose.position.x = 4.0;
+  pose2.pose.position.y = 5.0;
+  quat2.setRPY(0, 0, 0.987);
+  pose2.pose.orientation.w = quat2.w();
+  pose2.pose.orientation.x = quat2.x();
+  pose2.pose.orientation.y = quat2.y();
+  pose2.pose.orientation.z = quat2.z();
+
+  time2 = node->now();
+  pose2.header.stamp = time2;
+  pose2.header.frame_id = "frame2_id";
+
+  poses.push_back(pose1);
+  poses.push_back(pose2);
+
+  nav_msgs::msg::Path path = posesToPath(poses);
+
+  EXPECT_EQ(path.poses.size(), 2ul);
+  EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
+  EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
+  EXPECT_EQ(path.poses[0].header.stamp, time1);
+  EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id");
+  EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
+  EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
+  EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id");
+
+  EXPECT_EQ(path.header.stamp, time1);
+}
+
+TEST(nav_2d_utils, PathToPathEmpty)
+{
+  nav_2d_msgs::msg::Path2D path2d;
+  nav_msgs::msg::Path path = pathToPath(path2d);
+  EXPECT_EQ(path.poses.size(), 0ul);
+}
+
+TEST(nav_2d_utils, PathToPathNoNEmpty)
+{
+  nav_2d_msgs::msg::Path2D path2d;
+
+  geometry_msgs::msg::Pose2D pose1;
+  pose1.x = 1.0;
+  pose1.y = 2.0;
+  pose1.theta = M_PI / 2.0;
+
+  geometry_msgs::msg::Pose2D pose2;
+  pose2.x = 4.0;
+  pose2.y = 5.0;
+  pose2.theta = M_PI;
+
+  path2d.poses.push_back(pose1);
+  path2d.poses.push_back(pose2);
+
+  nav_msgs::msg::Path path = pathToPath(path2d);
+  EXPECT_EQ(path.poses.size(), 2ul);
+  EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
+  EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
+
+  tf2::Quaternion quat;
+  quat.setRPY(0, 0, M_PI / 2.0);
+  EXPECT_EQ(path.poses[0].pose.orientation.w, quat.w());
+  EXPECT_EQ(path.poses[0].pose.orientation.x, quat.x());
+  EXPECT_EQ(path.poses[0].pose.orientation.y, quat.x());
+  EXPECT_EQ(path.poses[0].pose.orientation.z, quat.z());
+
+  EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
+  EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
+  quat.setRPY(0, 0, M_PI);
+  EXPECT_EQ(path.poses[1].pose.orientation.w, quat.w());
+  EXPECT_EQ(path.poses[1].pose.orientation.x, quat.x());
+  EXPECT_EQ(path.poses[1].pose.orientation.y, quat.x());
+  EXPECT_EQ(path.poses[1].pose.orientation.z, quat.z());
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt

+ament_add_gtest(2d_utils_tests 2d_utils_test.cpp)
+target_link_libraries(2d_utils_tests conversions)
+
+ament_add_gtest(path_ops_tests path_ops_test.cpp)
+target_link_libraries(path_ops_tests path_ops)
+

nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Wilco Bonestroo
+ *  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 the copyright holder 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 HOLDER 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.
+ */
+
+#include <cmath>
+#include "gtest/gtest.h"
+#include "nav_2d_utils/path_ops.hpp"
+
+using std::sqrt;
+using nav_2d_utils::adjustPlanResolution;
+
+TEST(path_ops_test, AdjustResolutionEmpty)
+{
+  nav_2d_msgs::msg::Path2D in;
+  nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, 2.0);
+  EXPECT_EQ(out.poses.size(), 0ul);
+}
+
+TEST(path_ops_test, AdjustResolutionSimple)
+{
+  nav_2d_msgs::msg::Path2D in;
+  const float RESOLUTION = 20.0;
+
+  geometry_msgs::msg::Pose2D pose1;
+  pose1.x = 0.0;
+  pose1.y = 0.0;
+  geometry_msgs::msg::Pose2D pose2;
+  pose2.x = 100.0;
+  pose2.y = 0.0;
+
+  in.poses.push_back(pose1);
+  in.poses.push_back(pose2);
+
+  nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, RESOLUTION);
+  float length = 100;
+  ulong number_of_points = ceil(length / (2 * RESOLUTION));
+  EXPECT_EQ(out.poses.size(), number_of_points);
+  float max_length = length / (number_of_points - 1);
+
+  for (unsigned int i = 1; i < out.poses.size(); i++) {
+    pose1 = out.poses[i - 1];
+    pose2 = out.poses[i];
+
+    double sq_dist = (pose1.x - pose2.x) * (pose1.x - pose2.x) +
+      (pose1.y - pose2.y) * (pose1.y - pose2.y);
+
+    EXPECT_TRUE(sqrt(sq_dist) <= max_length);
+  }
+}