Initial Warthog addition

clearpath_control/config/twist_mux.yaml

@@ -7,9 +7,13 @@ twist_mux:
     topics.interactive_marker.topic: twist_marker_server/cmd_vel
     topics.interactive_marker.timeout: 0.5
     topics.interactive_marker.priority: 8
+    topics.rc.topic: rc_teleop/cmd_vel
+    topics.rc.timeout: 0.5
+    topics.rc.priority: 12
     topics.external.topic: cmd_vel
     topics.external.timeout: 0.5
     topics.external.priority: 1
     locks.e_stop.topic: platform/emergency_stop
     locks.e_stop.timeout: 0.0
-    locks.e_stop.priority: 255
+    locks.e_stop.priority: 255
+

clearpath_control/config/w200/control.yaml

@@ -0,0 +1,69 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz
+    use_sim_time: False
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    platform_velocity_controller:
+      type: diff_drive_controller/DiffDriveController
+
+platform_velocity_controller:
+  ros__parameters:
+    use_sim_time: False
+    left_wheel_names: [ "front_left_wheel_joint", "rear_left_wheel_joint" ]
+    right_wheel_names: [ "front_right_wheel_joint", "rear_right_wheel_joint" ]
+
+    wheel_separation: 1.08 
+    wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.098 # TODO: Update this
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
+
+    open_loop: false
+    enable_odom_tf: false
+
+    cmd_vel_timeout: 0.25
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10
+
+    # Preserve turning radius when limiting speed/acceleration/jerk
+    preserve_turning_radius: true
+
+    # Publish limited velocity
+    publish_cmd: true
+
+    # Publish wheel data
+    publish_wheel_data: true
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 5.0
+    linear.x.min_velocity: -5.0
+    linear.x.max_acceleration: 50.0
+    linear.x.min_acceleration: -50.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 4.0
+    angular.z.min_velocity: -4.0
+    angular.z.max_acceleration: 40.0
+    angular.z.min_acceleration: -40.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0

clearpath_control/config/w200/localization.yaml

@@ -0,0 +1,19 @@
+ekf_node:
+  ros__parameters:
+    odom_frame: odom
+    base_link_frame: base_link
+    world_frame: odom
+    publish_tf: true
+    two_d_mode: true
+    use_sim_time: false
+
+    frequency: 50.0
+
+    odom0: platform/odom
+    odom0_config: [false, false, false,
+                   false, false, false,
+                   true, true, false,
+                   false, false, true,
+                   false, false, false]
+    odom0_differential: false
+    odom0_queue_size: 10

clearpath_control/config/w200/teleop_interactive_markers.yaml

@@ -0,0 +1,8 @@
+twist_server_node:
+  ros__parameters:
+    use_sim_time: False
+    link_name: base_link
+    robot: j100
+    linear_scale: 1.0
+    max_positive_linear_velocity: 1.0
+    max_negative_linear_velocity: -1.0

clearpath_control/config/w200/teleop_logitech.yaml

@@ -0,0 +1,66 @@
+# Teleop configuration for Logitech F710 Gamepad using the x-pad configuration.
+# Left thumb-stick up/down for velocity, left/right for twist
+# LB for enable
+# RB for enable-turbo
+#
+#         (LB)                                 (RB)
+#         (LT)                                 (RT)
+#       _=====_            D(  .)X            _=====_
+#      / _____ \                             / _____ \
+#    +.-'_____'-.---------------------------.-'_____'-.+
+#   /   |     |  '.                       .'  |      |   \
+#  / ___| /|\ |___ \ (back)(Lgtc)(strt)  / ___| (Y)  |___ \
+# / |      |      | ;  __           __  ; |              | |
+# | | <---   ---> | | (__) .       (__) | | (X)       (B)| |
+# | |___   |   ___| ; MODE         VIBE ; |___       ____| /
+# |\    | \|/ |    /  _     ___      _   \    | (A) |    /|
+# | \   |_____|  .','" "', |___|  ,'" "', '.  |_____|  .' |
+# |  '-.______.-' /       \ANALOG/       \  '-._____.-'   |
+# |               |  LJ   |------|   RJ  |                |
+# |              /\       /      \       /\               |
+# |             /  '.___.'        '.___.'  \              |
+# |            /                            \             |
+#  \          /                              \           /
+#   \________/                                \_________/
+#
+# BUTTON         Value
+#   LB             4
+#   RB             5
+#   A              0
+#   B              1
+#   X              2
+#   Y              3
+#
+#    AXIS        Value
+# Left Horiz.      0
+# Left Vert.       1
+# Right Horiz.     3
+# Right Vert.      4
+# Left Trigger     2
+# Right Trigger    5
+# D-pad Horiz.     6
+# D-pad Vert.      7
+
+joy_teleop/teleop_twist_joy_node:
+  ros__parameters:
+    use_sim_time: False
+    axis_linear:
+      x: 1
+    scale_linear:
+      x: 0.5
+    scale_linear_turbo:
+      x: 1.0
+    axis_angular:
+      yaw: 0
+    scale_angular:
+      yaw: 2.4
+    scale_angular_turbo:
+      yaw: 2.4
+    enable_button: 4
+    enable_turbo_button: 5
+joy_teleop/joy_node:
+  ros__parameters:
+    use_sim_time: False
+    deadzone: 0.1
+    autorepeat_rate: 20.0
+    dev: /dev/input/f710

clearpath_control/config/w200/teleop_ps4.yaml

@@ -0,0 +1,79 @@
+# Teleop configuration for PS4 joystick using the x-pad configuration.
+# Left thumb-stick up/down for velocity, left/right for twist
+# Left shoulder button for enable
+# Right shoulder button for enable-turbo
+#
+#          L1                                       R1
+#          L2                                       R2
+#       _=====_                                  _=====_
+#      / _____ \                                / _____ \
+#    +.-'_____'-.------------------------------.-'_____'-.+
+#   /   |     |  '.        S O N Y           .'  |  _  |   \
+#  / ___| /|\ |___ \                        / ___| /_\ |___ \      (Y)
+# / |      |      | ;                      ; | _         _ ||
+# | | <---   ---> | |                      | ||_|       (_)||  (X)     (B)
+# | |___   |   ___| ;                      ; |___       ___||
+# |\    | \|/ |    /  _      ____      _   \    | (X) |    /|      (A)
+# | \   |_____|  .','" "',  (_PS_)  ,'" "', '.  |_____|  .' |
+# |  '-.______.-' /       \        /       \  '-._____.-'   |
+# |               |  LJ   |--------|  RJ   |                |
+# |              /\       /        \       /\               |
+# |             /  '.___.'          '.___.'  \              |
+# |            /                              \             |
+#  \          /                                \           /
+#   \________/                                  \_________/
+#
+#          ^ x
+#          |
+#          |
+#  y <-----+      Accelerometer axes
+#           \
+#            \
+#             > z (out)
+#
+# BUTTON         Value
+#   L1             4
+#   L2             6
+#   R1             5
+#   R2             7
+#   A              1
+#   B              2
+#   X              0
+#   Y              3
+#
+#    AXIS        Value
+# Left Horiz.      0
+# Left Vert.       1
+# Right Horiz.     2
+# Right Vert.      5
+#    L2            3
+#    R2            4
+# D-pad Horiz.     9
+# D-pad Vert.      10
+# Accel x          7
+# Accel y          6
+# Accel z          8
+
+joy_teleop/teleop_twist_joy_node:
+  ros__parameters:
+    use_sim_time: False
+    axis_linear:
+      x: 1
+    scale_linear:
+      x: 0.5
+    scale_linear_turbo:
+      x: 1.0
+    axis_angular:
+      yaw: 0
+    scale_angular:
+      yaw: 2.4
+    scale_angular_turbo:
+      yaw: 2.4
+    enable_button: 4
+    enable_turbo_button: 5
+joy_teleop/joy_node:
+  ros__parameters:
+    use_sim_time: False
+    deadzone: 0.1
+    autorepeat_rate: 20.0
+    dev: /dev/input/ps4

clearpath_platform_description/urdf/w200/attachments/warthog_arm_mount.urdf.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="warthog_arm_mount">
+    <link name="arm_mount_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/arm-mount-plate.stl" />
+        </geometry>
+        <material name="light_grey"/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/arm-mount-plate.stl" />
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0 0 -0.01" rpy="0 0 0"/>
+        <mass value="1.45"/>
+        <xacro:box_inertia m="1.45" x="0.16838" y="0.16838" z="0.02" />
+      </inertial>
+    </link>
+  </xacro:macro>
+</robot>

clearpath_platform_description/urdf/w200/attachments/warthog_bulkhead.urdf.xacro

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="warthog_bulkhead">
+    <link name="bulkhead_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/bulkhead.stl" />
+        </geometry>
+        <material name="black"><color rgba="0.15 0.15 0.15 1.0" /></material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/bulkhead-collision.stl" />
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0.35915 0 -0.6956" rpy="0 0 0"/>
+        <mass value="14.38835"/>
+        <inertia ixx="1.13395" ixy="0.0" ixz="-0.42719" iyy="2.59727" iyz="0.0" izz="3.42344"/>
+      </inertial>
+    </link>
+
+    <link name="bulkhead_front_mount_link"/>
+  </xacro:macro>
+</robot>

clearpath_platform_description/urdf/w200/attachments/warthog_generator.urdf.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="warthog_generator">
+    <link name="generator_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/generator.stl" />
+        </geometry>
+        <material name="black"><color rgba="0.15 0.15 0.15 1.0" /></material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://warthog_description/meshes/generator.stl" />
+        </geometry>
+      </collision>
+      <inertial>
+        <origin xyz="0.35915 0 -0.6956" rpy="0 0 0"/>
+        <mass value="14.38835"/>
+        <inertia ixx="1.13395" ixy="0.0" ixz="-0.42719" iyy="2.59727" iyz="0.0" izz="3.42344"/>
+      </inertial>
+    </link>
+
+  </xacro:macro>
+</robot>