ros-controls · bmagyar · Nov 9, 2020 · Nov 12, 2020 · Nov 15, 2020 · Nov 18, 2020
diff --git a/.github/workspace.repos b/.github/workspace.repos
@@ -15,3 +15,7 @@ repositories:
     type: git
     url: https://github.com/ros/angles.git
     version: ros2
+  xacro:
+    type: git
+    url: https://github.com/ros/xacro.git
+    version: dashing-devel
diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -22,6 +23,7 @@ find_package(realtime_tools REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(urdf REQUIRED)
 
 add_library(diff_drive_controller SHARED
   src/diff_drive_controller.cpp
@@ -43,6 +45,8 @@ ament_target_dependencies(diff_drive_controller
   sensor_msgs
   tf2
   tf2_msgs
+  control_msgs
+  urdf
 )
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -61,34 +65,15 @@ install(TARGETS diff_drive_controller
 )
 
 if(BUILD_TESTING)
+  find_package(ros_testing REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   find_package(controller_manager REQUIRED)
   find_package(test_robot_hardware REQUIRED)
 
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(test_diff_drive_controller
-    test/test_diff_drive_controller.cpp
-    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)
-  target_include_directories(test_diff_drive_controller PRIVATE include)
-  target_link_libraries(test_diff_drive_controller
-    diff_drive_controller
-  )
-
-  ament_target_dependencies(test_diff_drive_controller
-    geometry_msgs
-    hardware_interface
-    nav_msgs
-    rclcpp
-    rclcpp_lifecycle
-    realtime_tools
-    sensor_msgs
-    test_robot_hardware
-    tf2
-    tf2_msgs
-  )
-
+  # Unit test: test_load_diff_drive_controller
   ament_add_gtest(
     test_load_diff_drive_controller
     test/test_load_diff_drive_controller.cpp
@@ -99,6 +84,7 @@ if(BUILD_TESTING)
     controller_manager
     test_robot_hardware
   )
+  # End Unit test: test_load_diff_drive_controller
 
   ament_add_gtest(
     test_accumulator
@@ -108,6 +94,42 @@ if(BUILD_TESTING)
   target_include_directories(test_accumulator PRIVATE include)
   ament_target_dependencies(test_accumulator)
 
+
+  # Unit test: test_diff_drive_controller
+  ament_add_gtest(test_diff_drive_controller
+    test/test_diff_drive_controller.cpp
+    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)
+  target_include_directories(test_diff_drive_controller PRIVATE include)
+  target_link_libraries(test_diff_drive_controller
+    diff_drive_controller
+  )
+  ament_target_dependencies(test_diff_drive_controller test_robot_hardware)
+  # End Unit test: test_diff_drive_controller
+
+  find_package(rosgraph_msgs REQUIRED)
+  find_package(std_srvs REQUIRED)
+  find_package(tf2_ros REQUIRED)
+
+  # Diffbot
+  # Robot with a DiffDriveController used for integration testing
+  add_executable(diffbot test/diffbot.hpp test/diffbot.cpp)
+  ament_target_dependencies(diffbot hardware_interface controller_manager rclcpp rosgraph_msgs lifecycle_msgs std_srvs)
+  # End Diffbot
+
+  # Gtest executables used in launch tests
+  set(launch_gtests
+    test_diff_drive
+  )
+
+  foreach(launch_gtest IN LISTS launch_gtests)
+    ament_add_gtest_executable(${launch_gtest}
+      test/test_common.hpp
+      test/${launch_gtest}.cpp)
+    ament_target_dependencies(${launch_gtest} tf2_ros std_srvs)
+    target_link_libraries(${launch_gtest} diff_drive_controller)
+    add_ros_test(test/${launch_gtest}.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+  endforeach()
+
 endif()
 
 ament_export_dependencies(

diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>controller_interface</depend>
+  <depend>control_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>nav_msgs</depend>
@@ -19,14 +20,19 @@
   <depend>realtime_tools</depend>
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
+  <depend>urdf</depend>
 
   <build_depend>pluginlib</build_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>controller_manager</test_depend>
+  <test_depend>ros_testing</test_depend>
+  <test_depend>std_srvs</test_depend>
   <test_depend>test_robot_hardware</test_depend>
+  <test_depend>tf2_ros</test_depend>
+  <test_depend>xacro</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/diff_drive_controller/test/config/diffbot.xacro b/diff_drive_controller/test/config/diffbot.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!--
+  Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+  -->
+  <xacro:include filename="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 -->
+  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+  <!-- 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/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+  <xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
+    <origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
+  </xacro:wheel>
+
+
+  <!-- Colour -->
+  <gazebo reference="base_link">
+    <material>Gazebo/Green</material>
+  </gazebo>
+
+  <gazebo reference="base_footprint">
+    <material>Gazebo/Purple</material>
+  </gazebo>
+
+</robot>
diff --git a/diff_drive_controller/test/config/diffbot.yaml b/diff_drive_controller/test/config/diffbot.yaml
@@ -0,0 +1,9 @@
+/**:  # Note: Wildcard necessary to permit parameter override via launch file
+  ros__parameters:
+    left_wheel_names: ["wheel_0_joint"]
+    right_wheel_names: ["wheel_1_joint"]
+    write_op_modes: ["op_status"]
+
+    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+    cmd_vel_timeout: 20000   # in ms
diff --git a/diff_drive_controller/test/config/wheel.xacro b/diff_drive_controller/test/config/wheel.xacro
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="wheel" params="name parent radius thickness *origin">
+    <link name="${name}_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <mass value="1"/>
+        <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${thickness}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${thickness}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_joint" type="continuous">
+      <parent link="${parent}_link"/>
+      <child link="${name}_link"/>
+      <xacro:insert_block name="origin"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+
+    <!-- Transmission is important to link the joints and the controller -->
+    <transmission name="${name}_joint_trans" type="SimpleTransmission">
+      <actuator name="${name}_joint_motor"/>
+      <joint name="${name}_joint"/>
+      <mechanicalReduction>1</mechanicalReduction>
+      <motorTorqueConstant>1</motorTorqueConstant>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <material>Gazebo/Red</material>
+    </gazebo>
+  </xacro:macro>
+</robot>
diff --git a/diff_drive_controller/test/diffbot.cpp b/diff_drive_controller/test/diffbot.cpp
@@ -0,0 +1,17 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "diffbot.hpp"
+
+int main(int argc, char ** argv) {diffbot_main_runner<2>(argc, argv);}