locusrobotics · efernandez · Apr 12, 2022 · Apr 12, 2022
diff --git a/fuse_models/CMakeLists.txt b/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()
@@ -227,6 +228,22 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD_REQUIRED YES
   )
 
+  # Imu2D tests
+  add_rostest_gtest(
+    test_imu_2d
+    test/imu_2d.test
+    test/test_imu_2d.cpp
+  )
+  target_link_libraries(test_imu_2d
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+  )
+  set_target_properties(test_imu_2d
+    PROPERTIES
+      CXX_STANDARD 14
+      CXX_STANDARD_REQUIRED YES
+  )
+
   # Unicycle2D Ignition tests
   add_rostest_gtest(
     test_unicycle_2d_ignition

diff --git a/fuse_models/package.xml b/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>

diff --git a/fuse_models/test/diffbot.launch b/fuse_models/test/diffbot.launch
@@ -0,0 +1,30 @@
+<launch>
+  <arg name="gui" default="false"/>
+
+  <!-- Start world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+
+  <!-- Load diffbot model -->
+  <param name="robot_description"
+         command="$(find xacro)/xacro '$(find fuse_models)/test/diffbot.xacro'"/>
+
+  <!-- Load diffbot config, e.g. PID gains -->
+  <rosparam command="load" file="$(find fuse_models)/test/diffbot.yaml"/>
+
+  <!-- Spawn diffbot robot in gazebo -->
+  <node name="spawn_diffbot" pkg="gazebo_ros" type="spawn_model"
+        args="-urdf -param robot_description -model diffbot -z 0.5"/>
+
+  <!-- Load controller config -->
+  <rosparam command="load" file="$(find fuse_models)/test/diffbot_controllers.yaml"/>
+
+  <!-- Spawn controller -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner"
+        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>
diff --git a/fuse_models/test/diffbot.xacro b/fuse_models/test/diffbot.xacro
@@ -0,0 +1,114 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find fuse_models)/test/wheel.xacro"/>
+
+  <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+  <!-- Constants for robot dimensions -->
+  <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+  <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+  <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+
+  <!-- Links: inertial,visual,collision -->
+  <link name="base_link">
+    <inertial>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="5"/>
+      <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="${width} 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <!-- origin is relative -->
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="${width} 0.1 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="base_footprint">
+    <visual>
+      <geometry>
+        <sphere radius="0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.00000001"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+  </joint>
+
+  <!-- Wheels -->
+  <xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${(width+thickness)/2} 0 0" rpy="0 ${90.0 * deg_to_rad} 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-(width+thickness)/2} 0 0" rpy="0 ${90.0 * deg_to_rad} 0"/>
+  </xacro:wheel>
+
+  <!-- IMU -->
+  <link name="imu_link"/>
+  <joint name="imu_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="imu_link" />
+    <origin xyz="0.1 -0.2 0.3" rpy="0.0 ${-90.0 * deg_to_rad} ${90.0 * deg_to_rad}"/>
+  </joint>
+
+  <gazebo reference="imu_link">
+    <gravity>true</gravity>
+    <sensor name="imu_sensor" type="imu">
+      <always_on>true</always_on>
+      <update_rate>50.0</update_rate>
+      <visualize>true</visualize>
+      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+        <topicName>imu</topicName>
+        <bodyName>imu_link</bodyName>
+        <updateRateHZ>50.0</updateRateHZ>
+        <frameName>imu_link</frameName>
+        <gaussianNoise>1.0e-6</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <initialOrientationAsReference>false</initialOrientationAsReference>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </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>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+</robot>
diff --git a/fuse_models/test/diffbot.yaml b/fuse_models/test/diffbot.yaml
@@ -0,0 +1,6 @@
+gazebo_ros_control:
+  pid_gains:
+    wheel_0_joint:
+      p: &p 100.0
+    wheel_1_joint:
+      p: *p
diff --git a/fuse_models/test/diffbot_controllers.yaml b/fuse_models/test/diffbot_controllers.yaml
@@ -0,0 +1,16 @@
+# 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'
+  right_wheel: 'wheel_1_joint'
+  publish_rate: 50.0 # defaults to 50
+  initial_pose_covariance_diagonal:  [0.001, 0.001, 0.01]
+  cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
+  k_l: 0.01
+  k_r: 0.01
+  wheel_resolution: 0.001
diff --git a/fuse_models/test/imu.yaml b/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
diff --git a/fuse_models/test/imu_2d.test b/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>