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>
28 lines
920 B
Python
28 lines
920 B
Python
from setuptools import find_packages, setup
|
|
|
|
package_name = "saltybot_compass"
|
|
|
|
setup(
|
|
name=package_name,
|
|
version="0.1.0",
|
|
packages=find_packages(exclude=["test"]),
|
|
data_files=[
|
|
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
|
("share/" + package_name, ["package.xml"]),
|
|
("share/" + package_name + "/launch", ["launch/compass_heading.launch.py"]),
|
|
("share/" + package_name + "/config", ["config/compass_config.yaml"]),
|
|
],
|
|
install_requires=["setuptools"],
|
|
zip_safe=True,
|
|
maintainer="Seb",
|
|
maintainer_email="seb@vayrette.com",
|
|
description="Tilt-compensated compass heading node with magnetic declination correction",
|
|
license="Apache-2.0",
|
|
tests_require=["pytest"],
|
|
entry_points={
|
|
"console_scripts": [
|
|
"compass_heading_node = saltybot_compass.compass_heading_node:main",
|
|
],
|
|
},
|
|
)
|