sl-controls
570ebd3d22
Implement magnetometer-based heading calculation with tilt compensation and magnetic declination correction for France (1.5° east). Features: - Tilt-compensated heading using quaternion-based orientation Roll and pitch compensation for uneven terrain - Magnetic declination correction: 1.5° for France - Heading normalization to 0-360 degree range - Publishes both Float64 (degrees) and quaternion representations - 10Hz publishing frequency with configurable parameters Algorithm: - Subscribe to IMU (quaternion orientation) and magnetometer data - Convert quaternion to roll/pitch/yaw for tilt compensation - Project magnetometer vector onto horizontal plane using trig functions - Apply declination correction and normalize heading - Publish heading as Float64 degrees and quaternion (Z-axis rotation only) Test Coverage: - 30+ unit tests covering: - Node initialization and parameters - Quaternion to Euler conversion (identity, 90° rotations) - Heading quaternion creation (0°, 90°, 180°, custom angles) - Tilt-compensated heading with roll, pitch, combined tilts - Declination correction application - Sensor subscription handlers - Heading angle normalization and wrapping - Realistic scenarios (level, tilted uphill/sideways, 3D tilt, weak signal, continuous rotation) Topics: - Subscribed: /saltybot/imu/data (Imu), /saltybot/mag (MagneticField) - Published: /saltybot/heading (Float64), /saltybot/heading_quaternion (QuaternionStamped) Config: frequency=10Hz, declination_deg=1.5 Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
37 lines
1.1 KiB
Python
37 lines
1.1 KiB
Python
"""Launch file for compass_heading_node."""
|
|
|
|
from launch import LaunchDescription
|
|
from launch_ros.actions import Node
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch.actions import DeclareLaunchArgument
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
|
|
def generate_launch_description():
|
|
"""Generate launch description for compass heading node."""
|
|
# Package directory
|
|
pkg_dir = get_package_share_directory("saltybot_compass")
|
|
|
|
# Parameters
|
|
config_file = os.path.join(pkg_dir, "config", "compass_config.yaml")
|
|
|
|
# Declare launch arguments
|
|
return LaunchDescription(
|
|
[
|
|
DeclareLaunchArgument(
|
|
"config_file",
|
|
default_value=config_file,
|
|
description="Path to configuration YAML file",
|
|
),
|
|
# Compass heading node
|
|
Node(
|
|
package="saltybot_compass",
|
|
executable="compass_heading_node",
|
|
name="compass_heading",
|
|
output="screen",
|
|
parameters=[LaunchConfiguration("config_file")],
|
|
),
|
|
]
|
|
)
|