add test: SubjuGator can spawn in Gazebo (#1201)

* add test: SubjuGator can spawn in Gazebo

* Testing new checkout action

* pre-commit changes

.github/workflows/ci.yaml

@@ -33,7 +33,7 @@ jobs:
           mkdir -p $GITHUB_WORKSPACE/catkin_ws/src
           sudo apt reinstall python3-pip
       - name: Check out code from GitHub
-        uses: actions/checkout@v3.0.2
+        uses: actions/checkout@v4
         with:
           submodules: recursive
           path: catkin_ws/src/mil
@@ -106,7 +106,7 @@ jobs:
     if: github.ref == 'refs/heads/master'
     steps:
       - name: Checkout
-        uses: actions/checkout@v3
+        uses: actions/checkout@v4
 
       - name: Make folders
         run: |

SubjuGator/simulation/subjugator_gazebo/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin
     rospy
     gazebo_ros
     roscpp
+    mil_tools
     message_generation xacro)
 
 catkin_python_setup()
@@ -107,7 +108,7 @@ target_link_libraries(
 catkin_package(
     INCLUDE_DIRS include
     LIBRARIES subjugator_buoyancy subjugator_thrusters subjugator_state_set subjugator_liftdrag
-    CATKIN_DEPENDS gazebo_ros roscpp rospy std_msgs message_runtime
+    CATKIN_DEPENDS gazebo_ros roscpp rospy std_msgs message_runtime mil_tools
     DEPENDS
 )
 
@@ -119,3 +120,11 @@ xacro_add_files(
   INSTALL DESTINATION urdf
   TARGET xacro_urdf
 )
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(subjugator_gazebo_spawn_test
+                    test/subjugator_gazebo.test
+                    test/spawn_test.cpp)
+  target_link_libraries(subjugator_gazebo_spawn_test ${catkin_LIBRARIES})
+endif()

SubjuGator/simulation/subjugator_gazebo/package.xml

@@ -10,6 +10,7 @@
   <depend>geometry_msgs</depend>
   <depend>message_generation</depend>
   <depend>mil_gazebo</depend>
+  <depend>mil_tools</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>
   <depend>std_msgs</depend>

SubjuGator/simulation/subjugator_gazebo/test/spawn_test.cpp

@@ -0,0 +1,25 @@
+/**
+ * Author: Cameron Brown
+ * Date: June 1, 2024
+ */
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+
+#include <mil_tools/test.hpp>
+
+using namespace mil_tools;
+
+// Ensure that at least sub8 or sub9 shows up when launching gazebo
+TEST(SpawnTest, spawnTest)
+{
+  ros::NodeHandle nh;
+  EXPECT_TRUE(mil_tools::gazeboModelExists(nh, "sub8") || mil_tools::gazeboModelExists(nh, "sub8_no_cams") ||
+              mil_tools::gazeboModelExists(nh, "sub9"));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "subjugator_gazebo_spawn_test");
+  return RUN_ALL_TESTS();
+}

SubjuGator/simulation/subjugator_gazebo/test/subjugator_gazebo.test

@@ -0,0 +1,7 @@
+<launch>
+  <include file="$(find subjugator_gazebo)/launch/duck.launch">
+    <arg name="gui" value="false" />
+  </include>
+  <include file="$(find subjugator_launch)/launch/tf.launch"></include>
+  <test test-name="subjugator_gazebo_spawn_test" pkg="subjugator_gazebo" type="subjugator_gazebo_spawn_test" />
+</launch>

mil_common/utils/mil_tools/include/mil_tools/test.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <gazebo_msgs/ModelStates.h>
+#include <ros/ros.h>
+
+namespace mil_tools
+{
+bool gazeboModelExists(ros::NodeHandle &nh, const std::string &_name,
+                       const ros::WallDuration _timeout = ros::WallDuration(5, 0))
+{
+  ros::WallTime timeout = ros::WallTime::now() + _timeout;
+  while (ros::WallTime::now() < timeout)
+  {
+    gazebo_msgs::ModelStatesConstPtr modelStates = ros::topic::waitForMessage<gazebo_msgs::ModelStates>(
+        std::string("/gazebo/model_states"), nh, ros::Duration(0.3));
+
+    if (!modelStates)
+      continue;
+    for (auto model : modelStates->name)
+    {
+      if (model == _name)
+        return true;
+    }
+  }
+  return false;
+}
+}  // namespace mil_tools

mil_common/utils/mil_tools/src/mil_tools/mil_tools.cpp

@@ -1,6 +1,7 @@
 #include <mil_tools/mil_tools.hpp>
 #include <mil_tools/msg_helpers.hpp>
 #include <mil_tools/param_helpers.hpp>
+#include <mil_tools/test.hpp>
 
 namespace mil_tools
 {

scripts/setup.bash

@@ -109,28 +109,28 @@ RED='\033[0;31m'
 # cm <package> --> catkin_make --only-pkg-with-deps <package>
 # cm <package> --test --> catkin_make --only-pkg-with-deps <package> run_tests
 cm() {
-    if [ $# -eq 0 ]; then
-        catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -DCATKIN_WHITELIST_PACKAGES="" -C "$MIL_WS"
-        mv "$MIL_WS/build/compile_commands.json" "$MIL_WS"
-    else
-        if [[ "$1" == "--test" ]]; then
-            catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=1 run_tests -C "$MIL_WS"
-            mv "$MIL_WS/build/compile_commands.json" "$MIL_WS"
-        elif [[ "$2" == "--test" ]]; then
-            # Build specific package then run tests
-            cd "$MIL_WS" || return
-            catkin_make --only-pkg-with-deps "$1"
-            catkin_make run_tests --only-pkg-with-deps "$1" -C "$MIL_WS"
-            cd - > /dev/null || exit
-            echo -e "${RED}!! Warning: Future calls to catkin_make will just build the '$1' package. To revert this, ensure you run 'cm' or 'cd $MIL_WS && catkin_make -DCATKIN_WHITELIST_PACKAGES=\"\"' when you want to recompile the entire repository.\e[0m"
-        else
-            # Build specific package
-            cd "$MIL_WS" || return
-            catkin_make --only-pkg-with-deps "$1"
-            cd - > /dev/null || exit
-            echo -e "${RED}!! Warning: Future calls to catkin_make will just build the '$1' package. To revert this, ensure you run 'cm' or 'cd $MIL_WS && catkin_make -DCATKIN_WHITELIST_PACKAGES=\"\"' when you want to recompile the entire repository.\e[0m"
-        fi
-    fi
+	if [ $# -eq 0 ]; then
+		catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -DCATKIN_WHITELIST_PACKAGES="" -C "$MIL_WS"
+		mv "$MIL_WS/build/compile_commands.json" "$MIL_WS"
+	else
+		if [[ $1 == "--test" ]]; then
+			catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=1 run_tests -C "$MIL_WS"
+			mv "$MIL_WS/build/compile_commands.json" "$MIL_WS"
+		elif [[ $2 == "--test" ]]; then
+			# Build specific package then run tests
+			cd "$MIL_WS" || return
+			catkin_make --only-pkg-with-deps "$1"
+			catkin_make run_tests --only-pkg-with-deps "$1" -C "$MIL_WS"
+			cd - >/dev/null || exit
+			echo -e "${RED}!! Warning: Future calls to catkin_make will just build the '$1' package. To revert this, ensure you run 'cm' or 'cd $MIL_WS && catkin_make -DCATKIN_WHITELIST_PACKAGES=\"\"' when you want to recompile the entire repository.\e[0m"
+		else
+			# Build specific package
+			cd "$MIL_WS" || return
+			catkin_make --only-pkg-with-deps "$1"
+			cd - >/dev/null || exit
+			echo -e "${RED}!! Warning: Future calls to catkin_make will just build the '$1' package. To revert this, ensure you run 'cm' or 'cd $MIL_WS && catkin_make -DCATKIN_WHITELIST_PACKAGES=\"\"' when you want to recompile the entire repository.\e[0m"
+		fi
+	fi
 }
 
 alias xbox=startxbox