create Jazzy branch

CMakeLists.txt

@@ -1,9 +1,9 @@
 cmake_minimum_required(VERSION 3.16)
 project(diffdrive_arduino LANGUAGES CXX)
 
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
-  add_compile_options(-Wall -Wextra)
-endif()
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
 
 # find dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -13,13 +13,21 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
 )
 
+# Specify the required version of ros2_control
+find_package(controller_manager 4.0.0)
+# Handle the case where the required version is not found
+if(NOT controller_manager_FOUND)
+  message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. "
+  "Are you using the correct branch of the ros2_control_demos repository?")
+endif()
+
 # find dependencies
+find_package(backward_ros REQUIRED)
 find_package(ament_cmake REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
-
 ## COMPILE
 add_library(
   diffdrive_arduino
@@ -31,17 +39,14 @@ target_include_directories(diffdrive_arduino PUBLIC
 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
 $<INSTALL_INTERFACE:include/diffdrive_arduino>
 )
-ament_target_dependencies(
-  diffdrive_arduino PUBLIC
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+target_link_libraries(diffdrive_arduino PUBLIC
+  hardware_interface::hardware_interface
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  serial
 )
 
-target_link_libraries(diffdrive_arduino PUBLIC serial)
-
-# Causes the visibility macros to use dllexport rather than dllimport,
-# which is appropriate when building the dll but not consuming it.
-target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFFDRIVE_ARDUINO_BUILDING_DLL")
-
 # Export hardware plugins
 pluginlib_export_plugin_description_file(hardware_interface diffdrive_arduino.xml)
 

README.md

@@ -13,7 +13,10 @@ For a tutorial on how to develop a hardware interface like this, check out the v
 
 https://youtu.be/J02jEKawE5U
 
-
+## Run
+```bash
+ros2 launch diffdrive_arduino diffbot.launch.py device:=/dev/ttyUSB0
+```
 
 ## To Do
 

bringup/config/diffbot_controllers.yaml

@@ -5,17 +5,16 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    diffbot_base_controller:
-      type: diff_drive_controller/DiffDriveController
-
 diffbot_base_controller:
   ros__parameters:
+    type: diff_drive_controller/DiffDriveController
+
     left_wheel_names: ["left_wheel_joint"]
     right_wheel_names: ["right_wheel_joint"]
 
-    wheel_separation: 0.10
+    wheel_separation: 0.21
     #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
-    wheel_radius: 0.015
+    wheel_radius: 0.0325
 
     wheel_separation_multiplier: 1.0
     left_wheel_radius_multiplier: 1.0
@@ -27,12 +26,11 @@ diffbot_base_controller:
     pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
     twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
 
-    open_loop: true
-    enable_odom_tf: true
+    open_loop: false
+    enable_odom_tf: false
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits

bringup/launch/diffbot.launch.py

@@ -13,15 +13,45 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Start robot with mock hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "device",
+            default_value="/dev/ttyUSB0",
+            description="Serial device for hardware interface.",
+        )
+    )
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
+    device = LaunchConfiguration("device")
+
     # Get URDF via xacro
     robot_description_content = Command(
         [
@@ -30,6 +60,10 @@ def generate_launch_description():
             PathJoinSubstitution(
                 [FindPackageShare("diffdrive_arduino"), "urdf", "diffbot.urdf.xacro"]
             ),
+            " ",
+            "use_mock_hardware:=", use_mock_hardware,
+            " ",
+            "device:=", device,
         ]
     )
     robot_description = {"robot_description": robot_description_content}
@@ -48,36 +82,40 @@ def generate_launch_description():
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers],
+        parameters=[robot_controllers],
         output="both",
     )
     robot_state_pub_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         output="both",
         parameters=[robot_description],
-        remappings=[
-            ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
-        ],
     )
     rviz_node = Node(
         package="rviz2",
         executable="rviz2",
         name="rviz2",
         output="log",
         arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
     )
 
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        arguments=["joint_state_broadcaster"],
     )
 
     robot_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"],
+        arguments=[
+            "diffbot_base_controller",
+            "--param-file",
+            robot_controllers,
+            "--controller-ros-args",
+            "-r /diffbot_base_controller/cmd_vel:=/cmd_vel",
+        ],
     )
 
     # Delay rviz start after `joint_state_broadcaster`
@@ -88,20 +126,21 @@ def generate_launch_description():
         )
     )
 
-    # Delay start of robot_controller after `joint_state_broadcaster`
-    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    # Delay start of joint_state_broadcaster after `robot_controller`
+    # TODO(anyone): This is a workaround for flaky tests. Remove when fixed.
+    delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler(
         event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[robot_controller_spawner],
+            target_action=robot_controller_spawner,
+            on_exit=[joint_state_broadcaster_spawner],
         )
     )
 
     nodes = [
         control_node,
         robot_state_pub_node,
-        joint_state_broadcaster_spawner,
+        robot_controller_spawner,
         delay_rviz_after_joint_state_broadcaster_spawner,
-        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+        delay_joint_state_broadcaster_after_robot_controller_spawner,
     ]
 
-    return LaunchDescription(nodes)
+    return LaunchDescription(declared_arguments + nodes)

description/launch/view_robot.launch.py

@@ -14,6 +14,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 
 from launch_ros.actions import Node
@@ -38,6 +39,14 @@ def generate_launch_description():
             description="URDF/XACRO description file with the robot.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start Rviz2 and Joint State Publisher gui automatically \
+        with this launch file.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "prefix",
@@ -51,6 +60,7 @@ def generate_launch_description():
     # Initialize Arguments
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
+    gui = LaunchConfiguration("gui")
     prefix = LaunchConfiguration("prefix")
 
     # Get URDF via xacro
@@ -59,7 +69,7 @@ def generate_launch_description():
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
-                [FindPackageShare(description_package), "urdf", description_file]
+                [FindPackageShare("diffdrive_arduino"), "urdf", description_file]
             ),
             " ",
             "prefix:=",
@@ -75,6 +85,7 @@ def generate_launch_description():
     joint_state_publisher_node = Node(
         package="joint_state_publisher_gui",
         executable="joint_state_publisher_gui",
+        condition=IfCondition(gui),
     )
     robot_state_publisher_node = Node(
         package="robot_state_publisher",
@@ -88,6 +99,7 @@ def generate_launch_description():
         name="rviz2",
         output="log",
         arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
     )
 
     nodes = [

description/ros2_control/diffbot.ros2_control.xacro

@@ -1,23 +1,31 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="diffbot_ros2_control" params="name prefix">
+  <xacro:macro name="diffbot_ros2_control" params="name prefix use_mock_hardware device">
 
     <ros2_control name="${name}" type="system">
-      <hardware>
-        <plugin>diffdrive_arduino/DiffDriveArduinoHardware</plugin>
-        <param name="left_wheel_name">left_wheel_joint</param>
-        <param name="right_wheel_name">right_wheel_joint</param>
-        <param name="loop_rate">30</param>
-        <param name="device">/dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.3:1.0-port0</param>
-        <param name="baud_rate">57600</param>
-        <param name="timeout_ms">1000</param>
-        <param name="enc_counts_per_rev">3436</param>
-        <param name="pid_p">20</param>
-        <param name="pid_d">12</param>
-        <param name="pid_i">0</param>
-        <param name="pid_o">50</param>
-      </hardware>
+      <xacro:unless value="${use_mock_hardware}">
+        <hardware>
+          <plugin>diffdrive_arduino/DiffDriveArduinoHardware</plugin>
+          <param name="left_wheel_name">left_wheel_joint</param>
+          <param name="right_wheel_name">right_wheel_joint</param>
+          <param name="loop_rate">30</param>
+          <param name="device">${device}</param>
+          <param name="baud_rate">57600</param>
+          <param name="timeout_ms">1000</param>
+          <param name="enc_counts_per_rev">1320</param>
+          <param name="pid_p">20</param>
+          <param name="pid_d">12</param>
+          <param name="pid_i">0</param>
+          <param name="pid_o">50</param>
+        </hardware>
+      </xacro:unless>
+      <xacro:if value="${use_mock_hardware}">
+        <hardware>
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="calculate_dynamics">true</param>
+        </hardware>
+      </xacro:if>
       <joint name="${prefix}left_wheel_joint">
         <command_interface name="velocity"/>
         <state_interface name="position"/>

description/urdf/diffbot.urdf.xacro

@@ -2,6 +2,8 @@
 <!-- Basic differential drive mobile base -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="diffdrive_robot">
   <xacro:arg name="prefix" default="" />
+  <xacro:arg name="use_mock_hardware" default="false" />
+  <xacro:arg name="device" default="/dev/ttyUSB0" />
 
   <xacro:include filename="$(find diffdrive_arduino)/urdf/diffbot_description.urdf.xacro" />
 
@@ -14,6 +16,6 @@
   <xacro:diffbot prefix="$(arg prefix)" />
 
   <xacro:diffbot_ros2_control
-    name="DiffBot" prefix="$(arg prefix)" />
+    name="DiffBot" prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)" device="$(arg device)"/>
 
 </robot>