diff --git a/.gitignore b/.gitignore index 259148f..8646a11 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,13 @@ *.exe *.out *.app + +# Python +__pycache__/ +*.pyc +*.pyo +*.pyd +.Python +*.egg-info/ +dist/ +build/ diff --git a/hunter_pltf_bringup/launch/hunter_pltf_bringup.launch.py b/hunter_pltf_bringup/launch/hunter_pltf_bringup.launch.py index f53c85f..85632e6 100755 --- a/hunter_pltf_bringup/launch/hunter_pltf_bringup.launch.py +++ b/hunter_pltf_bringup/launch/hunter_pltf_bringup.launch.py @@ -13,13 +13,15 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from ament_index_python.packages import get_package_share_directory from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare import os +import sys +from pathlib import Path def generate_launch_description(): @@ -33,6 +35,7 @@ def generate_launch_description(): is_sim = LaunchConfiguration('is_sim' , default='false') enable_pd_regulator = LaunchConfiguration('enable_pd_regulator', default='False') use_sim_time = LaunchConfiguration('use_sim_time', default='False') + robot_id = LaunchConfiguration('robot_id', default='default') gui_declare = DeclareLaunchArgument( "gui", default_value=gui, description="Start RViz2 automatically with this launch file.") @@ -50,24 +53,43 @@ def generate_launch_description(): enable_pd_regulator_declare = DeclareLaunchArgument('enable_pd_regulator', default_value=enable_pd_regulator , description='Use PD regulator estimate residual control to the robot') - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("hunter_pltf_description"), "description" ,"hunter_pltf.urdf.xacro"] - ), - " ", - "is_sim:=", - is_sim, - " ", - "prefix:=''", - " ", - ] - ) + def generate_robot_description(context, *args, **kwargs): + """Generate robot description with robot-specific configuration.""" + is_sim_val = LaunchConfiguration('is_sim').perform(context) + robot_id_val = LaunchConfiguration('robot_id').perform(context) + + # Import config loader + config_dir = Path(get_package_share_directory('hunter_pltf_description')) / 'config' / 'robots' + sys.path.insert(0, str(config_dir)) + from config_loader import load_robot_config, get_xacro_args, format_xacro_args + + # Load robot configuration + is_sim_bool = is_sim_val.lower() == 'true' + config = load_robot_config(robot_id_val) + xacro_args = get_xacro_args(config, is_sim_bool) + + # Build xacro command with all arguments + xacro_file = PathJoinSubstitution( + [FindPackageShare("hunter_pltf_description"), "description", "hunter_pltf.urdf.xacro"] + ) + + # Format all arguments + args_str = f"is_sim:={is_sim_val} prefix:='' {format_xacro_args(xacro_args)}" + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + xacro_file, + " ", + args_str, + ] + ) + + return robot_description_content - robot_description = {"robot_description": robot_description_content} + # Note: robot_description will be generated in the OpaqueFunction + # to allow runtime evaluation of robot_id robot_controllers = PathJoinSubstitution( [ @@ -79,31 +101,46 @@ def generate_launch_description(): base_launch = os.path.join(get_package_share_directory("hunter_base"), "launch", "hunter_base.launch.py") - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - output="both", - ) - - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) + def launch_setup(context, *args, **kwargs): + """Setup launch nodes with robot configuration.""" + robot_description_content = generate_robot_description(context) + robot_description = {"robot_description": robot_description_content} + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + hunter_base_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource(base_launch), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'kp_v': kp_v, + 'kd_v': kd_v, + 'kp_w': kp_w, + 'kd_w': kd_w, + 'enable_pd_regulator': enable_pd_regulator + }.items(), + ) + + return [ + robot_state_pub_node, + hunter_base_node, + ] - hunter_base_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource(base_launch), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'kp_v': kp_v, - 'kd_v': kd_v, - 'kp_w': kp_w, - 'kd_w': kd_w, - 'enable_pd_regulator': enable_pd_regulator - }.items(), - ) + robot_id_declare = DeclareLaunchArgument( + 'robot_id', default_value='default', + description='Robot instance ID (e.g., hunter_01, hunter_02). Uses robot-specific \ + configuration from config/robots/.yaml. Defaults to default.yaml.') # Create the launch description and populate ld = LaunchDescription() @@ -117,8 +154,8 @@ def generate_launch_description(): ld.add_action(kd_w_val_declare) ld.add_action(enable_pd_regulator_declare) ld.add_action(use_sim_time_declare) + ld.add_action(robot_id_declare) - ld.add_action(robot_state_pub_node) - ld.add_action(hunter_base_node) + ld.add_action(OpaqueFunction(function=launch_setup)) return ld \ No newline at end of file diff --git a/hunter_pltf_description/CMakeLists.txt b/hunter_pltf_description/CMakeLists.txt index 8d72bcd..5950d58 100755 --- a/hunter_pltf_description/CMakeLists.txt +++ b/hunter_pltf_description/CMakeLists.txt @@ -24,7 +24,7 @@ if(BUILD_TESTING) endif() install( - DIRECTORY launch meshes description + DIRECTORY launch meshes description config DESTINATION share/${PROJECT_NAME} ) diff --git a/hunter_pltf_description/config/robots/EXAMPLE_USAGE.md b/hunter_pltf_description/config/robots/EXAMPLE_USAGE.md new file mode 100644 index 0000000..76dd83b --- /dev/null +++ b/hunter_pltf_description/config/robots/EXAMPLE_USAGE.md @@ -0,0 +1,272 @@ +# Example Usage: Robot-Specific Configuration + +This document provides practical examples of using the robot configuration system. + +## Quick Start + +### 1. Launch with Default Configuration + +```bash +# Launch robot state publisher with default sensor poses +ros2 launch hunter_pltf_description pltf_rsp.launch.py + +# Launch in simulation with default configuration +ros2 launch hunter_pltf_gazebo launch_sim.launch.py + +# Bringup real robot with default configuration +ros2 launch hunter_pltf_bringup hunter_pltf_bringup.launch.py +``` + +### 2. Launch with Hunter-01 Configuration + +```bash +# Launch robot state publisher with hunter_01 calibration +ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_01 + +# Launch in simulation with hunter_01 configuration +ros2 launch hunter_pltf_gazebo launch_sim.launch.py robot_id:=hunter_01 + +# Bringup real robot hunter_01 +ros2 launch hunter_pltf_bringup hunter_pltf_bringup.launch.py robot_id:=hunter_01 +``` + +## Configuration File Examples + +### Default Configuration (Baseline) + +```yaml +# config/robots/default.yaml +robot_id: "default" + +sensors: + imu: + x: -0.25 + y: 0.0 + z: 0.47 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + topic: "/gps_base/yaw" + + front_camera: + x: 0.55 + y: 0.0 + z: 0.72 + # ... more sensors +``` + +### Robot-Specific Configuration (Hunter-01) + +The hunter_01 configuration only specifies differences from default: + +```yaml +# config/robots/hunter_01.yaml +robot_id: "hunter_01" + +sensors: + imu: + x: -0.251 # 1mm offset + y: 0.001 # 1mm offset + z: 0.471 # 1mm offset + # roll, pitch, yaw, topic inherited from default + + front_camera: + x: 0.552 # 2mm offset + y: 0.001 # 1mm offset + z: 0.721 # 1mm offset + # roll, pitch, yaw inherited from default +``` + +## Testing Your Configuration + +### 1. Validate Configuration File + +```bash +cd /path/to/hunter_pltf_description/config/robots +python3 config_loader.py hunter_01 true +``` + +Expected output: +``` +Loading configuration for: hunter_01 +Simulation mode: True +------------------------------------------------------------ +Robot ID: hunter_01 + +Sensors configured: ['imu', 'imu1', 'front_camera', 'back_camera', 'gps_base', 'front_lidar_link', 'back_lidar_link'] + +Xacro arguments: + imu_x: -0.251 + imu_y: 0.001 + imu_z: 0.471 + ... +``` + +### 2. Visualize in RViz + +```bash +# Launch with RViz to visualize sensor frames +ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_01 gui:=true +``` + +This will: +- Load the hunter_01 configuration +- Start robot_state_publisher with calibrated poses +- Open RViz to visualize the TF tree +- Show joint_state_publisher_gui + +### 3. Check TF Frames + +```bash +# List all frames +ros2 run tf2_ros tf2_echo base_link front_camera_link + +# View entire TF tree +ros2 run tf2_tools view_frames +``` + +## Creating a New Robot Configuration + +### Step 1: Create Configuration File + +```bash +cd /path/to/hunter_pltf_description/config/robots +cp default.yaml hunter_02.yaml +``` + +### Step 2: Edit Configuration + +```yaml +# hunter_02.yaml +robot_id: "hunter_02" + +# Calibration date: 2024-11-15 +# Notes: Front camera mounted 3mm higher than baseline + +sensors: + front_camera: + z: 0.723 # Only specify what changed +``` + +### Step 3: Test Configuration + +```bash +# Test configuration loading +python3 config_loader.py hunter_02 true + +# Visualize in RViz +ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_02 gui:=true +``` + +### Step 4: Deploy + +```bash +# Use in simulation +ros2 launch hunter_pltf_gazebo launch_sim.launch.py robot_id:=hunter_02 + +# Use on real robot +ros2 launch hunter_pltf_bringup hunter_pltf_bringup.launch.py robot_id:=hunter_02 +``` + +## Environment Variable Alternative + +You can also use environment variables: + +```bash +# Set robot ID via environment +export HUNTER_ROBOT_ID=hunter_01 + +# Launch without robot_id argument (will use environment variable) +ros2 launch hunter_pltf_description pltf_rsp.launch.py + +# Or override with argument +ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_02 +``` + +## Comparison: Default vs Hunter-01 + +| Sensor | Parameter | Default | Hunter-01 | Difference | +|--------|-----------|---------|-----------|------------| +| IMU | x | -0.25 | -0.251 | +1mm | +| IMU | y | 0.0 | 0.001 | +1mm | +| IMU | z | 0.47 | 0.471 | +1mm | +| IMU1 | x | 0.25 | 0.249 | -1mm | +| IMU1 | y | 0.0 | -0.002 | -2mm | +| IMU1 | z | 0.47 | 0.469 | -1mm | +| Front Camera | x | 0.55 | 0.552 | +2mm | +| Front Camera | y | 0.0 | 0.001 | +1mm | +| Front Camera | z | 0.72 | 0.721 | +1mm | +| Back Camera | x | -0.55 | -0.548 | +2mm | +| Back Camera | z | 0.72 | 0.719 | -1mm | +| Front LiDAR | x | 0.56 | 0.561 | +1mm | +| Front LiDAR | y | 0.235 | 0.236 | +1mm | +| Front LiDAR | z | 0.46 | 0.461 | +1mm | +| Front LiDAR | roll (real) | -0.0174533 | -0.0175 | -0.0467° | +| Front LiDAR | pitch (real) | 0.0 | 0.001 | +0.0573° | + +These small variations (±1-3mm, <1°) are typical of manufacturing tolerances and calibration adjustments. + +## Troubleshooting + +### Configuration Not Loading + +**Symptom:** Robot uses default configuration despite specifying `robot_id` + +**Solutions:** +1. Check file exists: `ls config/robots/hunter_01.yaml` +2. Validate YAML: `python3 -c "import yaml; yaml.safe_load(open('config/robots/hunter_01.yaml'))"` +3. Check console output for error messages + +### Incorrect Sensor Positions + +**Symptom:** Sensors appear in wrong locations in RViz + +**Solutions:** +1. Verify coordinate frame (base_link reference) +2. Check sign of values (positive X = forward) +3. Confirm units are meters (not millimeters) +4. Use `ros2 run tf2_tools view_frames` to inspect TF tree + +### Launch File Issues + +**Symptom:** Launch fails with Python error + +**Solutions:** +1. Check Python syntax: `python3 -m py_compile config_loader.py` +2. Verify config directory is installed: `ros2 pkg prefix hunter_pltf_description` +3. Check import errors in launch file output + +## Advanced: Programmatic Access + +You can also use the config loader in your own Python code: + +```python +from config_loader import load_robot_config, get_xacro_args + +# Load configuration +config = load_robot_config('hunter_01') + +# Get xacro arguments for simulation +xacro_args = get_xacro_args(config, is_sim=True) + +# Access specific sensor parameters +imu_x = config['sensors']['imu']['x'] +print(f"IMU X position: {imu_x}m") +``` + +## Best Practices + +1. **Always start from default.yaml** when creating new robot configs +2. **Only specify differences** to minimize duplication +3. **Document calibration** in YAML comments (date, notes, measurements) +4. **Test in simulation first** before deploying to real robot +5. **Version control** robot configs in the repository +6. **Use descriptive robot IDs** (e.g., hunter_01, hunter_lab_01) +7. **Validate before committing** using `config_loader.py` test script + +## Additional Resources + +- Full documentation: `README.md` +- Configuration format: `default.yaml` +- Example calibration: `hunter_01.yaml` +- Python API: `config_loader.py` diff --git a/hunter_pltf_description/config/robots/GENERIC_USAGE.md b/hunter_pltf_description/config/robots/GENERIC_USAGE.md new file mode 100644 index 0000000..b01f22d --- /dev/null +++ b/hunter_pltf_description/config/robots/GENERIC_USAGE.md @@ -0,0 +1,443 @@ +# Generic Configuration System - Usage for Any Robot + +This document demonstrates how the configuration system can be used with **any robot**, not just Hunter platform. + +## Overview + +The configuration system is **completely generic** and reusable. It automatically maps nested YAML structures to flat xacro arguments, making it suitable for any ROS robot. + +## Example 1: Simple Mobile Robot + +### YAML Configuration + +```yaml +# my_mobile_robot.yaml +robot_id: "mobile_bot_01" + +sensors: + laser: + x: 0.15 + y: 0.0 + z: 0.25 + range: 30.0 + angle: 3.14159 + + camera: + x: 0.20 + y: 0.0 + z: 0.30 + fov: 1.57 + resolution: "640x480" + +wheels: + left: + radius: 0.1 + width: 0.05 + right: + radius: 0.1 + width: 0.05 +``` + +### Generated Xacro Arguments + +The system automatically generates: + +```bash +laser_x:=0.15 +laser_y:=0.0 +laser_z:=0.25 +laser_range:=30.0 +laser_angle:=3.14159 +camera_x:=0.20 +camera_y:=0.0 +camera_z:=0.30 +camera_fov:=1.57 +camera_resolution:=640x480 +left_radius:=0.1 +left_width:=0.05 +right_radius:=0.1 +right_width:=0.05 +``` + +### URDF Xacro File + +Your xacro file would define these arguments: + +```xml + + + + + + + + + + + + + + + + +``` + +## Example 2: Robotic Manipulator + +### YAML Configuration + +```yaml +# manipulator.yaml +robot_id: "arm_01" + +joints: + shoulder_pan: + position: 0.0 + velocity_limit: 2.0 + effort_limit: 100.0 + shoulder_lift: + position: -1.57 + velocity_limit: 2.0 + effort_limit: 100.0 + elbow: + position: 1.57 + velocity_limit: 2.0 + effort_limit: 50.0 + wrist_1: + position: 0.0 + velocity_limit: 3.0 + effort_limit: 25.0 + +gripper: + max_opening: 0.08 + force_limit: 20.0 +``` + +### Generated Xacro Arguments + +```bash +shoulder_pan_position:=0.0 +shoulder_pan_velocity_limit:=2.0 +shoulder_pan_effort_limit:=100.0 +shoulder_lift_position:=-1.57 +shoulder_lift_velocity_limit:=2.0 +shoulder_lift_effort_limit:=100.0 +elbow_position:=1.57 +elbow_velocity_limit:=2.0 +elbow_effort_limit:=50.0 +wrist_1_position:=0.0 +wrist_1_velocity_limit:=3.0 +wrist_1_effort_limit:=25.0 +max_opening:=0.08 +force_limit:=20.0 +``` + +## Example 3: Drone with Multiple Cameras + +### YAML Configuration + +```yaml +# drone.yaml +robot_id: "drone_alpha" + +cameras: + front: + x: 0.15 + y: 0.0 + z: 0.02 + pitch: 0.0 + fov: 1.91 + resolution: "1920x1080" + + bottom: + x: 0.0 + y: 0.0 + z: -0.05 + pitch: -1.57 + fov: 1.57 + resolution: "640x480" + + gimbal: + x: 0.0 + y: 0.0 + z: -0.10 + pitch_range: 1.57 + yaw_range: 3.14 + stabilized: true + +propellers: + diameter: 0.25 + thrust_coefficient: 8.5e-6 + count: 4 +``` + +### Generated Xacro Arguments + +```bash +front_x:=0.15 +front_y:=0.0 +front_z:=0.02 +front_pitch:=0.0 +front_fov:=1.91 +front_resolution:=1920x1080 +bottom_x:=0.0 +bottom_y:=0.0 +bottom_z:=-0.05 +bottom_pitch:=-1.57 +bottom_fov:=1.57 +bottom_resolution:=640x480 +gimbal_x:=0.0 +gimbal_y:=0.0 +gimbal_z:=-0.10 +gimbal_pitch_range:=1.57 +gimbal_yaw_range:=3.14 +gimbal_stabilized:=True +diameter:=0.25 +thrust_coefficient:=8.5e-06 +count:=4 +``` + +## Loading Configurations from Different Sources + +### Local File (Simple Robot ID) + +```python +from config_loader import load_robot_config, get_xacro_args_from_config + +# Looks in config/robots/mobile_bot_01.yaml +config = load_robot_config('mobile_bot_01') +args = get_xacro_args_from_config(config, prefix='sensors') +``` + +### Absolute Path + +```python +# Load from anywhere on filesystem +config = load_robot_config('/opt/robot_configs/production/arm_01.yaml') +args = get_xacro_args_from_config(config, prefix='joints') +``` + +### Remote URL (HTTPS) + +```python +# Load from central configuration server +config = load_robot_config('https://robots.company.com/configs/drone_alpha.yaml') +args = get_xacro_args_from_config(config, prefix='cameras') +``` + +## Customizing the Mapping + +### Using Different Prefixes + +```python +# Extract from 'joints' section instead of 'sensors' +config = load_robot_config('manipulator') +args = get_xacro_args_from_config(config, prefix='joints') +``` + +### No Prefix (Top-Level Mapping) + +```python +# Map entire YAML structure +config = load_robot_config('simple_robot') +args = get_xacro_args_from_config(config, prefix=None) +``` + +### Excluding Keys + +```python +# Exclude metadata from xacro arguments +config = load_robot_config('myrobot') +args = get_xacro_args_from_config( + config, + prefix='sensors', + exclude_keys=['robot_id', 'calibration_date', 'notes'] +) +``` + +## Simulation vs Real Hardware + +The system automatically handles `_sim` and `_real` suffixes: + +### YAML Configuration + +```yaml +sensors: + lidar: + x: 0.3 + y: 0.0 + z: 0.5 + # Different values for sim vs real + roll_sim: 0.0 + roll_real: -0.0175 + noise_sim: 0.01 + noise_real: 0.05 +``` + +### Generated Arguments (is_sim=True) + +```bash +lidar_x:=0.3 +lidar_y:=0.0 +lidar_z:=0.5 +lidar_roll_sim:=0.0 +lidar_noise_sim:=0.01 +``` + +### Generated Arguments (is_sim=False) + +```bash +lidar_x:=0.3 +lidar_y:=0.0 +lidar_z:=0.5 +lidar_roll_real:=-0.0175 +lidar_noise_real:=0.05 +``` + +## Integration with Launch Files + +### Python Launch File Example + +```python +from launch import LaunchDescription +from launch.actions import OpaqueFunction, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from pathlib import Path +import sys + +def generate_robot_description(context, *args, **kwargs): + robot_config_uri = LaunchConfiguration('robot_config').perform(context) + is_sim = LaunchConfiguration('is_sim').perform(context).lower() == 'true' + + # Import config loader + config_dir = Path(__file__).parent / 'config' / 'robots' + sys.path.insert(0, str(config_dir)) + from config_loader import load_robot_config, get_xacro_args_from_config, format_xacro_args + + # Load configuration (works with any URI) + config = load_robot_config(robot_config_uri) + + # Generate xacro arguments (customize prefix for your robot) + xacro_args = get_xacro_args_from_config(config, is_sim=is_sim, prefix='sensors') + + # Use in xacro command + robot_description_content = Command([ + 'xacro', + ' ', + 'path/to/your/robot.urdf.xacro', + ' ', + format_xacro_args(xacro_args) + ]) + + return robot_description_content + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'robot_config', + default_value='default', + description='Robot configuration (ID, path, or URL)' + ), + DeclareLaunchArgument( + 'is_sim', + default_value='true', + description='Simulation mode' + ), + OpaqueFunction(function=generate_robot_description) + ]) +``` + +### Usage + +```bash +# Simple robot ID +ros2 launch my_robot robot.launch.py robot_config:=mobile_bot_01 + +# Absolute path +ros2 launch my_robot robot.launch.py robot_config:=/path/to/config.yaml + +# Remote URL +ros2 launch my_robot robot.launch.py robot_config:=https://server.com/config.yaml +``` + +## Best Practices + +### 1. Structure Your YAML to Match Xacro + +Organize your YAML to mirror your xacro argument naming: + +```yaml +# If xacro has: camera_front_x, camera_front_y +sensors: + camera_front: + x: 0.5 + y: 0.0 +``` + +### 2. Use Descriptive Keys + +Make keys self-documenting: + +```yaml +sensors: + laser_scanner: # Not just "laser" + max_range: 30.0 + min_range: 0.1 +``` + +### 3. Group Related Parameters + +Use nested structures for organization: + +```yaml +gripper: + fingers: + left: + length: 0.1 + width: 0.02 + right: + length: 0.1 + width: 0.02 +``` + +### 4. Document Units + +Include units in comments: + +```yaml +sensors: + camera: + x: 0.15 # meters + fov: 1.57 # radians + framerate: 30 # Hz +``` + +## Validation + +Test your configuration before deployment: + +```bash +# Test configuration loading +python3 config_loader.py /path/to/your/robot.yaml true + +# Verify xacro argument generation +python3 config_loader.py https://server.com/robot.yaml false +``` + +## Summary + +The configuration system is **completely generic** and works with: +- ✅ Any robot type (mobile, manipulator, drone, etc.) +- ✅ Any YAML structure (sensors, joints, links, etc.) +- ✅ Any source location (local, remote, URLs) +- ✅ Any URDF/xacro structure +- ✅ Simulation and real hardware modes + +It automatically handles: +- ✅ Flattening nested structures +- ✅ Type conversion to strings +- ✅ Simulation vs real hardware variants +- ✅ Inheritance and overrides +- ✅ URI resolution (paths, URLs) + +No modifications needed for different robots - just structure your YAML appropriately! diff --git a/hunter_pltf_description/config/robots/README.md b/hunter_pltf_description/config/robots/README.md new file mode 100644 index 0000000..41a71ba --- /dev/null +++ b/hunter_pltf_description/config/robots/README.md @@ -0,0 +1,331 @@ +# Generic Robot Configuration System + +This directory contains a **generic, reusable** robot configuration system that automatically maps YAML configurations to URDF xacro arguments. The system is not specific to Hunter platforms and can be used with any ROS robot. + +## Key Features + +- **Load from anywhere**: Local files, absolute paths, file:// URLs, or https:// URLs +- **Generic mapping**: Automatically maps YAML structure to xacro arguments (no hardcoded sensor names) +- **Inheritance-based**: Robot-specific configs only override what differs from defaults +- **Reusable**: Works with any URDF/xacro structure, not just Hunter platform + +## Overview + +The configuration system allows you to: +- Define baseline sensor poses in `default.yaml` or any custom file +- Create robot-specific overrides in separate YAML files +- Load configurations from local files, remote URLs, or any accessible location +- Automatically map nested YAML structures to flat xacro arguments +- Maintain only the differences from the baseline, minimizing duplication +- Version control calibration data for each robot instance + +## File Structure + +``` +config/robots/ +├── README.md # This file +├── default.yaml # Baseline configuration for all robots +├── hunter_01.yaml # Configuration for Hunter-01 robot +└── hunter_XX.yaml # Additional robot-specific configurations +``` + +## Configuration File Format + +Each configuration file contains: + +```yaml +robot_id: "hunter_01" # Unique identifier for the robot + +sensors: + sensor_name: + x: 0.0 # Position in meters (X-axis, forward) + y: 0.0 # Position in meters (Y-axis, left) + z: 0.0 # Position in meters (Z-axis, up) + roll: 0.0 # Orientation in radians (rotation around X-axis) + pitch: 0.0 # Orientation in radians (rotation around Y-axis) + yaw: 0.0 # Orientation in radians (rotation around Z-axis) +``` + +### Sensor Names (Hunter Platform Example) + +For the Hunter platform, the following sensors are configurable: + +- **`imu`**: Rear IMU sensor (at GPS base location) +- **`imu1`**: Front IMU sensor +- **`front_camera`**: Front depth camera +- **`back_camera`**: Rear depth camera +- **`gps_base`**: GPS base antenna +- **`front_lidar`**: Front Mid-360 LiDAR +- **`back_lidar`**: Rear Mid-360 LiDAR + +**Note**: The sensor names in your YAML should match the xacro argument prefixes in your URDF. The system automatically flattens nested structures (e.g., `sensors.imu.x` → `imu_x`). + +### LiDAR Configuration + +LiDAR sensors have separate orientation values for simulation and real hardware: + +```yaml +front_lidar_link: + x: 0.56 + y: 0.235 + z: 0.46 + # Simulation orientations + roll_sim: 0.0 + pitch_sim: 0.0 + yaw_sim: 0.0 + # Real hardware orientations (calibrated) + roll_real: -0.0174533 + pitch_real: 0.0 + yaw_real: 0.00872665 + topic: "front_lidar/points" +``` + +## Creating a New Robot Configuration + +1. **Copy the default configuration:** + ```bash + cp default.yaml hunter_XX.yaml + ``` + +2. **Update the robot_id:** + ```yaml + robot_id: "hunter_XX" + ``` + +3. **Remove entries that match the default** - only keep values that differ + +4. **Calibrate sensor positions:** + - Physically measure sensor positions relative to `base_link` + - Update the YAML file with measured values + - Test in simulation first, then on real hardware + +5. **Document your calibration:** + - Add calibration date and notes in comments + - Include any special considerations or known issues + +## Using Robot Configurations + +### In Launch Files + +The system supports multiple ways to specify robot configurations: + +#### 1. Simple Robot ID (Local File) +```bash +# Using default configuration from config/robots/default.yaml +ros2 launch hunter_pltf_description pltf_rsp.launch.py + +# Using hunter_01 from config/robots/hunter_01.yaml +ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_01 + +# Using hunter_01 for bringup +ros2 launch hunter_pltf_bringup hunter_pltf_bringup.launch.py robot_id:=hunter_01 + +# Using hunter_01 for simulation +ros2 launch hunter_pltf_gazebo launch_sim.launch.py robot_id:=hunter_01 +``` + +#### 2. Absolute File Path +```bash +# Load from absolute path +ros2 launch hunter_pltf_description pltf_rsp.launch.py \ + robot_id:=/path/to/my/robot_config.yaml +``` + +#### 3. File URL +```bash +# Load from file:// URL +ros2 launch hunter_pltf_description pltf_rsp.launch.py \ + robot_id:=file:///path/to/my/robot_config.yaml +``` + +#### 4. Remote HTTPS URL +```bash +# Load from remote server (e.g., central configuration repository) +ros2 launch hunter_pltf_description pltf_rsp.launch.py \ + robot_id:=https://config.example.com/robots/hunter_01.yaml +``` + +### Environment Variable (Alternative) + +You can also set the robot ID via environment variable: + +```bash +export HUNTER_ROBOT_ID=hunter_01 +ros2 launch hunter_pltf_description pltf_rsp.launch.py + +# Or with full path/URL +export HUNTER_ROBOT_ID=https://config.example.com/robots/hunter_01.yaml +ros2 launch hunter_pltf_description pltf_rsp.launch.py +``` + +## Inheritance and Overrides + +Robot-specific configuration files **inherit** from `default.yaml`. Only specify values that differ: + +**Example:** If Hunter-02 only has a different front camera position: + +```yaml +# hunter_02.yaml +robot_id: "hunter_02" + +sensors: + front_camera: + x: 0.553 # Only this value differs from default + # All other sensors use default values +``` + +## Validation and Testing + +After creating or modifying a configuration: + +1. **Validate YAML syntax:** + ```bash + python3 -c "import yaml; yaml.safe_load(open('hunter_XX.yaml'))" + ``` + +2. **Test in simulation:** + ```bash + ros2 launch hunter_pltf_gazebo launch_sim.launch.py robot_id:=hunter_XX + ``` + +3. **Visualize in RViz:** + ```bash + ros2 launch hunter_pltf_description pltf_rsp.launch.py robot_id:=hunter_XX gui:=true + ``` + +4. **Verify sensor frames:** + ```bash + ros2 run tf2_tools view_frames + ``` + +## Generic YAML-to-Xacro Mapping + +The configuration system uses **automatic, generic mapping** from nested YAML structures to flat xacro arguments. + +### How It Works + +The system: +1. Extracts the `sensors` section from your YAML (configurable) +2. Flattens the nested structure using underscore separators +3. Filters based on simulation mode (`_sim` vs `_real` suffixes) +4. Converts all values to strings for xacro + +### Example Mapping + +**YAML Input:** +```yaml +sensors: + imu: + x: -0.25 + y: 0.0 + topic: "/imu/data" + front_lidar: + x: 0.56 + roll_sim: 0.0 + roll_real: -0.0174533 +``` + +**Xacro Arguments (is_sim=True):** +``` +imu_x:=-0.25 +imu_y:=0.0 +imu_topic:=/imu/data +front_lidar_x:=0.56 +front_lidar_roll_sim:=0.0 +``` + +**Xacro Arguments (is_sim=False):** +``` +imu_x:=-0.25 +imu_y:=0.0 +imu_topic:=/imu/data +front_lidar_x:=0.56 +front_lidar_roll_real:=-0.0174533 +``` + +### Reusability for Other URDFs + +This system works with **any URDF**, not just Hunter platform: + +1. **Define your YAML structure** to match your xacro argument names +2. **Use nested dictionaries** for organization (e.g., `sensors`, `joints`, `links`) +3. **Flattening is automatic** - `sensors.camera.x` becomes `camera_x` +4. **Use _sim/_real suffixes** for mode-specific values + +**Example for a different robot:** +```yaml +# my_robot_config.yaml +robot_id: "myrobot_01" + +# Your own structure - not limited to "sensors" +manipulator: + joint1: + position: 0.0 + velocity_limit: 2.0 + joint2: + position: 1.57 + velocity_limit: 1.5 + +sensors: + laser: + x: 0.3 + y: 0.0 + range: 30.0 +``` + +This maps to: `joint1_position`, `joint1_velocity_limit`, `joint2_position`, etc. + +## Calibration Guidelines + +### Tools Needed +- Tape measure or laser distance meter +- Level +- Reference markers on robot chassis + +### Coordinate Frame +All positions are relative to `base_link`: +- **X-axis**: Forward (positive = front of robot) +- **Y-axis**: Left (positive = left side of robot) +- **Z-axis**: Up (positive = above robot) + +### Measurement Process +1. Mark the `base_link` origin on the robot +2. Measure X, Y, Z distances from origin to sensor center +3. Measure orientation angles if sensor is tilted +4. Record values in robot-specific YAML file +5. Test and verify in RViz and during operation + +### Typical Tolerances +- Position: ±2mm typical manufacturing tolerance +- Orientation: ±1 degree typical mounting tolerance +- Critical sensors (LiDAR, cameras): ±1mm, ±0.5 degrees + +## Troubleshooting + +### Configuration Not Loading +- Check YAML syntax (indentation, colons, quotes) +- Verify file is in `config/robots/` directory +- Check console output for error messages + +### Sensor Position Incorrect +- Verify coordinate frame (base_link reference) +- Check sign of values (positive X = forward) +- Confirm units (meters, not millimeters) + +### Launch File Issues +- Ensure `robot_id` argument is passed correctly +- Check that configuration file exists +- Verify launch file has been updated to support `robot_id` + +## Version Control Best Practices + +- **Commit calibration files** to repository +- **Document changes** in commit messages +- **Tag releases** when calibration is stable +- **Track calibration history** for maintenance + +## Additional Resources + +- [ROS 2 URDF Tutorial](https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html) +- [xacro Documentation](http://wiki.ros.org/xacro) +- [TF2 Debugging](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Debugging-Tf2-Problems.html) diff --git a/hunter_pltf_description/config/robots/config_loader.py b/hunter_pltf_description/config/robots/config_loader.py new file mode 100644 index 0000000..79e23e7 --- /dev/null +++ b/hunter_pltf_description/config/robots/config_loader.py @@ -0,0 +1,346 @@ +#!/usr/bin/env python3 +""" +Generic Robot Configuration Loader + +This module provides utilities to load robot-specific YAML configurations +from various sources (local files, remote URLs) and automatically map them +to xacro arguments. It supports inheritance where robot-specific +configurations only need to specify values that differ from the baseline. + +Usage: + from config_loader import load_robot_config, get_xacro_args_from_config + + # Load from local file, URI, or URL + config = load_robot_config('hunter_01') # Local in config/robots/ + config = load_robot_config('/path/to/config.yaml') # Absolute path + config = load_robot_config('https://example.com/robot.yaml') # Remote URL + + # Automatically map to xacro arguments + xacro_args = get_xacro_args_from_config(config, is_sim=True) +""" + +import os +import yaml +import urllib.request +import urllib.parse +from typing import Dict, Any, Optional, Union +from pathlib import Path + + +def get_config_dir() -> Path: + """Get the path to the robot configuration directory.""" + return Path(__file__).parent + + +def load_yaml_from_uri(uri: str) -> Dict[str, Any]: + """ + Load a YAML file from a URI (local file path, file:// URL, or https:// URL). + + Args: + uri: URI to the YAML file. Can be: + - Absolute file path: /path/to/file.yaml + - Relative file path: relative/path.yaml + - File URL: file:///path/to/file.yaml + - HTTPS URL: https://example.com/config.yaml + + Returns: + Dictionary containing the YAML contents + + Raises: + FileNotFoundError: If the file doesn't exist (local files) + urllib.error.URLError: If URL cannot be accessed + yaml.YAMLError: If the content is not valid YAML + """ + parsed = urllib.parse.urlparse(uri) + + # Handle HTTPS URLs + if parsed.scheme in ('http', 'https'): + try: + with urllib.request.urlopen(uri, timeout=10) as response: + content = response.read().decode('utf-8') + return yaml.safe_load(content) or {} + except urllib.error.URLError as e: + raise urllib.error.URLError(f"Failed to fetch configuration from {uri}: {e}") + except yaml.YAMLError as e: + raise yaml.YAMLError(f"Error parsing YAML from {uri}: {e}") + + # Handle file:// URLs and local paths + if parsed.scheme == 'file': + filepath = Path(parsed.path) + else: + filepath = Path(uri) + + if not filepath.exists(): + raise FileNotFoundError(f"Configuration file not found: {filepath}") + + with open(filepath, 'r') as f: + try: + return yaml.safe_load(f) or {} + except yaml.YAMLError as e: + raise yaml.YAMLError(f"Error parsing YAML file {filepath}: {e}") + + +def deep_merge(base: Dict[str, Any], override: Dict[str, Any]) -> Dict[str, Any]: + """ + Deep merge two dictionaries, with override values taking precedence. + + Args: + base: Base dictionary + override: Override dictionary + + Returns: + Merged dictionary + """ + result = base.copy() + + for key, value in override.items(): + if key in result and isinstance(result[key], dict) and isinstance(value, dict): + result[key] = deep_merge(result[key], value) + else: + result[key] = value + + return result + + +def load_robot_config(robot_id_or_uri: str = 'default', + default_config_uri: Optional[str] = None) -> Dict[str, Any]: + """ + Load robot-specific configuration, merging with default values. + + This function supports loading configurations from: + - Simple robot ID (looks in config/robots/ directory) + - Absolute or relative file paths + - file:// URLs + - https:// URLs + + Args: + robot_id_or_uri: Robot identifier, file path, or URL. Examples: + - 'hunter_01' -> loads config/robots/hunter_01.yaml + - '/path/to/robot.yaml' -> loads from absolute path + - 'file:///path/to/robot.yaml' -> loads from file URL + - 'https://example.com/robot.yaml' -> loads from HTTPS + default_config_uri: Optional URI to default configuration. If not provided, + uses 'default.yaml' from config/robots/ directory. + + Returns: + Complete configuration dictionary with all sensor parameters + + Raises: + FileNotFoundError: If configuration files don't exist + urllib.error.URLError: If URL cannot be accessed + yaml.YAMLError: If YAML files are invalid + """ + # Determine if input is a URI (path or URL) or just a robot ID + parsed = urllib.parse.urlparse(robot_id_or_uri) + is_uri = (parsed.scheme in ('http', 'https', 'file') or + '/' in robot_id_or_uri or + '\\' in robot_id_or_uri or + Path(robot_id_or_uri).exists()) + + # Load default configuration + if default_config_uri: + default_config = load_yaml_from_uri(default_config_uri) + else: + config_dir = get_config_dir() + default_file = config_dir / 'default.yaml' + default_config = load_yaml_from_uri(str(default_file)) + + # If requesting default by ID, return it directly + if robot_id_or_uri == 'default' and not is_uri: + return default_config + + # Load robot-specific configuration + if is_uri: + # Direct URI provided + robot_config = load_yaml_from_uri(robot_id_or_uri) + else: + # Simple robot ID - look in config/robots/ directory + config_dir = get_config_dir() + robot_file = config_dir / f'{robot_id_or_uri}.yaml' + if not robot_file.exists(): + print(f"Warning: Configuration for '{robot_id_or_uri}' not found, using default") + return default_config + robot_config = load_yaml_from_uri(str(robot_file)) + + # Merge configurations (robot-specific overrides default) + merged_config = deep_merge(default_config, robot_config) + + return merged_config + + +def get_sensor_param(config: Dict[str, Any], sensor_name: str, param_name: str, + default: Any = None) -> Any: + """ + Get a sensor parameter from the configuration. + + Args: + config: Configuration dictionary + sensor_name: Name of the sensor (e.g., 'imu', 'front_camera') + param_name: Name of the parameter (e.g., 'x', 'y', 'z') + default: Default value if parameter not found + + Returns: + Parameter value or default + """ + try: + return config['sensors'][sensor_name][param_name] + except (KeyError, TypeError): + return default + + +def flatten_dict(d: Dict[str, Any], parent_key: str = '', sep: str = '_') -> Dict[str, Any]: + """ + Flatten a nested dictionary into a single-level dictionary with concatenated keys. + + Args: + d: Dictionary to flatten + parent_key: Parent key for recursion + sep: Separator between keys + + Returns: + Flattened dictionary + + Example: + {'sensors': {'imu': {'x': 1.0, 'y': 2.0}}} + -> {'sensors_imu_x': 1.0, 'sensors_imu_y': 2.0} + """ + items = [] + for k, v in d.items(): + new_key = f"{parent_key}{sep}{k}" if parent_key else k + if isinstance(v, dict): + items.extend(flatten_dict(v, new_key, sep=sep).items()) + else: + items.append((new_key, v)) + return dict(items) + + +def get_xacro_args_from_config(config: Dict[str, Any], + is_sim: bool = True, + prefix: str = 'sensors', + exclude_keys: Optional[list] = None) -> Dict[str, str]: + """ + Generic converter: automatically map nested YAML configuration to xacro arguments. + + This function flattens the configuration dictionary and converts it to xacro + argument format. It's designed to work with any URDF structure, not just + specific hardcoded sensor names. + + Args: + config: Configuration dictionary from load_robot_config() + is_sim: Whether running in simulation mode (affects _sim/_real suffix handling) + prefix: Top-level key to extract and flatten (default: 'sensors') + exclude_keys: List of keys to exclude from the output (e.g., ['robot_id']) + + Returns: + Dictionary of xacro argument names and values (all as strings) + + Example: + Input config: + { + 'sensors': { + 'imu': {'x': -0.25, 'y': 0.0, 'topic': '/imu'}, + 'camera': {'x': 0.5, 'roll_sim': 0.0, 'roll_real': 0.1} + } + } + + Output (is_sim=True): + { + 'imu_x': '-0.25', + 'imu_y': '0.0', + 'imu_topic': '/imu', + 'camera_x': '0.5', + 'camera_roll_sim': '0.0' + } + """ + if exclude_keys is None: + exclude_keys = ['robot_id'] + + # Extract the section to process (e.g., 'sensors') + data_to_process = config.get(prefix, {}) if prefix else config + + # Flatten the nested dictionary + flattened = flatten_dict(data_to_process) + + # Convert to xacro arguments + args = {} + for key, value in flattened.items(): + # Skip excluded keys + if any(excluded in key for excluded in exclude_keys): + continue + + # Handle sim/real variants - only include the relevant one + if '_sim' in key and not is_sim: + continue # Skip _sim keys when not in simulation + if '_real' in key and is_sim: + continue # Skip _real keys when in simulation + + # Convert value to string + args[key] = str(value) + + return args + + +# Backward compatibility: keep old function name but redirect to new one +def get_xacro_args(config: Dict[str, Any], is_sim: bool = True) -> Dict[str, str]: + """ + Convert configuration dictionary to xacro arguments (backward compatibility). + + This function is maintained for backward compatibility. New code should use + get_xacro_args_from_config() which is more flexible and generic. + + Args: + config: Configuration dictionary from load_robot_config() + is_sim: Whether running in simulation mode + + Returns: + Dictionary of xacro argument names and values (all as strings) + """ + return get_xacro_args_from_config(config, is_sim=is_sim, prefix='sensors') + + +def format_xacro_args(args: Dict[str, str]) -> str: + """ + Format xacro arguments as a command-line string. + + Args: + args: Dictionary of argument names and values + + Returns: + Formatted string for use in xacro command + """ + return ' '.join([f'{key}:={value}' for key, value in args.items()]) + + +if __name__ == '__main__': + # Test the configuration loader + import sys + + robot_id_or_uri = sys.argv[1] if len(sys.argv) > 1 else 'default' + is_sim = sys.argv[2].lower() == 'true' if len(sys.argv) > 2 else True + + print(f"Loading configuration for: {robot_id_or_uri}") + print(f"Simulation mode: {is_sim}") + print("-" * 60) + + try: + config = load_robot_config(robot_id_or_uri) + print(f"Robot ID: {config.get('robot_id', 'unknown')}") + print(f"\nSensors configured: {list(config.get('sensors', {}).keys())}") + + print(f"\nXacro arguments (generic mapping):") + xacro_args = get_xacro_args_from_config(config, is_sim) + for key, value in sorted(xacro_args.items()): + print(f" {key}: {value}") + + print(f"\nCommand line format:") + print(f" {format_xacro_args(xacro_args)}") + + # Show that we can load from URIs + if robot_id_or_uri not in ['default', 'hunter_01']: + print(f"\nNote: Loaded from URI: {robot_id_or_uri}") + + except Exception as e: + import traceback + print(f"Error: {e}", file=sys.stderr) + traceback.print_exc() + sys.exit(1) diff --git a/hunter_pltf_description/config/robots/default.yaml b/hunter_pltf_description/config/robots/default.yaml new file mode 100644 index 0000000..8724f58 --- /dev/null +++ b/hunter_pltf_description/config/robots/default.yaml @@ -0,0 +1,158 @@ +# Default robot configuration for Hunter Platform +# This file contains the baseline sensor poses for all Hunter robots +# Robot-specific configurations should inherit from this file and only override necessary values + +robot_id: "default" + +# Sensor pose configurations +# All poses are specified relative to the base_link frame +# Units: meters for xyz, radians for rpy (roll, pitch, yaw) + +sensors: + # IMU sensor (rear, at GPS base location) + imu: + x: -0.25 + y: 0.0 + z: 0.47 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + topic: "/gps_base/yaw" + + # IMU sensor 1 (front) + imu1: + x: 0.25 + y: 0.0 + z: 0.47 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + topic: "/imu/data" + + # Front depth camera + front_camera: + x: 0.55 + y: 0.0 + z: 0.72 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # Back depth camera + back_camera: + x: -0.55 + y: 0.0 + z: 0.72 + roll: 0.0 + pitch: 0.0 + yaw: 3.14159265359 # 180 degrees (facing backward) + + # GPS base antenna + gps_base: + x: -0.25 + y: 0.0 + z: 0.47 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # Front Mid-360 LiDAR + # Note: The key name here (front_lidar) maps to xacro argument prefix front_lidar_* + front_lidar: + x: 0.56 + y: 0.235 + z: 0.46 + # RPY values differ between simulation and real hardware + # Simulation uses: 0 0 0 + # Real hardware uses: -0.0174533 0 0.00872665 + roll_sim: 0.0 + pitch_sim: 0.0 + yaw_sim: 0.0 + roll_real: -0.0174533 + pitch_real: 0.0 + yaw_real: 0.00872665 + topic: "front_lidar/points" + + # Back Mid-360 LiDAR + # Note: The key name here (back_lidar) maps to xacro argument prefix back_lidar_* + back_lidar: + x: -0.56 + y: -0.235 + z: 0.46 + # RPY values differ between simulation and real hardware + # Simulation uses: 0 0 3.14159 + # Real hardware uses: 0 0 3.13286335 + roll_sim: 0.0 + pitch_sim: 0.0 + yaw_sim: 3.14159265359 # 180 degrees + roll_real: 0.0 + pitch_real: 0.0 + yaw_real: 3.13286335 + topic: "back_lidar/points" + +# Optional sensors (currently commented out in main xacro) +# Uncomment and configure if needed for specific robots +optional_sensors: + # front_lidar: # 2D LiDAR (not Mid-360) + # x: 0.615 + # y: 0.28 + # z: 0.33 + # roll: 0.0 + # pitch: 0.0 + # yaw: 0.0 + + # back_lidar: # 2D LiDAR (not Mid-360) + # x: -0.565 + # y: -0.28 + # z: 0.33 + # roll: 0.0 + # pitch: 0.0 + # yaw: 3.14159265359 + + # back_antenna: + # x: -0.5 + # y: -0.245 + # z: 0.80 + # roll: 0.0 + # pitch: 0.0 + # yaw: 0.0 + + # front_antenna: + # x: 0.5 + # y: -0.245 + # z: 0.80 + # roll: 0.0 + # pitch: 0.0 + # yaw: 0.0 + + # fish_front_camera: + # x: 0.55 + # y: 0.0 + # z: 0.93 + # roll: 0.0 + # pitch: 0.0 + # yaw: 0.0 + + # fish_right_camera: + # x: 0.495 + # y: 0.08 + # z: 0.93 + # roll: 0.0 + # pitch: 0.0 + # yaw: 1.57079632679 # 90 degrees + + # fish_back_camera: + # x: 0.179 + # y: 0.0 + # z: 1.28 + # roll: 0.0 + # pitch: 0.0 + # yaw: 3.14159265359 # 180 degrees + + # fish_left_camera: + # x: 0.495 + # y: -0.08 + # z: 0.93 + # roll: 0.0 + # pitch: 0.0 + # yaw: -1.57079632679 # -90 degrees diff --git a/hunter_pltf_description/config/robots/hunter_01.yaml b/hunter_pltf_description/config/robots/hunter_01.yaml new file mode 100644 index 0000000..4e27042 --- /dev/null +++ b/hunter_pltf_description/config/robots/hunter_01.yaml @@ -0,0 +1,86 @@ +# Robot-specific configuration for Hunter-01 +# This file contains calibrated sensor poses for the Hunter-01 robot instance +# Only values that differ from default.yaml need to be specified here + +robot_id: "hunter_01" + +# Sensor pose overrides for Hunter-01 +# This robot has been calibrated and has specific mounting variations +# Calibration date: 2024-11-10 +# Calibration notes: Minor adjustments for manufacturing tolerances and physical measurements + +sensors: + # IMU sensor (rear, at GPS base location) + # Slight offset due to mounting bracket tolerance + imu: + x: -0.251 + y: 0.001 + z: 0.471 + # roll, pitch, yaw use defaults (0.0, 0.0, 0.0) + # topic uses default: "/gps_base/yaw" + + # IMU sensor 1 (front) + # Adjusted based on physical measurement + imu1: + x: 0.249 + y: -0.002 + z: 0.469 + # roll, pitch, yaw use defaults (0.0, 0.0, 0.0) + # topic uses default: "/imu/data" + + # Front depth camera + # Fine-tuned position after installation + front_camera: + x: 0.552 + y: 0.001 + z: 0.721 + # roll, pitch, yaw use defaults (0.0, 0.0, 0.0) + + # Back depth camera + # Adjusted for mounting surface variation + back_camera: + x: -0.548 + y: -0.001 + z: 0.719 + yaw: 3.14259265359 # Slightly adjusted from default 180 degrees + + # GPS base antenna + # Matches IMU position (co-located) + gps_base: + x: -0.251 + y: 0.001 + z: 0.471 + # roll, pitch, yaw use defaults (0.0, 0.0, 0.0) + + # Front Mid-360 LiDAR + # Calibrated orientation for this specific unit + front_lidar: + x: 0.561 + y: 0.236 + z: 0.461 + # Simulation RPY values use defaults + # Real hardware RPY values are calibrated: + roll_real: -0.0175 + pitch_real: 0.001 + yaw_real: 0.0088 + # topic uses default: "front_lidar/points" + + # Back Mid-360 LiDAR + # Adjusted for mounting alignment + back_lidar: + x: -0.559 + y: -0.236 + z: 0.461 + # Simulation RPY values use defaults + # Real hardware RPY values are calibrated: + roll_real: 0.001 + pitch_real: 0.0 + yaw_real: 3.13386335 + # topic uses default: "back_lidar/points" + +# Notes: +# - All measurements in meters for xyz, radians for rpy +# - Values not specified here inherit from default.yaml +# - Small variations (< 5mm, < 1 degree) are typical due to manufacturing tolerances +# - These calibrated values should be used for both simulation and real robot deployment +# to maintain consistency across environments diff --git a/hunter_pltf_description/description/hunter_pltf.urdf.xacro b/hunter_pltf_description/description/hunter_pltf.urdf.xacro index 12f7615..097bea9 100755 --- a/hunter_pltf_description/description/hunter_pltf.urdf.xacro +++ b/hunter_pltf_description/description/hunter_pltf.urdf.xacro @@ -2,15 +2,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - - + + @@ -19,21 +82,39 @@ - - + + - - + + - + - - + + \ No newline at end of file diff --git a/hunter_pltf_description/launch/pltf_rsp.launch.py b/hunter_pltf_description/launch/pltf_rsp.launch.py index dd21614..bc6e52f 100755 --- a/hunter_pltf_description/launch/pltf_rsp.launch.py +++ b/hunter_pltf_description/launch/pltf_rsp.launch.py @@ -1,59 +1,95 @@ from launch_ros.actions import Node from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch.conditions import IfCondition +import os +import sys +from pathlib import Path -def generate_launch_description(): - - use_sim_time = LaunchConfiguration('use_sim_time') - gui = LaunchConfiguration("gui") +def generate_robot_description(context, *args, **kwargs): + """Generate robot description with robot-specific configuration.""" + use_sim_time = LaunchConfiguration('use_sim_time').perform(context) + robot_id = LaunchConfiguration('robot_id').perform(context) + + # Import config loader + config_dir = Path(__file__).parent.parent / 'config' / 'robots' + sys.path.insert(0, str(config_dir)) + from config_loader import load_robot_config, get_xacro_args, format_xacro_args + + # Load robot configuration + is_sim = use_sim_time.lower() == 'true' + config = load_robot_config(robot_id) + xacro_args = get_xacro_args(config, is_sim) + + # Build xacro command with all arguments + xacro_file = PathJoinSubstitution( + [FindPackageShare("hunter_pltf_description"), "description", "hunter_pltf.urdf.xacro"] + ) + + # Format all arguments + args_str = f"is_sim:={use_sim_time} prefix:='' {format_xacro_args(xacro_args)}" robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", - PathJoinSubstitution( - [FindPackageShare("hunter_pltf_description"), "description" ,"hunter_pltf.urdf.xacro"] - ), - " ", - "is_sim:=", - use_sim_time, - " ", - "prefix:=''", + xacro_file, " ", + args_str, ] ) + + return robot_description_content + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + gui = LaunchConfiguration("gui") + robot_id = LaunchConfiguration('robot_id') + + # Note: robot_description_content will be generated in the OpaqueFunction + # to allow runtime evaluation of robot_id rviz_config_file = PathJoinSubstitution( [FindPackageShare("hunter_description"), "rviz", "robot_view.rviz"] - ) - - # Create a robot_state_publisher node - params = {'robot_description': robot_description_content, 'use_sim_time': use_sim_time} - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[params] ) + + def launch_setup(context, *args, **kwargs): + """Setup launch nodes with robot configuration.""" + robot_description_content = generate_robot_description(context) + + # Create a robot_state_publisher node + params = {'robot_description': robot_description_content, 'use_sim_time': use_sim_time} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + return [ + node_robot_state_publisher, + joint_state_publisher_node, + rviz_node, + ] # Launch! return LaunchDescription([ @@ -68,7 +104,11 @@ def generate_launch_description(): description="Start Rviz2 and Joint State Publisher gui automatically \ with this launch file.", ), - node_robot_state_publisher, - joint_state_publisher_node, - rviz_node, + DeclareLaunchArgument( + 'robot_id', + default_value='default', + description='Robot instance ID (e.g., hunter_01, hunter_02). Uses robot-specific \ + configuration from config/robots/.yaml. Defaults to default.yaml.', + ), + OpaqueFunction(function=launch_setup), ]) diff --git a/hunter_pltf_gazebo/launch/launch_sim.launch.py b/hunter_pltf_gazebo/launch/launch_sim.launch.py index cc09a8f..c14f94c 100755 --- a/hunter_pltf_gazebo/launch/launch_sim.launch.py +++ b/hunter_pltf_gazebo/launch/launch_sim.launch.py @@ -1,10 +1,13 @@ import os +import sys +from pathlib import Path from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler, + OpaqueFunction, ) from launch_ros.actions import Node from launch.conditions import IfCondition @@ -32,35 +35,57 @@ def generate_launch_description(): yaw = LaunchConfiguration('yaw', default='1.45') use_rviz = LaunchConfiguration('use_rviz', default='false') use_gazebo = LaunchConfiguration('use_gazebo', default='true') + robot_id = LaunchConfiguration('robot_id', default='default') # Gazebo parameters gazebo_params_file = os.path.join(hunter_gazebo_pkg_dir, 'config', 'gazebo_params.yaml') - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("hunter_pltf_description"), "description" ,"hunter_pltf.urdf.xacro"] - ), - " ", - "is_sim:=", - use_sim_time, - " ", - "prefix:=''", - " ", - ] - ) - robot_description = { - "robot_description": ParameterValue(robot_description_content, value_type=str) - } - # Create a robot_state_publisher node - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[robot_description, {'use_sim_time': use_sim_time}] - ) + def generate_robot_description(context, *args, **kwargs): + """Generate robot description with robot-specific configuration.""" + use_sim_time_val = LaunchConfiguration('use_sim_time').perform(context) + robot_id_val = LaunchConfiguration('robot_id').perform(context) + + # Import config loader + config_dir = Path(get_package_share_directory('hunter_pltf_description')) / 'config' / 'robots' + sys.path.insert(0, str(config_dir)) + from config_loader import load_robot_config, get_xacro_args, format_xacro_args + + # Load robot configuration + is_sim = use_sim_time_val.lower() == 'true' + config = load_robot_config(robot_id_val) + xacro_args = get_xacro_args(config, is_sim) + + # Build xacro command with all arguments + xacro_file = PathJoinSubstitution( + [FindPackageShare("hunter_pltf_description"), "description", "hunter_pltf.urdf.xacro"] + ) + + # Format all arguments + args_str = f"is_sim:={use_sim_time_val} prefix:='' {format_xacro_args(xacro_args)}" + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + xacro_file, + " ", + args_str, + ] + ) + + robot_description = { + "robot_description": ParameterValue(robot_description_content, value_type=str) + } + + # Create a robot_state_publisher node + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description, {'use_sim_time': use_sim_time}] + ) + + return [node_robot_state_publisher] # Include the Gazebo launch file, provided by the gazebo_ros package gazebo = IncludeLaunchDescription( @@ -125,6 +150,9 @@ def generate_launch_description(): DeclareLaunchArgument('world_path', default_value=world_path, description='Gazebo world file path'), DeclareLaunchArgument('use_rviz', default_value='false', description='Whether to start RViZ'), DeclareLaunchArgument('use_gazebo', default_value='true', description='Whether to start Gazebo'), + DeclareLaunchArgument('robot_id', default_value='default', + description='Robot instance ID (e.g., hunter_01, hunter_02). Uses robot-specific \ + configuration from config/robots/.yaml. Defaults to default.yaml.'), RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, @@ -139,6 +167,6 @@ def generate_launch_description(): ), gazebo, rviz, - node_robot_state_publisher, + OpaqueFunction(function=generate_robot_description), spawn_entity, ])