Merge pull request #71 from robbiefish/main

Adds GQ7 as possible sensor

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"

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,

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"
 

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
 ]