Add IMU test

Author: efernandez

fuse_models/CMakeLists.txt

@@ -149,6 +149,7 @@ install(
 
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
+  find_package(catkin REQUIRED COMPONENTS angles)
 
   # Lint tests
   roslint_add_test()

fuse_models/package.xml

@@ -39,6 +39,7 @@
 
   <exec_depend>message_runtime</exec_depend>
 
+  <test_depend>angles</test_depend>
   <!-- The official rosdep has an entry for Google's benchmark deb
        (https://github.com/ros/rosdistro/blob/2cbcebdedcc1e827501acfecaf2386cd33f26809/rosdep/base.yaml#L232-L239)
        since https://github.com/ros/rosdistro/pull/23163 got merged.
@@ -52,6 +53,11 @@
        For this reason we conditionally test_depend on benchmark only if the $ROS_DISTRO is ROS Melodic or newer.
   -->
   <test_depend condition="$ROS_DISTRO >= melodic">benchmark</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>diff_drive_controller</test_depend>
+  <test_depend>gazebo_ros</test_depend>
+  <test_depend>joint_state_controller</test_depend>
+  <test_depend>robot_state_publisher</test_depend>
   <test_depend>rostest</test_depend>
 
   <export>

fuse_models/test/diffbot.launch

@@ -22,5 +22,9 @@
 
   <!-- Spawn controller -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner"
-        args="diffbot_controller"/>
+        args="joint_state_controller
+              diffbot_controller"/>
+
+  <!-- Convert joint states to TF transforms -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
 </launch>

fuse_models/test/diffbot.xacro

@@ -77,7 +77,7 @@
         <bodyName>imu_link</bodyName>
         <updateRateHZ>50.0</updateRateHZ>
         <frameName>imu_link</frameName>
-        <gaussianNoise>0.001</gaussianNoise>
+        <gaussianNoise>1.0e-6</gaussianNoise>
         <xyzOffset>0 0 0</xyzOffset>
         <rpyOffset>0 0 0</rpyOffset>
         <initialOrientationAsReference>false</initialOrientationAsReference>
@@ -93,6 +93,16 @@
     </plugin>
   </gazebo>
 
+  <!-- ground truth plugin -->
+  <gazebo>
+    <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
+      <bodyName>base_link</bodyName>
+      <topicName>ground_truth_odom</topicName>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>50.0</updateRate>
+    </plugin>
+  </gazebo>
+
   <!-- Colour -->
   <gazebo reference="base_link">
     <material>Gazebo/Green</material>

fuse_models/test/diffbot_controllers.yaml

@@ -1,3 +1,9 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# Diff drive controller
 diffbot_controller:
   type: "diff_drive_controller/DiffDriveController"
   left_wheel: 'wheel_0_joint'

fuse_models/test/imu.yaml

@@ -0,0 +1,7 @@
+imu:
+  topic: /imu
+  angular_velocity_dimensions: ['yaw']
+  differential: true
+  orientation_dimensions: ['yaw']
+  orientation_target_frame: &target_frame base_link
+  twist_target_frame: *target_frame

fuse_models/test/imu_2d.test

@@ -0,0 +1,12 @@
+<launch>
+  <!-- Simulat robot -->
+  <include file="$(find fuse_models)/test/diffbot.launch"/>
+
+  <!-- Load IMU sensor configuration -->
+  <rosparam file="$(find fuse_models)/test/imu.yaml" command="load" ns="imu_2d_test"/>
+
+  <!-- Run test -->
+  <test test-name="imu_2d_test" pkg="fuse_models" type="test_imu_2d" time-limit="120">
+    <remap from="odom" to="/ground_truth_odom"/>
+  </test>
+</launch>