diff --git a/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml b/jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml new file mode 100644 index 0000000..d44d67d --- /dev/null +++ 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 diff --git a/jetson/ros2_ws/src/saltybot_description/launch/robot_description.launch.py b/jetson/ros2_ws/src/saltybot_description/launch/robot_description.launch.py new file mode 100644 index 0000000..f75366c --- /dev/null +++ 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, + ]) diff --git a/jetson/ros2_ws/src/saltybot_description/package.xml b/jetson/ros2_ws/src/saltybot_description/package.xml new file mode 100644 index 0000000..c5306c5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_description/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_description + 0.1.0 + + 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. + + sl-firmware + MIT + + robot_state_publisher + xacro + tf2_ros + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_description/resource/saltybot_description b/jetson/ros2_ws/src/saltybot_description/resource/saltybot_description new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_description/saltybot_description/__init__.py b/jetson/ros2_ws/src/saltybot_description/saltybot_description/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_description/setup.cfg b/jetson/ros2_ws/src/saltybot_description/setup.cfg new file mode 100644 index 0000000..b9e8148 --- /dev/null +++ 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 diff --git a/jetson/ros2_ws/src/saltybot_description/setup.py b/jetson/ros2_ws/src/saltybot_description/setup.py new file mode 100644 index 0000000..cb3d817 --- /dev/null +++ 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': [], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro b/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro new file mode 100644 index 0000000..6274f64 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_description/urdf/saltybot.urdf.xacro @@ -0,0 +1,286 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +