2026-02-28 23:08:46 -05:00
8 changed files with 457 additions and 0 deletions
--- a/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml
+++ b/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml
@ -0,0 +1,48 @@
+# saltybot_properties.yaml — SaltyBot robot dimensions
+# Sourced from chassis/chassis_frame.scad and chassis/ASSEMBLY.md  (Rev A 2026-02-28)
+# All values in SI units (meters, kilograms, radians).
+# Update after physical measurement of the built robot.
+
+robot:
+  # ── Drivetrain ─────────────────────────────────────────────────────────────
+  # WHEELBASE = 600mm (chassis_frame.scad)
+  wheel_separation: 0.600      # m, lateral center-to-center between axles
+  # 8-inch hoverboard hub motor: 203mm overall wheel+tire diameter / 2
+  wheel_radius: 0.1015         # m
+  # Hub motor housing width (measured, approx)
+  wheel_width: 0.065           # m
+
+  # ── Ground clearance / body height ─────────────────────────────────────────
+  # AXLE_HEIGHT = 310mm (chassis_frame.scad) — axle CL above flat ground
+  axle_height: 0.310           # m  (= base_footprint → base_link z-offset)
+
+  # ── Main deck ──────────────────────────────────────────────────────────────
+  # Deck: 640×220×6mm aluminum plate (chassis_frame.scad)
+  deck_length: 0.640           # m (x-axis, fore-aft)
+  deck_width:  0.220           # m (y-axis, lateral)
+  deck_thickness: 0.006        # m  DECK_THICKNESS = 6mm
+
+  # ── Stem ───────────────────────────────────────────────────────────────────
+  # 38.1mm (1.5") EMT conduit, cut ~1050mm  (stem_battery_clamp.scad)
+  stem_od:     0.0381          # m  STEM_OD = 38.1mm
+  stem_height: 1.050           # m  nominal cut length
+
+  # ── FC / IMU (MAMBA F722S) ──────────────────────────────────────────────────
+  # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105)
+  # z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm
+  imu_x:  0.050                # m  forward of base_link center
+  imu_y:  0.000                # m
+  imu_z:  0.012                # m  above base_link (pad + standoff)
+
+  # ── Sensor head (top of stem) ───────────────────────────────────────────────
+  # sensor_head.scad: ARM_R = 50mm, COL_H = 36mm
+  sensor_head_arm_radius: 0.050   # m  ARM_R — radial arm length from stem CL
+  sensor_head_collar_h:   0.036   # m  COL_H — collar height (RPLIDAR above ring)
+  # IMX219 and RealSense tilt 10° nose-down (from sensor_head.scad note)
+  camera_tilt_down_deg: 10        # degrees downward from horizontal
+
+  # ── Mass estimates (for inertials / Gazebo) ─────────────────────────────────
+  # TODO: update with scale measurements before simulation
+  chassis_mass: 3.0            # kg  deck + frame + forks
+  wheel_mass:   1.5            # kg  per wheel (hub motor)
+  payload_mass: 2.5            # kg  Jetson + battery + electronics
--- a/jetson/ros2_ws/src/saltybot_description/launch/robot_description.launch.py
+++ b/jetson/ros2_ws/src/saltybot_description/launch/robot_description.launch.py
@ -0,0 +1,59 @@
+"""
+robot_description.launch.py — Publish SaltyBot robot description + static TF
+
+Runs robot_state_publisher with the saltybot.urdf.xacro model.
+
+Publishes:
+  /robot_description   (std_msgs/String)  — full compiled URDF
+  /tf_static           (tf2_msgs/TFMessage) — all fixed joints:
+      base_footprint → base_link
+      base_link → imu_link
+      base_link → stem_link
+      base_link → sensor_head_link
+      sensor_head_link → laser
+      sensor_head_link → camera_link
+      sensor_head_link → camera_{front,left,rear,right}_link
+
+Continuous joints (wheel_left_link_joint, wheel_right_link_joint) are
+published only when /joint_states arrive from the motor controller.
+
+Typical usage:
+  ros2 launch saltybot_description robot_description.launch.py
+
+Desktop visualisation (publishes dummy /joint_states for wheels):
+  ros2 run joint_state_publisher_gui joint_state_publisher_gui
+
+Note: When this launch file is running, the static TF publishers in
+saltybot_cameras/launch/camera_tf.launch.py and the laser/camera TF
+nodes in saltybot_bringup/launch/sensors.launch.py become redundant.
+Remove or disable them to avoid TF tree conflicts.
+"""
+
+import os
+import xacro
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('saltybot_description')
+    urdf_file = os.path.join(pkg_share, 'urdf', 'saltybot.urdf.xacro')
+
+    # Compile xacro → URDF XML string at launch time
+    robot_description_content = xacro.process_file(urdf_file).toxml()
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': robot_description_content,
+            'use_sim_time': False,
+        }],
+    )
+
+    return LaunchDescription([
+        robot_state_publisher,
+    ])
--- a/jetson/ros2_ws/src/saltybot_description/package.xml
+++ b/jetson/ros2_ws/src/saltybot_description/package.xml
@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_description</name>
+  <version>0.1.0</version>
+  <description>
+    SaltyBot URDF robot description.
+    Defines the kinematic chain (base_footprint → base_link → wheels,
+    imu_link, sensor_head_link → laser / cameras) for use with
+    robot_state_publisher, SLAM, and Nav2.
+  </description>
+  <maintainer email="sl-firmware@saltylab.local">sl-firmware</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
--- a/jetson/ros2_ws/src/saltybot_description/resource/saltybot_description
+++ b/jetson/ros2_ws/src/saltybot_description/resource/saltybot_description
--- a/jetson/ros2_ws/src/saltybot_description/saltybot_description/init.py
+++ b/jetson/ros2_ws/src/saltybot_description/saltybot_description/init.py
--- a/jetson/ros2_ws/src/saltybot_description/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_description/setup.cfg
@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_description
+[install]
+install_scripts=$base/lib/saltybot_description
--- a/jetson/ros2_ws/src/saltybot_description/setup.py
+++ b/jetson/ros2_ws/src/saltybot_description/setup.py
@ -0,0 +1,32 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'saltybot_description'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+         ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'urdf'),
+         glob('urdf/*.xacro') + glob('urdf/*.urdf')),
+        (os.path.join('share', package_name, 'launch'),
+         glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'config'),
+         glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sl-firmware',
+    maintainer_email='sl-firmware@saltylab.local',
+    description='SaltyBot URDF robot description package',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [],
+    },
+)
--- a/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro
+++ b/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro
@ -0,0 +1,286 @@
+<?xml version="1.0"?>
+<!--
+  saltybot.urdf.xacro  —  SaltyBot robot description
+
+  Dimensions sourced from chassis/chassis_frame.scad and
+  chassis/ASSEMBLY.md (Rev A 2026-02-28).
+
+  REP-105 conventions: x=forward, y=left, z=up.
+
+  TF tree:
+    base_footprint
+    └── base_link  (z = axle_height = 0.310m)
+        ├── wheel_left_link    (y = +0.300m, continuous)
+        ├── wheel_right_link   (y = −0.300m, continuous)
+        ├── imu_link           (FC/MPU-6000, x=+0.050m, z=+0.012m)
+        ├── stem_link          (visual: 38.1mm EMT, 1.050m tall)
+        └── sensor_head_link   (top of stem, z = +1.050m)
+            ├── laser          (RPLIDAR A1M8, z=+0.036m — matches slam_toolbox)
+            ├── camera_link    (RealSense D435i, x=+0.050m — matches realsense2_camera)
+            ├── camera_front_link  (IMX219, x=+0.050m, 10° down)
+            ├── camera_left_link   (IMX219, y=+0.050m, 10° down)
+            ├── camera_rear_link   (IMX219, x=-0.050m, 10° down)
+            └── camera_right_link  (IMX219, y=-0.050m, 10° down)
+
+  Note: camera_front_link/left/rear/right match positions defined in
+  saltybot_cameras/launch/camera_tf.launch.py — that file may be retired
+  once this URDF is published by robot_state_publisher.
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="saltybot">
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       Properties  (SI: metres, kg, radians)
+       Update saltybot_properties.yaml when hardware dimensions change.
+       ═══════════════════════════════════════════════════════════════════ -->
+  <xacro:property name="pi"  value="3.14159265358979"/>
+
+  <!-- Drivetrain -->
+  <xacro:property name="wheel_separation" value="0.600"/>   <!-- WHEELBASE 600mm -->
+  <xacro:property name="wheel_radius"     value="0.1015"/>  <!-- 203mm tire ∅ / 2 -->
+  <xacro:property name="wheel_width"      value="0.065"/>   <!-- hub motor width   -->
+
+  <!-- Body -->
+  <xacro:property name="axle_height"    value="0.310"/>    <!-- AXLE_HEIGHT 310mm -->
+  <xacro:property name="deck_length"    value="0.640"/>
+  <xacro:property name="deck_width"     value="0.220"/>
+  <xacro:property name="deck_thickness" value="0.006"/>    <!-- DECK_THICKNESS 6mm -->
+
+  <!-- Stem (38.1mm EMT, ~1050mm cut) -->
+  <xacro:property name="stem_od"     value="0.0381"/>
+  <xacro:property name="stem_height" value="1.050"/>
+
+  <!-- Sensor head -->
+  <xacro:property name="sensor_head_collar_h" value="0.036"/>  <!-- COL_H 36mm -->
+  <xacro:property name="sensor_arm_r"         value="0.050"/>  <!-- ARM_R 50mm -->
+  <xacro:property name="cam_tilt"             value="0.1745"/> <!-- 10° in rad -->
+
+  <!-- FC / IMU offsets
+       fc_x = -50mm SCAD (front = -X SCAD = +X ROS);
+       z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm -->
+  <xacro:property name="imu_x" value="0.050"/>
+  <xacro:property name="imu_y" value="0.000"/>
+  <xacro:property name="imu_z" value="0.012"/>
+
+  <!-- Mass estimates -->
+  <xacro:property name="chassis_mass" value="3.0"/>
+  <xacro:property name="wheel_mass"   value="1.5"/>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       Materials
+       ═══════════════════════════════════════════════════════════════════ -->
+  <material name="aluminum">   <color rgba="0.75 0.75 0.75 1.0"/> </material>
+  <material name="black_rubber"><color rgba="0.15 0.15 0.15 1.0"/> </material>
+  <material name="dark_grey">  <color rgba="0.30 0.30 0.30 1.0"/> </material>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       Inertia macros
+       ═══════════════════════════════════════════════════════════════════ -->
+  <xacro:macro name="box_inertia" params="m l w h">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia
+        ixx="${m*(w*w + h*h)/12}" ixy="0" ixz="0"
+        iyy="${m*(l*l + h*h)/12}" iyz="0"
+        izz="${m*(l*l + w*w)/12}"/>
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="cylinder_inertia" params="m r h">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia
+        ixx="${m*(3*r*r + h*h)/12}" ixy="0" ixz="0"
+        iyy="${m*(3*r*r + h*h)/12}" iyz="0"
+        izz="${m*r*r/2}"/>
+    </inertial>
+  </xacro:macro>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       Wheel macro
+         name  — link name (e.g. "wheel_left_link")
+         side  — +1 = left (+Y), -1 = right (-Y)
+       ═══════════════════════════════════════════════════════════════════ -->
+  <xacro:macro name="wheel" params="name side">
+
+    <link name="${name}">
+      <visual>
+        <!-- rpy pi/2 0 0: cylinder principal axis → y, so it rolls laterally -->
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+        <material name="black_rubber"/>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
+    </link>
+
+    <joint name="${name}_joint" type="continuous">
+      <parent link="base_link"/>
+      <child  link="${name}"/>
+      <!-- REP-105: left = +y (side=+1), right = -y (side=-1) -->
+      <origin xyz="0 ${side * wheel_separation / 2} 0" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="0.01" friction="0.1"/>
+    </joint>
+
+  </xacro:macro>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       base_footprint  — virtual ground-projection frame (REP-105)
+       Origin: ground plane, centred between wheels.
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="base_footprint"/>
+
+  <joint name="base_footprint_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child  link="base_link"/>
+    <origin xyz="0 0 ${axle_height}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       base_link  — main body at axle height
+       Visual: deck plate (640×220×6mm aluminium).
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="${deck_length} ${deck_width} ${deck_thickness}"/>
+      </geometry>
+      <material name="aluminum"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="${deck_length} ${deck_width} ${deck_thickness}"/>
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="${chassis_mass}"
+                       l="${deck_length}" w="${deck_width}" h="${deck_thickness}"/>
+  </link>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       Wheels  (continuous joints; /joint_states from motor controller)
+       ═══════════════════════════════════════════════════════════════════ -->
+  <xacro:wheel name="wheel_left_link"  side=" 1"/>
+  <xacro:wheel name="wheel_right_link" side="-1"/>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       imu_link  — MPU-6000 on MAMBA F722S flight controller
+       fc_x = -50mm SCAD = +x ROS;  z = pad + standoff above deck = 12mm
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="imu_link"/>
+
+  <joint name="imu_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="imu_link"/>
+    <origin xyz="${imu_x} ${imu_y} ${imu_z}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       stem_link  — 38.1mm EMT conduit, 1050mm tall (visual only)
+       Frame origin at stem base (= base_link level); visual centred.
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="stem_link">
+    <visual>
+      <origin xyz="0 0 ${stem_height/2}" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="${stem_od/2}" length="${stem_height}"/>
+      </geometry>
+      <material name="dark_grey"/>
+    </visual>
+  </link>
+
+  <joint name="stem_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="stem_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       sensor_head_link  — top of stem; parent for all sensors
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="sensor_head_link"/>
+
+  <joint name="sensor_head_joint" type="fixed">
+    <parent link="base_link"/>
+    <child  link="sensor_head_link"/>
+    <origin xyz="0 0 ${stem_height}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       laser  — RPLIDAR A1M8 scan plane
+       Mounted on top of sensor-head collar (COL_H = 36mm above ring).
+       Frame name "laser" matches slam_toolbox config in saltybot_bringup.
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="laser"/>
+
+  <joint name="laser_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="laser"/>
+    <origin xyz="0 0 ${sensor_head_collar_h}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       camera_link  — RealSense D435i
+       Mounted on front sensor arm (ARM_R = 50mm), 10° nose-down tilt.
+       Frame name "camera_link" matches realsense2_camera launch files.
+       ═══════════════════════════════════════════════════════════════════ -->
+  <link name="camera_link"/>
+
+  <joint name="camera_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="camera_link"/>
+    <origin xyz="${sensor_arm_r} 0 0" rpy="0 ${cam_tilt} 0"/>
+  </joint>
+
+  <!-- ═══════════════════════════════════════════════════════════════════
+       IMX219 surround cameras  (4× at 90° on sensor-head ring)
+       Radial offset = ARM_R = 50mm from stem CL.
+       Facing direction: yaw set so x-axis of link points outward.
+       10° downward pitch (cam_tilt) for ground coverage.
+
+       Positions/yaws match saltybot_cameras/launch/camera_tf.launch.py.
+       When robot_state_publisher is live, camera_tf.launch.py static TFs
+       become redundant and should be removed to avoid TF conflicts.
+       ═══════════════════════════════════════════════════════════════════ -->
+
+  <!-- Front camera: faces robot +X (forward), x=+0.050m -->
+  <link name="camera_front_link"/>
+  <joint name="camera_front_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="camera_front_link"/>
+    <origin xyz="${sensor_arm_r} 0 0" rpy="0 ${cam_tilt} 0"/>
+  </joint>
+
+  <!-- Left camera: faces robot +Y (left), y=+0.050m, yaw=+90° -->
+  <link name="camera_left_link"/>
+  <joint name="camera_left_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="camera_left_link"/>
+    <origin xyz="0 ${sensor_arm_r} 0" rpy="0 ${cam_tilt} ${pi/2}"/>
+  </joint>
+
+  <!-- Rear camera: faces robot -X (rear), x=-0.050m, yaw=180° -->
+  <link name="camera_rear_link"/>
+  <joint name="camera_rear_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="camera_rear_link"/>
+    <origin xyz="${-sensor_arm_r} 0 0" rpy="0 ${cam_tilt} ${pi}"/>
+  </joint>
+
+  <!-- Right camera: faces robot -Y (right), y=-0.050m, yaw=-90° -->
+  <link name="camera_right_link"/>
+  <joint name="camera_right_joint" type="fixed">
+    <parent link="sensor_head_link"/>
+    <child  link="camera_right_link"/>
+    <origin xyz="0 ${-sensor_arm_r} 0" rpy="0 ${cam_tilt} ${-pi/2}"/>
+  </joint>
+
+</robot>