From cd827496c3f5afb5c2be1c9aafdea88b16b34ee6 Mon Sep 17 00:00:00 2001 From: robbiefish Date: Wed, 31 Jul 2024 20:07:58 +0000 Subject: [PATCH] Adds GQ7 as possible sensor --- .../sample/j100/j100_microstrain_gq7.yaml | 49 +++++++++++ clearpath_config/sensors/sensors.py | 3 + clearpath_config/sensors/types/gps.py | 84 +++++++++++++++++++ clearpath_config/tests/test_samples.py | 4 +- 4 files changed, 139 insertions(+), 1 deletion(-) create mode 100644 clearpath_config/sample/j100/j100_microstrain_gq7.yaml diff --git a/clearpath_config/sample/j100/j100_microstrain_gq7.yaml b/clearpath_config/sample/j100/j100_microstrain_gq7.yaml new file mode 100644 index 0000000..8dab402 --- /dev/null +++ b/clearpath_config/sample/j100/j100_microstrain_gq7.yaml @@ -0,0 +1,49 @@ +serial_number: j100-0000 +version: 0 +system: + hosts: + - hostname: cpr-j100-0000 + ip: 192.168.131.1 + ros2: + namespace: j100_0000 +platform: + attachments: + - name: front_fender + type: j100.fender + - name: rear_fender + type: j100.fender + rpy: [0.0, 0.0, 3.1415] +links: + box: + - name: antenna_bar + parent: default_mount + xyz: [0.0, 0.0, 0.0175] + rpy: [0.0, 0.0, 0.0] + size: [0.075, 1.0, 0.035] + cylinder: + - name: left_antenna + parent: antenna_bar_link + xyz: [0.0, 0.475, 0.0225] + rpy: [0.0, 0.0, 0.0] + radius: 0.02 + length: 0.01 + - name: right_antenna + parent: antenna_bar_link + xyz: [0.0, -0.475, 0.0225] + rpy: [0.0, 0.0, 0.0] + radius: 0.02 + length: 0.01 +sensors: + gps: + - model: microstrain_gq7 + urdf_enabled: true + launch_enabled: true + parent: rear_0_mount + xyz: [0.0, 0.0, 0.0] + rpy: [0.0, 0.0, 0.0] + ros_parameters: + microstrain_inertial_driver: + port: "/dev/microstrain_main" + baudrate: 115200 + gnss1_frame_id: "left_antenna_link" + gnss2_frame_id: "right_antenna_link" diff --git a/clearpath_config/sensors/sensors.py b/clearpath_config/sensors/sensors.py index a6cc389..aef1e3a 100644 --- a/clearpath_config/sensors/sensors.py +++ b/clearpath_config/sensors/sensors.py @@ -40,6 +40,7 @@ from clearpath_config.sensors.types.gps import ( BaseGPS, SwiftNavDuro, + MicrostrainGQ7, Garmin18x, NovatelSmart6, NovatelSmart7, @@ -114,12 +115,14 @@ def __new__(cls, model: str) -> BaseCamera: class GlobalPositioningSystem(): SWIFTNAV_DURO = SwiftNavDuro.SENSOR_MODEL + MICROSTRAIN_GQ7 = MicrostrainGQ7.SENSOR_MODEL GARMIN_18X = Garmin18x.SENSOR_MODEL NOVATEL_SMART6 = NovatelSmart6.SENSOR_MODEL NOVATEL_SMART7 = NovatelSmart7.SENSOR_MODEL MODEL = { SWIFTNAV_DURO: SwiftNavDuro, + MICROSTRAIN_GQ7: MicrostrainGQ7, GARMIN_18X: Garmin18x, NOVATEL_SMART6: NovatelSmart6, NOVATEL_SMART7: NovatelSmart7, diff --git a/clearpath_config/sensors/types/gps.py b/clearpath_config/sensors/types/gps.py index 9aba9f8..a68d1d5 100644 --- a/clearpath_config/sensors/types/gps.py +++ b/clearpath_config/sensors/types/gps.py @@ -123,6 +123,9 @@ def get_frame_id(self) -> str: def set_frame_id(self, link: str) -> None: self.frame_id = link + def has_imu(self) -> bool: + return False + class SwiftNavDuro(BaseGPS): SENSOR_MODEL = "swiftnav_duro" @@ -217,6 +220,87 @@ def set_port(self, port: int) -> None: self.port = port +class MicrostrainGQ7(BaseGPS): + SENSOR_MODEL = "microstrain_gq7" + + FRAME_ID = "link" + PORT = "/dev/microstrain_main" + BAUD = 115200 + + class ROS_PARAMETER_KEYS: + FRAME_ID = "microstrain_inertial_driver.frame_id" + PORT = "microstrain_inertial_driver.port" + BAUD = "microstrain_inertial_driver.baudrate" + + class TOPICS: + FIX = "fix" + NAME = { + FIX: "fix", + } + RATE = { + FIX: 60, + } + + def __init__( + self, + idx: int = None, + name: str = None, + topic: str = BaseGPS.TOPIC, + frame_id: str = FRAME_ID, + port: str = PORT, + baud: int = BAUD, + urdf_enabled: bool = BaseSensor.URDF_ENABLED, + launch_enabled: bool = BaseSensor.LAUNCH_ENABLED, + ros_parameters: str = BaseSensor.ROS_PARAMETERS, + parent: str = Accessory.PARENT, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY + ) -> None: + # Port + self.port = port + # Baud + self.baud = baud + # ROS Paramater Template + ros_parameters_template = { + self.ROS_PARAMETER_KEYS.PORT: MicrostrainGQ7.port, + self.ROS_PARAMETER_KEYS.BAUD: MicrostrainGQ7.baud + } + super().__init__( + idx, + name, + topic, + frame_id, + urdf_enabled, + launch_enabled, + ros_parameters, + ros_parameters_template, + parent, + xyz, + rpy + ) + + @property + def port(self) -> str: + return str(self._port) + + @port.setter + def port(self, file: str) -> str: + self._port = File(str(file)) + + @property + def baud(self) -> int: + return self._baud + + @baud.setter + def baud(self, baud: int) -> None: + assert isinstance(baud, int), ("Baud must be of type 'int'.") + assert baud >= 0, ("Baud must be positive integer.") + self._baud = baud + + def has_imu(self) -> bool: + return True + + class NMEA(BaseGPS): SENSOR_MODEL = "nmea_gps" diff --git a/clearpath_config/tests/test_samples.py b/clearpath_config/tests/test_samples.py index 090e67f..8a979e3 100644 --- a/clearpath_config/tests/test_samples.py +++ b/clearpath_config/tests/test_samples.py @@ -39,6 +39,7 @@ J100_DEFAULT = sample + "/j100/j100_default.yaml" J100_DUAL_LASER = sample + "/j100/j100_dual_laser.yaml" J100_VELODYNE = sample + "/j100/j100_velodyne.yaml" +J100_MICROSTRAIN_GQ7 = sample + "/j100/j100_microstrain_gq7.yaml" A200_SAMPLES = [ A200_DEFAULT, @@ -49,7 +50,8 @@ J100_SAMPLES = [ J100_DEFAULT, J100_DUAL_LASER, - J100_VELODYNE + J100_VELODYNE, + J100_MICROSTRAIN_GQ7 ]