Only released in EOL distros:  

Package Summary

The microstrain_3dm_gx5_45 package provides a driver for the LORD/Microstrain 3DM_GXx_45 GPS-aided IMU sensor.

Package Summary

The microstrain_3dm_gx5_45 package provides a driver for the LORD/Microstrain 3DM_GXx_45 GPS-aided IMU sensor.

Overview

Interface (driver) software, including ROS node, for Microstrain 3DM-GX5-45.

The interface makes use of the MIP SDK Version 1.1 from Microstrain to communicate with the device. Includes the following applications:

Supported Devices

  • LORD/Microstrain 3DM-GX5-45: This driver has been mainly tested using the LORD/Microstrain 3DM-GX5-45 sensor. We have done limited testing on the 3DM-GX4-45, and the interface seems to be the same. We have not tested this with the 3DM-GX3-45, but the interface (MIP) should be the same. Please let us know if you find issues.

  • Limited testing has been done using the 3DM-GX4-25 (AHRS-only) device. See the microstrain_25.launch file for an example of how to configure the driver for use with the -25.

ROS Nodes

microstrain_3dm_gx5_45

ROS node reports the GPS data, AHRS data and Nav Filter estimate

Published Topics

gps/fix (sensor_msgs/NavSatFix)
  • See [[#navsatmsg]Nav Sat Fix message description] below.
imu/data (sensor_msgs/Imu)
  • IMU (AHRS) data is provided
nav/odom (nav_msgs/Odometry) nav/status (std_msgs/Int16MultiArray)

Services

reset_kf (std_srvs/Empty)
  • Resets the Kalman filter on the device

Parameters

port (string, default: /dev/ttyACM0)
  • Serial port - Linux only
baud_rate (int, default: 115200)
  • Baud rate of serial connection
device_setup (bool, default: true)
  • If true, puts device in idle mode and configures the device.If false, skips configuration. Important: the configuration parameters below are not effective unless this flag is true.
readback_settings (bool, default: true)
  • coming soon
auto-init (bool, default: true)
  • tbd
dynamics_mode (int, default: 1)
  • Vehicle dynamics mode 0x01=Portable, 0x02=Automotive, 0x03=Airborne
declination_source (int, default: 2)
  • Possible declination sources: 0x01=Node, device reports magnetic north, 0x02=Internal World Magnetic Model, 0x03=Manual (see declination parameter)
gps_frame_id (string, default: wgs84)
  • Value for the frame_id field in the header of the NavSatFix message published on the gps/fix topic
imu_frame_id (string, default: base_link)
  • Value of the frame_id field in the header of the Imu message publised in the imu/data topic
odom_frame_id (string, default: wgs84)
  • Value of the frame_id field in the header of the Odometry message published on the nav/odom topic
odom_child_frame_id (string, default: base_link)
  • Value of the child_frame_id field in the Odometry message published on the nav/odom topic.
publish_gps (bool, default: true)
  • Sets if ~gps/fix should be advertised/published or not. Note - to maximize performance you may want to only publish the Odometry messages
publish_imu (bool, default: true)
  • Sets if ~imu/data should be advertised/published or not.
publish_gps (bool, default: true)
  • Sets if ~nav/odom should be advertised/published or not.
gps_rate (int, default: 1)
  • Target update (publishing) rate for gps/fix messages. See update [[#rates] Update Rates] below.
imu_rate (int, default: 10)
  • Target update (publishing) rate for imu/data messages. See update [[#rates] Update Rates] below.
odom_rate (int, default: 10)
  • Target update (publishing) rate for nav/odom messages. See update [[#rates] Update Rates] below.

Update Rates

The rates are set as a target value in Hz. The device accepts a decimation value for each output; the packet rate is base_rate/decimation, where decimation is an integer. The program calculates the decimation to get close the the desired rate, based on polling the sensor for its base rate.

For the 3DM-GX4-45 and 3DM-GX5-45 devices tested the base rates were...

  • GPS - base rate = 4 Hz
  • IMU - base rate = 500 Hz
  • Filter - base rate = 500 Hz

Odometry message information

  • Currently the pose.position is the longitude (x), latitude (y) and ellipsoid height (z)
  • pose.covariance and twist.covariance include diagonal elements for position and attitude

  • Includes three values - see communication protocol for full documentation.
    • filter_state
      • 0x00 – Startup
      • 0x01 – Initialization (see status flags)
      • 0x02 – Running, Solution Valid
      • 0x03 – Running, Solution Error (see status flags)
    • dynamics mode
      • 0x01 – Portable (device default)
      • 0x02 – Automotive
      • 0x03 – Airborne
    • status_flags
      • See device documentation

Position covariance is populated with diagonals based on reported horizontal and vertical accuracy. The status.status field is the LLH position data "valid flag"-1. The valid flag mapping from the 3DM protocol is

  • 0x0001 – Latitude and Longitude Valid
  • 0x0002 – Ellipsoid Height Valid
  • 0x0004 – MSL Height Valid
  • 0x0008 – Horizontal Accuracy Valid
  • 0x0010 – Vertical Accuracy Valid
  • E.g., if all valid, then the status.status field should be 30.

Build Instructions

Building from source

DISTRO={hydro|indigo}
cd ~/catkin_ws
rosdep update
rosdep check --from-paths src/microstrain_3dm_gx5_45/ --rosdistro=$DISRO
rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO --simulate
rosdep install --from-paths src/microstrain_3dm_gx5_45/ --ignore-src --rosdistro=$DISTRO
catkin_make
source devel/setup.bash

Dev Notes

  • The mip_sdk_user_functions are C functions that need to be called by various parts of the SDK. The main purpose of these functions is to implement the platform-specific serial (RS232) port elements. The prototype serial port open function takes the COM number as an integer input - which is clunky for Linux serial ports. Changed this to take a string defining the port (e.g., /dev/ttyS0), but this necessitated also modifying the mip_sdk_interface.[ch] files, since this is what is called by the application - changed the mip_interface_init function to accept a string argument for specifying the port.

TODO

  • Verify order of quaternions

Launch File Examples

There are launch files in the /launch directory to illustrate use of the node.

Wiki: microstrain_3dm_gx5_45 (last edited 2017-04-14 02:01:12 by BrianBingham)