Skip to content

Commit

Permalink
Adds GQ7 as possible sensor
Browse files Browse the repository at this point in the history
  • Loading branch information
robbiefish committed Jul 31, 2024
1 parent bfe7c92 commit cd82749
Show file tree
Hide file tree
Showing 4 changed files with 139 additions and 1 deletion.
49 changes: 49 additions & 0 deletions clearpath_config/sample/j100/j100_microstrain_gq7.yaml
Original file line number Diff line number Diff line change
@@ -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"
3 changes: 3 additions & 0 deletions clearpath_config/sensors/sensors.py
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
from clearpath_config.sensors.types.gps import (
BaseGPS,
SwiftNavDuro,
MicrostrainGQ7,
Garmin18x,
NovatelSmart6,
NovatelSmart7,
Expand Down Expand Up @@ -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,
Expand Down
84 changes: 84 additions & 0 deletions clearpath_config/sensors/types/gps.py
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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"

Expand Down
4 changes: 3 additions & 1 deletion clearpath_config/tests/test_samples.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -49,7 +50,8 @@
J100_SAMPLES = [
J100_DEFAULT,
J100_DUAL_LASER,
J100_VELODYNE
J100_VELODYNE,
J100_MICROSTRAIN_GQ7
]


Expand Down

0 comments on commit cd82749

Please sign in to comment.