diff --git a/clearpath_launch_generator/clearpath_launch_generator/sensors.py b/clearpath_launch_generator/clearpath_launch_generator/sensors.py
index afe6b14..0084631 100644
--- a/clearpath_launch_generator/clearpath_launch_generator/sensors.py
+++ b/clearpath_launch_generator/clearpath_launch_generator/sensors.py
@@ -33,6 +33,7 @@
from clearpath_config.sensors.base import BaseSensor
from clearpath_config.sensors.lidars_2d import HokuyoUST10, SickLMS1XX
from clearpath_config.sensors.cameras import IntelRealsense
+from clearpath_config.sensors.imu import Microstrain
from clearpath_config.parser import ClearpathConfigParser
from clearpath_launch_generator.launch_writer import LaunchFile, Package, ParameterFile
@@ -149,6 +150,11 @@ def __init__(self, sensor: IntelRealsense, output_path: str = '/etc/clearpath/se
super().__init__(sensor, output_path)
self.generate_config()
+ class MicrostrainIMULaunch(BaseLaunch):
+ def __init__(self, sensor: Microstrain, output_path: str = '/etc/clearpath/sensors/') -> None:
+ super().__init__(sensor, output_path)
+ self.generate_config()
+
class VelodyneLidarLaunch(BaseLaunch):
def __init__(self, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensors/') -> None:
super().__init__(sensor, output_path)
@@ -185,7 +191,8 @@ def generate_config(self):
MODEL = {
HokuyoUST10.SENSOR_MODEL: HokuyoUST10Launch,
SickLMS1XX.SENSOR_MODEL: SickLMS1XXLaunch,
- IntelRealsense.SENSOR_MODEL: IntelRealsenseLaunch
+ IntelRealsense.SENSOR_MODEL: IntelRealsenseLaunch,
+ Microstrain.SENSOR_MODEL: MicrostrainIMULaunch
}
def __new__(cls, sensor: BaseSensor, output_path: str = '/etc/clearpath/sensors/') -> BaseLaunch:
diff --git a/clearpath_sensors/config/microstrain_imu.yaml b/clearpath_sensors/config/microstrain_imu.yaml
new file mode 100644
index 0000000..defe75e
--- /dev/null
+++ b/clearpath_sensors/config/microstrain_imu.yaml
@@ -0,0 +1,501 @@
+microstrain_inertial_driver:
+ ros__parameters:
+ # Standalone example params file for GX3, GX4, GX/CX5, RQ1 and GQ7 series devices
+ # Note: Feature support is device-dependent and some of the following settings may have no affect on your device.
+ # Please consult your device's documentation for supported features
+
+ # ******************************************************************
+ # NOTE: This file is formatted to work with ROS and will not work if specified as the params_file argument in ROS2.
+ # If you want to override parameters for ROS2, start with https://github.com/LORD-MicroStrain/microstrain_inertial/blob/ros2/microstrain_inertial_driver/config/empty.yml
+ # ******************************************************************
+
+ # ******************************************************************
+ # General Settings
+ # ******************************************************************
+
+ # port is the main port that the device will communicate over. For all devices except the GQ7, this is the only available port.
+ # aux_port is only available for the GQ7 and is only needed when streaming RTCM corrections to the device from ROS, or if you want to publish NMEA sentences from this node
+ port: "/dev/ttyACM0"
+ aux_port: "/dev/ttyACM1"
+ baudrate: 115200
+ debug: False
+ diagnostics: False
+
+ # If set to true, this will configure the requested baudrate on the device.
+ # Note that this will be set on both USB and serial, but will only actually affect the baudrate of a serial connection.
+ set_baud: False
+
+ # Frame IDs used in the different messages. By default these are set to arbitrary strings as not to interfere with other ROS services.
+ # For more information on common frame IDs, check out: https://www.ros.org/reps/rep-0105.html
+ #
+ # filter_frame_id and filter_child_frame_id are specifically useful as the node will also publish a transform to the /tf topic
+ # that contains the transform between these two frames. Many ROS tools such as RViz will use the /tf topic to display things like robot position.
+ imu_frame_id: "sensor"
+ gnss1_frame_id: "gnss1_antenna_wgs84"
+ gnss2_frame_id: "gnss2_antenna_wgs84"
+ filter_frame_id: "sensor_wgs84"
+ filter_child_frame_id: "sensor"
+ nmea_frame_id: "nmea"
+
+ # Waits for a configurable amount of time until the device exists
+ # If poll_max_tries is set to -1 we will poll forever until the device exists
+ poll_port: False
+ poll_rate_hz: 1.0
+ poll_max_tries: 60
+
+ # Controls if the driver outputs data with-respect-to ENU frame
+ # false - position, velocity, and orientation are WRT the NED frame (native device frame)
+ # true - position, velocity, and orientation are WRT the ENU frame
+ use_enu_frame: False
+
+ # Controls if the driver-defined setup is sent to the device
+ # false - The driver will ignore the settings below and use the device's current settings
+ # true - Overwrite the current device settings with those listed below
+ device_setup: True
+
+ # Controls if the driver-defined settings are saved
+ # false - Do not save the settings
+ # true - Save the settings in the device's non-volatile memory
+ save_settings: False
+
+ # Controls if the driver uses the device generated timestamp (if available) for timestamping messages
+ # false - Use PC received time for timestamping
+ # true - Use device generated timestamp
+ use_device_timestamp: False
+
+ # Controls if the driver uses ROS time for timestamping messages. Can be useful when running under simulation.
+ # NOTE: This can be used in conjunction with use_device_timestamp in which case, the device timestmap will be used for sensor_msgs/TimeReference messages, and ROS time will be used for all other messages
+ # false - Use PC received time for timestamping
+ # true - Use ROS time for timestamping all non sensor_msgs/TimeReference messages
+ use_ros_time: False
+
+ # Controls if the driver creates a raw binary file
+ # false - Do not create the file
+ # true - Create the file
+ #
+ # Notes: 1) The filename will have the following format -
+ # model_number "_" serial_number "_" datetime (year_month_day_hour_minute_sec) ".bin"
+ # example: "3DM-GX5-45_6251.00001_20_12_01_01_01_01.bin"
+ # 2) This file is useful for getting support from the manufacturer
+ raw_file_enable: False
+
+ # (GQ7/CV7 only) Controls if the driver requests additional factory support data to be included in the raw data file
+ # false - Do not request the additional data
+ # true - Request the additional channels (please see notes below!)
+ #
+ # Notes: **We recommend only enabling this feature when specifically requested by Microstrain.**
+ #
+ # Including this feature increases communication bandwidth requirements significantly... for serial data connections
+ # please ensure the baudrate is sufficient for the added data channels.
+ raw_file_include_support_data: False
+
+ # The directory to store the raw data file (no trailing '/')
+ raw_file_directory: "/home/your_name"
+
+ # ******************************************************************
+ # IMU Settings
+ # ******************************************************************
+ imu_data_rate: 100
+
+ # The speed at which the individual IMU publishers will publish at.
+ # If set to -1, will use imu_data_rate to determine the rate at which to stream and publish
+ # If set to 0, the stream will be turned off and the publisher will not be created
+ imu_raw_data_rate: -1 # Rate of imu/data topic
+ imu_mag_data_rate: -1 # Rate of mag topic
+ imu_gps_corr_data_rate:
+ -1 # Rate of gps_corr topic
+ # Note that for philo devices (GX5, CV5, CX5), this will be published at the highest IMU data rate if "use_device_timestamp" is set to true
+ imu_overrange_status_data_rate: -1 # Rate of imu/overrange_status topic
+
+ # Static IMU message covariance values (the device does not generate these)
+ # Since internally these are std::vector we need to use the rosparam tags
+ imu_orientation_cov: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+ imu_linear_cov: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+ imu_angular_cov: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+
+ # ******************************************************************
+ # GNSS Settings (only applicable for devices with GNSS)
+ # ******************************************************************
+ gnss1_data_rate: 2
+
+ # The speed at which the individual GNSS1 publishers will publish at.
+ # If set to -1, will use gnss1_data_rate to determine the rate at which to stream and publish
+ # If set to 0, the stream will be turned off and the publisher will not be created
+ #
+ # Note: The parameters' "gnss1_nav_sat_fix_data_rate", "gnss1_odom_data_rate" associated ROS messages share several MIP fields,
+ # so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate
+ gnss1_nav_sat_fix_data_rate: -1 # Rate of gnss1/fix topic
+ gnss1_odom_data_rate: -1 # Rate of gnss1/odom topic
+ gnss1_time_reference_data_rate:
+ -1 # Rate of gnss1/time_ref topic
+ # Note that on philo devices, if use_device_timestamp is set to true, this will be streamed at the highest GNSS rate
+ gnss1_fix_info_data_rate: -1 # Rate of gnss1/fix_info topic
+ gnss1_sbas_info_data_rate: -1 # Rate of gnss1/sbas_info topic
+ gnss1_rf_error_detection_data_rate: -1 # Rate of gnss1/rf_error_detection topic
+
+ # Antenna #1 lever arm offset vector
+ # For GQ7 - in the vehicle frame wrt IMU origin (meters)
+ # For all other models - in the IMU frame wrt IMU origin (meters)
+ # Note: Make this as accurate as possible for good performance
+ gnss1_antenna_offset: [0.0, -0.7, -1.0]
+
+ # GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7)
+ gnss2_data_rate: 2
+
+ # The speed at which the individual GNSS2 publishers will publish at.
+ # If set to -1, will use gnss2_data_rate to determine the rate at which to stream and publish
+ # If set to 0, the stream will be turned off and the publisher will not be created
+ #
+ # Note: The parameters' "gnss2_nav_sat_fix_data_rate", "gnss2_odom_data_rate" associated ROS messages share several MIP fields,
+ # so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate
+ gnss2_nav_sat_fix_data_rate: -1 # Rate of gnss2/fix topic
+ gnss2_odom_data_rate: -1 # Rate of gnss2/odom topic
+ gnss2_time_reference_data_rate: -1 # Rate of gnss2/time_ref topic
+ gnss2_fix_info_data_rate: -1 # Rate of gnss2/fix_info topic
+ gnss2_sbas_info_data_rate: -1 # Rate of gnss2/sbas_info topic
+ gnss2_rf_error_detection_data_rate: -1 # Rate of gnss1/rf_error_detection topic
+
+ # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )
+ # Note: Make this as accurate as possible for good performance
+ gnss2_antenna_offset: [0.0, 0.7, -1.0]
+
+ # (GQ7 Only) Enable RTK dongle interface
+ # Note: Enabling this will cause the node to publish on one of two topics depending
+ # on the version of the RTK dongle connected to the GQ7
+ # Note: This is also required to be True in order to publish NMEA from the aux port
+ rtk_dongle_enable: False
+
+ # Speed at which RTK data will be published if an individual topic data rate is not specified
+ rtk_data_rate: 1
+
+ # The speed at which the individual RTK publishers will publish at.
+ # If set to 0, the stream will be turned off and the publisher will not be created
+ rtk_status_data_rate: 1 # Rate of rtk/status or rtk/status_v1 topics
+
+ # (GQ7 Only) Allow the user to send RTCM messages to this node, and stream those messages to the GQ7
+ subscribe_rtcm: False
+ rtcm_topic: "/rtcm"
+
+ # (GQ7 Only) Send NMEA sentences out on a ROS topic
+ # Note: Prior to version 3.0.0, this would only publish NMEA sentences from the aux port.
+ # This option may now be used to publish NMEA sentences from the main port as well.
+ # Note: If the main port is configured to stream NMEA sentences, and the aux port is also connected,
+ # this will publish NMEA sentences from both the main and aux port on the same topic.
+ publish_nmea: False
+
+ # ******************************************************************
+ # Kalman Filter Settings (only applicable for devices with a Kalman Filter)
+ # ******************************************************************
+ filter_data_rate: 10
+
+ # The speed at which the individual Filter publishers will publish at.
+ # If set to -1, will use filter_data_rate to determine the rate at which to stream and publish
+ # If set to 0, the stream will be turned off and the publisher will not be created
+ #
+ # Note: The parameters' "filter_odom_data_rate", "filter_imu_data_rate", "filter_relative_odom_data_rate" associated ROS messages share several MIP fields,
+ # so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate
+ filter_status_data_rate: -1 # Rate of nav/status topic
+ filter_heading_data_rate: -1 # Rate of nav/heading topic
+ filter_heading_state_data_rate: -1 # Rate of nav/heading_state topic
+ filter_odom_data_rate: -1 # Rate of nav/odom topic
+ filter_imu_data_rate: -1 # Rate of nav/filtered_imu/data
+ filter_relative_odom_data_rate:
+ -1 # Rate of nav/relative_pos topic and the transform between filter_frame_id and filter_child_frame_id
+ # Note that this data rate will also control the rate at which the transform between "filter_frame_id" and "filter_child_frame_id" will be published at
+ filter_aiding_status_data_rate: -1 # Rate of gnss1/aiding_status and gnss2/aiding_status topics
+ filter_antenna_offset_correction_data_rate: -1 # Rate of gnss1/antenna_offset_corection and gnss2/antenna_offset_correction topics
+ filter_gnss_dual_antenna_data_rate: -1 # Rate of nav/dual_antenna_status topic
+ filter_aiding_measurement_summary_data_rate: -1 # Rate of nav/aiding_summary topic
+
+ # Sensor2vehicle frame transformation selector
+ # 0 = None, 1 = Euler Angles, 2 - matrix, 3 - quaternion
+ # Notes: These are different ways of setting the same parameter in the device.
+ # The different options are provided as a convenience.
+ # Support for matrix and quaternion options is firmware version dependent (GQ7 supports Quaternion as of firmware 1.0.07)
+ # Quaternion order is [i, j, k, w]
+ filter_sensor2vehicle_frame_selector: 1
+
+ filter_sensor2vehicle_frame_transformation_euler: [0.0, 0.0, 0.0]
+ filter_sensor2vehicle_frame_transformation_matrix:
+ [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+ filter_sensor2vehicle_frame_transformation_quaternion: [0.0, 0.0, 0.0, 1.0]
+
+ # Controls if the Kalman filter is reset after the settings are configured
+ filter_reset_after_config: True
+
+ # Controls if the Kalman filter will auto-init or requires manual initialization
+ filter_auto_init: True
+
+ # (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual
+ # Note: When using a CV7, this MUST be changed to either 1, or 3 or the node will not start
+ filter_declination_source: 2
+ filter_declination: 0.23
+
+ # (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)
+ # Note: When using a -10/-AR product. This MUST be set to 0 or the node will not start
+ filter_heading_source: 1
+
+ # (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs)
+ filter_dynamics_mode: 1
+
+ # Controls what kind of linear acceleration data is used in the Filter IMU message.
+ # If this is set to true, the acceleration will not factor out gravity, if set to false gravity will be filtered out of the linear acceleration.
+ filter_use_compensated_accel: True
+
+ # ZUPT control
+ filter_velocity_zupt_topic: "/moving_vel"
+ filter_angular_zupt_topic: "/moving_ang"
+ filter_velocity_zupt: True
+ filter_angular_zupt: True
+
+ # (GQ7/CV7 full support, GX5-45 limited support) Adaptive filter settings
+ # Adaptive level: 0 - off, 1 - Conservative, 2 = Moderate (default), 3 = agressive
+ # Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000
+ filter_adaptive_level: 2
+ filter_adaptive_time_limit_ms: 15000
+
+ # (GQ7/CV7 only) Aiding measurement control
+ filter_enable_gnss_pos_vel_aiding: True
+ filter_enable_gnss_heading_aiding: True
+ filter_enable_altimeter_aiding: False
+ filter_enable_odometer_aiding: False
+ filter_enable_magnetometer_aiding: False
+ filter_enable_external_heading_aiding: False
+
+ # External GPS Time Update Control
+ # Notes: filter_external_gps_time_topic should publish at no more than 1 Hz.
+ # gps_leap_seconds should be updated to reflect the current number
+ # of leap seconds.
+ filter_enable_external_gps_time_update: False
+ filter_external_gps_time_topic: "/external_gps_time"
+ gps_leap_seconds: 18.0
+
+ # External Speed Control. This node will subscribe on this topic only if filter_enable_odometer_aiding is set to true
+ # Notes: This subscription will be disabled if enable_hardware_odometer is set to true
+ filter_external_speed_topic: "/external_speed"
+
+ # Velocity frame control.
+ # If set to false, the velocity will be reported in the NED/ENU frame
+ # If set to true, the velocity will be reported in the vehicle frame
+ filter_vel_in_vehicle_frame: True
+
+ # (GQ7 Only) Reference point lever arm offset control.
+ # Note: This offset will affect the position and velocity measurements in the following topics: nav/odom, nav/relative_pos/odom
+ # Note: This offset is in the vehicle reference frame.
+ filter_lever_arm_offset: [0.0, 0.0, 0.0]
+
+ # (GQ7 Only) Hardware Odometer Control
+ enable_hardware_odometer: False
+ odometer_scaling: 0.0
+ odometer_uncertainty: 0.0
+
+ # (GQ7/CV7 only) GPIO Configuration
+ # Notes: For information on possible configurations and specific pin options
+ # refer to the MSCL MipNodeFeatures command, supportedGpioConfigurations.
+ #
+ # GQ7 GPIO Pins =
+ # 1 - GPIO1 (primary port pin 7) - Features = 0 - Unused, 1 - GPIO, 2 - PPS, 3 - Encoder
+ # 2 - GPIO2 (primary port pin 9) - Features = 0 - Unused, 1 - GPIO, 2 - PPS, 3 - Encoder
+ # 3 - GPIO3 (aux port pin 7) - Features = 0 - Unused, 1 - GPIO, 2 - PPS, 3 - Encoder
+ # 4 - GPIO4 (aux port pin 9) - Features = 0 - Unused, 1 - GPIO, 2 - PPS, 3 - Encoder
+ #
+ # CV7 GPIO Pins =
+ # 1 - GPIO1 (pin 7) - Features = 0 - Unused, 1 - GPIO, 2 - PPS
+ # 2 - GPIO2 (pin 9) - Features = 0 - Unused, 1 - GPIO, 2 - PPS
+ # 3 - GPIO3 (pin 6) - Features = 0 - Unused, 1 - GPIO, 2 - PPS
+ # 4 - GPIO4 (pin 10) - Features = 0 - Unused, 1 - GPIO, 2 - PPS
+ #
+ # Feature:
+ # 0 - Unused - Behaviors = 0 - unused
+ # 1 - GPIO - Behaviors = 0 - unused, 1 - input, 2 - output low, 3 - output high
+ # 2 - PPS - Behaviors = 0 - unused, 1 - input, 2 - output
+ # 3 - Encoder - Behaviors = 0 - unused, 1 - enc A, 2 - enc B
+ #
+ # GPIO Behavior:
+ # 0 - Unused
+ # 1 - Input
+ # 2 - Output Low
+ # 3 - Output High
+ #
+ # PPS Behavior:
+ # 0 - Unused
+ # 1 - Input
+ # 2 - Output
+ #
+ # Encoder Behavior:
+ # 0 - Unused
+ # 1 - Encoder A
+ # 2 - Encoder B
+ #
+ # Pin Mode Bitfield:
+ # 1 - open drain
+ # 2 - pulldown
+ # 4 - pullup
+ gpio_config: False
+
+ gpio1_feature: 0
+ gpio1_behavior: 0
+ gpio1_pin_mode: 0
+
+ gpio2_feature: 0
+ gpio2_behavior: 0
+ gpio2_pin_mode: 0
+
+ gpio3_feature: 0
+ gpio3_behavior: 0
+ gpio3_pin_mode: 0
+
+ gpio4_feature: 0
+ gpio4_behavior: 0
+ gpio4_pin_mode: 0
+
+ # (GQ7 only) Filter Initialization control
+
+ # Init Condition source =
+ # 0 - auto pos, vel, attitude (default)
+ # 1 - auto pos, vel, roll, pitch, manual heading
+ # 2 - auto pos, vel, manual attitude
+ # 3 - manual pos, vel, attitude
+ #
+ # Auto-Heading alignment selector (note this is a bitfield, you can use more than 1 source) =
+ # Bit 0 - Dual-antenna GNSS
+ # Bit 1 - GNSS kinematic (requires motion, e.g. a GNSS velocity)
+ # Bit 2 - Magnetometer
+ #
+ # Reference frame =
+ # 1 - WGS84 Earth-fixed, earth centered (ECEF) position, velocity, attitude
+ # 2 - WGS84 Latitude, Longitude, height above ellipsoid position, NED velocity and attitude
+ filter_init_condition_src: 0
+ filter_auto_heading_alignment_selector: 1
+ filter_init_reference_frame: 2
+ filter_init_position: [0.0, 0.0, 0.0]
+ filter_init_velocity: [0.0, 0.0, 0.0]
+ filter_init_attitude: [0.0, 0.0, 0.0]
+
+ # (GQ7 only) Relative Position Configuration
+ # Reference frame =
+ # 1 - Relative ECEF position
+ # 2 - Relative LLH position
+ #
+ # Source =
+ # 0 - Position will be reported relative to the base station. filter_relative_position_ref will be ignored
+ # 1 - Position will be reported relative to filter_relative_position_ref
+ #
+ # Reference position - Units provided by reference frame (ECEF - meters, LLH - deg, deg, meters)
+ # Note: prior to firmware version 1.0.06 this command will fail for non-positive heights. 1.0.06 fixes this)
+ filter_relative_position_config: False
+ filter_relative_position_frame: 2
+ filter_relative_position_source: 1
+ filter_relative_position_ref: [0.0, 0.0, 0.01]
+
+ # (GQ7 only) Speed Lever Arm Configuration
+ # Lever Arm - In vehicle reference frame (meters)
+ filter_speed_lever_arm: [0.0, 0.0, 0.0]
+
+ # (GQ7 only) Wheeled Vehicle Constraint Control
+ # Note: When enabled, the filter uses the assumption that velocity is constrained to the primary vehicle axis.
+ # By convention, the primary vehicle axis is the vehicle X-axis
+ filter_enable_wheeled_vehicle_constraint: False
+
+ # (GQ7 only) Vertical Gyro Constraint Control
+ # Note: When enabled and no valid GNSS measurements are available, the filter uses the accelerometers to track
+ # pitch and roll under the assumption that the sensor platform is not undergoing linear acceleration.
+ # This constraint is useful to maintain accurate pitch and roll during GNSS signal outages.
+ filter_enable_vertical_gyro_constraint: False
+
+ # (GQ7 only) GNSS Antenna Calibration Control
+ # Note: When enabled, the filter will enable lever arm error tracking, up to the maximum offset specified in meters.
+ filter_enable_gnss_antenna_cal: True
+ filter_gnss_antenna_cal_max_offset: 0.1
+
+ # (GQ7/CV7 only) PPS Source
+ # PPS Source =
+ # 0 - Disabled
+ # 1 - Reciever 1 (default)
+ # 2 - Reciever 2
+ # 3 - GPIO (provided by external source if supported). Use the GPIO config above to further configure
+ # 4 - Generated from system oscillator
+ filter_pps_source: 1
+
+ # (GQ7 only) SBAS options
+ sbas_enable: True
+ sbas_enable_ranging: False
+ sbas_enable_corrections: True
+ sbas_apply_integrity: False
+
+ # (GQ7 only) SBAS included PRNs
+ # Note: ROS2 can't handle empty arrays in a yml file, so uncomment this line and fill it in if you want to specify PRNs
+ #sbas_included_prns: []
+
+ # (GQ7 only) NMEA message format. If set to false, all NMEA message configuration will not have any affect
+ nmea_message_config: False
+
+ # Allow NMEA messages with the same talker IDs on different data sources (descriptor sets)
+ # In most cases, this should be set to False, as multiple messages of the same type with the same talker ID from a different descriptor set could cause confusion when parsing.
+ nmea_message_allow_duplicate_talker_ids: False
+
+ # NMEA messages in the sensor (IMU) descriptor set
+ # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
+ # Note: In order to publish, 'publish_nmea' must also be set to True
+ imu_nmea_prkr_data_rate: 0
+
+ # NMEA messages in the GNSS1 descriptor set
+ # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
+ #
+ # Note: In order to publish, 'publish_nmea' must also be set to True
+ #
+ # Note: gnss1_nmea_talker_id can be any of the follwing numeric values:
+ # 1 - Sentences will start with GN
+ # 2 - Sentences will start with GP
+ # 3 - Sentences will start with GA
+ # 4 - Sentences will start with GL
+ # The purpose of the talker ID is to differentiate when the same message_id comes from different data sources (descriptor sets)
+ # The gnss1_nmea_talker_id will be applied to all NMEA messages from the GNSS1 descriptor set
+ gnss1_nmea_talker_id: 1
+ gnss1_nmea_gga_data_rate: 0
+ gnss1_nmea_gll_data_rate: 0
+ gnss1_nmea_gsv_data_rate: 0 # Note that this message_id will not use the gnss1_talker_id since the talker ID will come from the actual constellation the message originates from
+ gnss1_nmea_rmc_data_rate: 0
+ gnss1_nmea_vtg_data_rate: 0
+ gnss1_nmea_hdt_data_rate: 0
+ gnss1_nmea_zda_data_rate: 0
+
+ # NMEA messages in the GNSS2 descriptor set
+ # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
+ #
+ # Note: In order to publish, 'publish_nmea' must also be set to True
+ #
+ # Note: gnss2_nmea_talker_id can be any of the follwing numeric values:
+ # 1 - Sentences will start with GN
+ # 2 - Sentences will start with GP
+ # 3 - Sentences will start with GA
+ # 4 - Sentences will start with GL
+ # The purpose of the talker ID is to differentiate when the same message_id comes from different data sources (descriptor sets)
+ # The gnss2_nmea_talker_id will be applied to all NMEA messages from the GNSS2 descriptor set
+ gnss2_nmea_talker_id: 2
+ gnss2_nmea_gga_data_rate: 0
+ gnss2_nmea_gll_data_rate: 0
+ gnss2_nmea_gsv_data_rate: 0 # Note that this message_id will not use the gnss1_talker_id since the talker ID will come from the actual constellation the message originates from
+ gnss2_nmea_rmc_data_rate: 0
+ gnss2_nmea_vtg_data_rate: 0
+ gnss2_nmea_hdt_data_rate: 0
+ gnss2_nmea_zda_data_rate: 0
+
+ # NMEA messages in the filter descriptor set
+ # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz
+ #
+ # Note: In order to publish, 'publish_nmea' must also be set to True
+ #
+ # Note: filter_nmea_talker_id can be any of the follwing numeric values:
+ # 1 - Sentences will start with GN
+ # 2 - Sentences will start with GP
+ # 3 - Sentences will start with GA
+ # 4 - Sentences will start with GL
+ # The purpose of the talker ID is to differentiate when the same message_id comes from different data sources (descriptor sets)
+ # The filter_nmea_talker_id will be applied to all NMEA messages from the filter descriptor set
+ filter_nmea_talker_id: 3
+ filter_nmea_gga_data_rate: 0
+ filter_nmea_gll_data_rate: 0
+ filter_nmea_rmc_data_rate: 0
+ filter_nmea_hdt_data_rate: 0
+ filter_nmea_prka_data_rate: 0 # Note that this message_id will not have any talker ID on it since it is proprietary and can only come from the filter descriptor set
diff --git a/clearpath_sensors/config/velodyne_lidar.yaml b/clearpath_sensors/config/velodyne_lidar.yaml
index 702125d..13b2a3d 100644
--- a/clearpath_sensors/config/velodyne_lidar.yaml
+++ b/clearpath_sensors/config/velodyne_lidar.yaml
@@ -1,4 +1,4 @@
-velodyne_lidar_driver:
+velodyne_driver_node:
ros__parameters:
device_ip: 192.168.131.20
gps_time: false
@@ -11,14 +11,14 @@ velodyne_lidar_driver:
model: VLP16
rpm: 600.0
port: 2368
-velodyne_lidar_pointcloud:
+velodyne_convert_node:
ros__parameters:
calibration: /opt/ros/humble/share/velodyne_pointcloud/params/VLP16db.yaml
min_range: 0.9
max_range: 130.0
view_direction: 0.0
organize_cloud: true
-velodyne_lidar_laserscan:
+velodyne_laserscan_node:
ros__parameters:
ring: -1
resolution: 0.007
\ No newline at end of file
diff --git a/clearpath_sensors/launch/microstrain_imu.launch.py b/clearpath_sensors/launch/microstrain_imu.launch.py
new file mode 100644
index 0000000..dba91dc
--- /dev/null
+++ b/clearpath_sensors/launch/microstrain_imu.launch.py
@@ -0,0 +1,43 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+ pkg_microstrain_inertial_driver = FindPackageShare('microstrain_inertial_driver')
+
+ parameters = LaunchConfiguration('parameters')
+ namespace = LaunchConfiguration('namespace')
+
+ launch_microstrain_imu = PathJoinSubstitution([
+ pkg_microstrain_inertial_driver, 'launch', 'microstrain_launch.py'])
+
+ arg_namespace = DeclareLaunchArgument(
+ 'namespace',
+ default_value='platform/sensors/imu_0')
+
+ arg_parameters = DeclareLaunchArgument(
+ 'parameters',
+ default_value=PathJoinSubstitution([
+ FindPackageShare('clearpath_sensors'),
+ 'config',
+ 'microstrain_imu.yaml'
+ ]))
+
+ launch_microstrain_imu = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([launch_microstrain_imu]),
+ launch_arguments=[
+ ('namespace', namespace),
+ ('params_file', parameters)
+ ]
+ )
+
+ ld = LaunchDescription()
+ ld.add_action(arg_namespace)
+ ld.add_action(arg_parameters)
+ ld.add_action(launch_microstrain_imu)
+ return ld
diff --git a/clearpath_sensors/package.xml b/clearpath_sensors/package.xml
index 78a6066..e18deb5 100644
--- a/clearpath_sensors/package.xml
+++ b/clearpath_sensors/package.xml
@@ -9,8 +9,12 @@
ament_cmake
- urg_node
+ microstrain_inertial_driver
realsense2_camera
+ urg_node
+ velodyne_driver
+ velodyne_pointcloud
+ velodyne_laserscan
ament_lint_auto
ament_lint_common