Add unit tests

Add rostest

CMakeLists.txt

@@ -82,3 +82,14 @@ install(
     TARGETS wpg_utils
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
+
+
+# Unit testing
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(${PROJECT_NAME}_utest launch/utest.test test/utest.cpp)
+    target_link_libraries(${PROJECT_NAME}_utest
+    wpg_utils
+    ${catkin_LIBRARIES}
+    )
+endif()

README.md

@@ -78,6 +78,19 @@ For example, triggering the playback after 3s.
 ```bash
 rosservice call /trigger_play 3
 ```
+## Unit Testing
+
+To run the unit test, ensure that the wayointgen package has been compiled previously.
+
+```bash
+catkin build waypointgen
+```
+
+After that, compile the test.
+
+```bash
+catkin test waypointgen
+```
 
 ## Changelog
 
@@ -99,3 +112,7 @@ rosservice call /trigger_play 3
 - Refactored code.
 - Removed custom msg.
 - Update launch files.
+
+### v0.1.3
+
+- Add unit tests & rostest for utils.

include/waypointgen/setpoint_marker.h

@@ -1,3 +1,6 @@
+#ifndef SETPOINT_MARKER_H
+#define SETPOINT_MARKER_H
+
 #include <boost/bind.hpp>
 #include <map>
 #include <ctime>
@@ -93,4 +96,6 @@ class waypointgen_marker {
 
   ros::NodeHandle nh_;
   std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
-};
+};
+
+#endif

include/waypointgen/setpoint_server.h

@@ -1,3 +1,6 @@
+#ifndef SETPOINT_SERVER_H
+#define SETPOINT_SERVER_H
+
 #include <chrono>
 #include <filesystem>
 #include <fstream>
@@ -143,3 +146,5 @@ class waypointgen_server {
       clock_type::now()};                  // Timer for benchmarking
   std::vector<bool> wpt_benchmark_success; // Benchmark vals
 };
+
+#endif

include/waypointgen/waypointgen_utils.h

@@ -1,3 +1,5 @@
+#ifndef WAYPOINTGEN_UTILS_H
+#define WAYPOINTGEN_UTILS_H
 
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/PoseWithCovariance.h>
@@ -27,3 +29,5 @@ class waypointgen_utils {
 
   static constexpr double PI = 3.1415926535897932385;
 };
+
+#endif

launch/utest.test

@@ -0,0 +1,3 @@
+<launch>
+	<test test-name="WPG Utils Unit Tests" pkg="waypointgen" type="waypointgen_utest" name="UTest"/>
+</launch>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>waypointgen</name>
-  <version>0.1.2</version>
+  <version>0.1.3</version>
   <description>The waypointgen package</description>
 
   <maintainer email="[email protected]">1487quantum</maintainer>
@@ -38,5 +38,6 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>yaml-cpp</exec_depend>
 
+  <test_depend>rosunit</test_depend>
 
 </package>

src/waypointgen_utils.cpp

@@ -3,8 +3,9 @@
 waypointgen_utils::waypointgen_utils(){};
 
 void waypointgen_utils::update_header(std_msgs::Header &hd,
-                                   const std::string &frameID) {
-  hd.frame_id = frameID; // reference frame
+                                      const std::string &frameID) {
+  if (!frameID.empty())
+    hd.frame_id = frameID; // reference frame
   hd.stamp = ros::Time::now();
 }
 
@@ -16,7 +17,22 @@ waypointgen_utils::addPoseCov(const geometry_msgs::Point &pt,
   quaternionTFToMsg(q_rotate, quat_msg);
 
   geometry_msgs::PoseWithCovariance cov_pose;
-  cov_pose.pose.position = pt;
+  auto isValidPoint{true};
+
+  if (pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
+    isValidPoint = 0;
+
+  geometry_msgs::Point fPoint;
+  if (!isValidPoint) {
+    //Return -1 if invalid num
+    geometry_msgs::Point tmp;
+    tmp.x = -1;
+    tmp.y = -1;
+    tmp.z = -1;
+    fPoint = tmp;
+  }
+
+  cov_pose.pose.position = isValidPoint ? pt : fPoint;
   cov_pose.pose.orientation = quat_msg;
   return cov_pose;
 }

test/utest.cpp

@@ -0,0 +1,92 @@
+#include <gtest/gtest.h>
+
+#include "waypointgen/waypointgen_utils.h"
+
+std_msgs::Header mHeader;
+std::string frameID = "world";
+
+waypointgen_utils wputil;
+
+auto genPoint(const double &x, const double &y, const double &z) {
+  geometry_msgs::Point a;
+  a.x = x;
+  a.y = y;
+  a.z = z;
+  return a;
+}
+
+auto genQuaternion(const double &x, const double &y, const double &z,
+                   const double &w) {
+  tf::Quaternion quat(x, y, z, w);
+  return quat;
+}
+
+// Util Test
+TEST(waypointgen_utils, frameStrTest) {
+  wputil.update_header(mHeader, frameID);
+  EXPECT_EQ(mHeader.frame_id, frameID);
+  wputil.update_header(mHeader, "");
+  EXPECT_EQ(mHeader.frame_id, mHeader.frame_id);
+  wputil.update_header(mHeader, " ");
+  EXPECT_EQ(mHeader.frame_id, mHeader.frame_id);
+}
+
+TEST(waypointgen_utils, addPosePositiveTest) {
+  auto qt1{genQuaternion(0, 0, 0, 1)};
+  auto pt1{genPoint(1, 2, 3)};
+  auto p1{wputil.addPoseCov(pt1, qt1)};
+  EXPECT_EQ(p1.pose.position, pt1);
+
+  auto pt2{genPoint(2.0f, 1.5f, 100.23f)};
+  auto p2{wputil.addPoseCov(pt2, qt1)};
+  EXPECT_EQ(p2.pose.position, pt2);
+
+  auto pt3{genPoint(3.1415926535897932382, 5.18346364, 1.618)};
+  auto p3{wputil.addPoseCov(pt3, qt1)};
+  EXPECT_EQ(p3.pose.position, pt3);
+}
+
+TEST(waypointgen_utils, addPoseNegativeTest) {
+  auto qt1{genQuaternion(0, 0, 0, 1)};
+  auto pt1{genPoint(-1, -2, -3)};
+  auto p1{wputil.addPoseCov(pt1, qt1)};
+  EXPECT_EQ(p1.pose.position, pt1);
+
+  auto pt2{genPoint(-2.0f, -1.5f, -100.23f)};
+  auto p2{wputil.addPoseCov(pt2, qt1)};
+  EXPECT_EQ(p2.pose.position, pt2);
+
+  auto pt3{genPoint(-3.1415926535897932382, -5.18346364, -1.618)};
+  auto p3{wputil.addPoseCov(pt3, qt1)};
+  EXPECT_EQ(p3.pose.position, pt3);
+}
+
+TEST(waypointgen_utils, addPoseZeroTest) {
+  auto qt1{genQuaternion(0, 0, 0, 1)};
+  auto pt1{genPoint(0, 0, 0)};
+  auto p1{wputil.addPoseCov(pt1, qt1)};
+  EXPECT_EQ(p1.pose.position, pt1);
+
+  auto pt2{genPoint(0.0f, 0.0f, 0.0f)};
+  auto p2{wputil.addPoseCov(pt2, qt1)};
+  EXPECT_EQ(p2.pose.position, pt2);
+
+  auto pt3{genPoint(0.0, 0.0, 0.0)};
+  auto p3{wputil.addPoseCov(pt3, qt1)};
+  EXPECT_EQ(p3.pose.position, pt3);
+}
+
+TEST(waypointgen_utils, addPoseUndefinedTest) {
+  auto qt1{genQuaternion(0, 0, 0, 1)};
+  auto pt1{genPoint(sqrt(-1), sqrt(-1), sqrt(-1))};
+  auto p1{wputil.addPoseCov(pt1, qt1)};
+  EXPECT_EQ(p1.pose.position, genPoint(-1, -1, -1));
+}
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_waypointgen");
+  ros::NodeHandle nh;
+  return RUN_ALL_TESTS();
+}