LORD-MicroStrain / microstrain_inertial

ROS driver for all of MicroStrain's current G and C series products. To learn more visit
https://www.microstrain.com/inertial
100 stars 75 forks source link

3DM-GQ7 ROS Driver Data Recieved in Bursts #58

Closed JTomasi closed 3 years ago

JTomasi commented 3 years ago

Hello,

I am inquiring about how the data that the 3DM-GQ7 generates is processed by the ROS driver. It seems that the data is processed/received in ROS in bursts. I wanted to confirm if this is intended behaviour or if there is perhaps something incorrect with my configuration. My exact issue is outlined below:

I have collected a large rosbag containing several topics including:

Both of these topics were set to a data rate of 100 Hz. I am using the rosbag for testing a simple node that is running at 100Hz and noticed that the messages on the /gq7/nav/odom/ topic are quite "bursty". To verify this, I wrote a very simple node that simply loops at 100Hz and prints to the terminal during each loop. The node also has two callback functions for each topic. During each callback, I simply print the message header seq to the terminal. The output of this node is below:

image

As you can see, the IMU data is read in at the expected 100Hz, but the filtered nav data seems to be processed by the ROS driver in bursts, i.e., there is no new nav message during some node loops, while there are a large number during others.

As an additional check, I plotted the difference in time between the /gq7/nav/odom header timestamps that were collected at 100 Hz. The nominal difference should be around 0.01s:

nav_message_stamp_time

Seen here, the average time difference between messages is around 0.01s but there is quite a bit of variation, which may be causing the above timing issues.

Please let me know if you have any insights or can confirm if the messages that the ROS driver sends are intended to be sent in bursts. Thanks for your help.

nathanmillermicrostrain commented 3 years ago

Hello,

A couple of things are probably going on here:

1) It is likely that you are using the "collected timestamp" and not the "device-generated timestamp." You can switch to the device timestamp using the following launch file setting:

<param name="use_device_timestamp" value="true" type="bool" />

One issue with doing this though, is the device time is referenced to GPS time, so any syncing you need to do with other systems would be affected, assuming they need local PC time.

2) Are you using USB? If you are, the host computer is in charge of polling the data from the USB device (GQ7.) This typically causes "bursty" data and is affected by processor load. There isn't much we can do about this one as the host is in control. You do get all the data and bandwidth isn't much of a concern, but the data can come on chunks.

3) Are you using the UART? If so, you might be overrunning the available bandwidth. I would recommend checking the requested data rates / data quantities against the baudrate. This is the better option for control systems that need data to arrive with minimum latency (and bursting), but bandwidth can become a concern.

Let me know if you need any additional info.

Nathan

JTomasi commented 3 years ago

Hey Nathan,

Thanks again for the detailed reponse.

I will try using the "device-generated timestamp" setting for future data collection runs. In regards to your other points, I am using UART with the maximum allowable baudrate of 921600 so I don't believe bandwidth is a concern.

Another plot that I can share is the plot of the time difference between sequential ROSbag message timestamps (this is different from the header timestamp that I shared in the message above). This plot shows the difference in time between sequential /gq7/nav/odom/ messages that the rosbag recorded from the driver. To me, this suggests that the driver bunches the data together before outputting it. Can you provide any insight here?

nav_message_stamp_time (1)

nathanmillermicrostrain commented 3 years ago

Could you provide your launch file so I can try to recreate this issue?

Thank you

JTomasi commented 3 years ago

Sure, please see below. Note that the baudrate is set to 115200 as per #47

<?xml version="1.1"?>
<launch>

  <!-- Standalone example launch 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 -->

  <!-- Declare arguments with default values -->
  <arg name="name"        default = "gq7" />
  <arg name="port"        default = "/dev/ttyS0" />
  <arg name="baudrate"    default = "115200" />
  <arg name="debug"       default = "false" />
  <arg name="diagnostics" default = "true" />

  <!-- ****************************************************************** -->
  <!-- Microstrain sensor node -->
  <!-- ****************************************************************** -->

  <node name="ros_mscl_node" pkg="ros_mscl" type="ros_mscl_node" output="screen" ns="$(arg name)">

    <!-- ****************************************************************** -->
    <!-- General Settings -->
    <!-- ****************************************************************** -->

    <param name="port"     value="$(arg port)"     type="str" />
    <param name="baudrate" value="$(arg baudrate)" type="int" />

    <!-- 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
     -->

    <param name="device_setup" value="true" type="bool" />

    <!-- 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
     -->

    <param name="save_settings" value="false" type="bool" />

    <!-- 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
     -->

    <param name="raw_file_enable" value="true" type="bool" />

    <!-- (GQ7 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

          Notes: Including this feature increases communication bandwidth requirements... for serial data connections,
                 please ensure the baudrate is sufficient for the added data channels.
     -->

    <param name="raw_file_include_support_data" value="true" type="bool" />

    <!-- The directory to store the raw data file (no trailing '/')-->

    <param name="raw_file_directory" value="~/ws_test/" type="str" />

    <!-- ****************************************************************** -->
    <!-- IMU Settings -->
    <!-- ****************************************************************** -->

    <param name="publish_imu" value="true" type="bool" />

    <param name="imu_data_rate" value="100"      type="int" />
    <param name="imu_frame_id"  value="gq7_link" type="str" />

    <!-- Static IMU message covariance values (the device does not generate these) -->
    <!-- Since internally these are std::vector we need to use the rosparam tags -->
    <rosparam param="imu_orientation_cov"> [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>
    <rosparam param="imu_linear_cov">      [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>
    <rosparam param="imu_angular_cov">     [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>

    <!-- If enabled, this message can be used to validate time IMU time syncronzation with gps -->
    <!-- It is most useful when using an external timesource and external PPS-->
    <!-- (see: filter_enable_external_gps_time_update) -->
    <param name="publish_gps_corr"  value="false" type="bool" />

    <!-- ****************************************************************** -->
    <!-- GNSS Settings (only applicable for devices with GNSS) -->
    <!-- ****************************************************************** -->

    <param name="publish_gnss1"   value="true" type="bool" />
    <param name="gnss1_data_rate" value="2"    type="int" />
    <param name="gnss1_frame_id"  value="gnss1_antenna_wgs84" type="str" />

    <!-- 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 -->
     -->
    <rosparam param="gnss1_antenna_offset"> [0.1479, -0.01561, 0.2088]</rosparam>

    <!-- GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) -->
    <param name="publish_gnss2"   value="true" type="bool" />
    <param name="gnss2_data_rate" value="2"    type="int" />
    <param name="gnss2_frame_id"  value="gnss2_antenna_wgs84" type="str" />

    <!-- Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )-->
    <!--Note: Make this as accurate as possible for good performance -->

    <rosparam param="gnss2_antenna_offset"> [1.2859, -0.01561, 0.2088]</rosparam>

    <!-- (GQ7 Only) Enable RTK dongle interface -->
    <param name="rtk_dongle_enable" value="true"    type="bool" />

    <!-- ****************************************************************** -->
    <!-- RTK Settings (only applicable for devices with RTK) -->
    <!-- ****************************************************************** -->

    <param name="publish_rtk"   value="true" type="bool" />
    <param name="rtk_data_rate" value="1"    type="int" />

    <!-- ****************************************************************** -->
    <!-- Kalman Filter Settings (only applicable for devices with a Kalman Filter) -->
    <!-- ****************************************************************** -->

    <param name="publish_filter" value="true" type="bool" />

    <param name="filter_data_rate"           value="100" type="int" />
    <param name="filter_odom_frame_id"       value="sensor_wgs84" type="str" />
    <param name="filter_odom_child_frame_id" value="sensor_ned" type="str" />

    <!-- 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.
                Quaternion order is [i, j, k, w]
     -->
    <param name="filter_sensor2vehicle_frame_selector" value="0" type="int" />

    <rosparam param="filter_sensor2vehicle_frame_transformation_euler">      [0.0, 0.0, 0.0]</rosparam>
    <rosparam param="filter_sensor2vehicle_frame_transformation_matrix">     [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
    <rosparam param="filter_sensor2vehicle_frame_transformation_quaternion"> [0.0, 0.0, 0.0, 1.0]</rosparam>

     <!-- Controls if the Kalman filter is reset after the settings are configured-->
    <param name="filter_reset_after_config" value="true" type="bool" />

     <!-- Controls if the Kalman filter will auto-init or requires manual initialization-->
    <param name="filter_auto_init" value="true" type="bool" />

    <!-- (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual -->
    <param name="filter_declination_source" value="2" type="int" />
    <param name="filter_declination"        value="0.23" type="double" />

    <!-- (All, except GQ7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)  -->
    <param name="filter_heading_source" value="2" type="int" />

    <!-- (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) -->
    <param name="filter_dynamics_mode" value="3" type="int" />>

    <!-- ZUPT control -->
    <param name="filter_velocity_zupt_topic" value="/moving_vel" type="str" />
    <param name="filter_angular_zupt_topic"  value="/moving_ang" type="str" />
    <param name="filter_velocity_zupt"       value="true" type="bool" />
    <param name="filter_angular_zupt"        value="true" type="bool" />

    <!-- (GQ7 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 
     -->

    <param name="filter_adaptive_level"         value="2" type="int" />
    <param name="filter_adaptive_time_limit_ms" value="15000" type="int" />

    <!-- (GQ7 only) Aiding measurement control -->
    <param name="filter_enable_gnss_pos_vel_aiding"     value="true"  type="bool" />
    <param name="filter_enable_gnss_heading_aiding"     value="true"  type="bool" />
    <param name="filter_enable_altimeter_aiding"        value="true" type="bool" />
    <param name="filter_enable_odometer_aiding"         value="false" type="bool" />
    <param name="filter_enable_magnetometer_aiding"     value="false" type="bool" />
    <param name="filter_enable_external_heading_aiding" value="false" type="bool" />

    <!--  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.
    -->
    <param name="filter_enable_external_gps_time_update"    value="false" type="bool" />
    <param name="filter_external_gps_time_topic"            value="/external_gps_time" type="str" />
    <param name="gps_leap_seconds"                          value="18" type="int" />

    <!--  (GQ7 only) GPIO Configuration
          Notes:    For information on possible configurations and specific pin options, 
                    refer to the MSCL MipNodeFeatures command, supportedGpioConfigurations.

          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 

          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
    -->
    <param name="gpio1_feature"     value="0" type="int" />
    <param name="gpio1_behavior"    value="0" type="int" />
    <param name="gpio1_pin_mode"    value="0" type="int" />

    <param name="gpio2_feature"     value="0" type="int" />
    <param name="gpio2_behavior"    value="0" type="int" />
    <param name="gpio2_pin_mode"    value="0" type="int" />

    <param name="gpio3_feature"     value="0" type="int" />
    <param name="gpio3_behavior"    value="0" type="int" />
    <param name="gpio3_pin_mode"    value="0" type="int" />

    <param name="gpio4_feature"     value="0" type="int" />
    <param name="gpio4_behavior"    value="0" type="int" />
    <param name="gpio4_pin_mode"    value="0" type="int" />

    <param name="gpio_config"       value="false" type="bool" />

    <!-- (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
     -->

    <param name="filter_init_condition_src"              value="0"     type="int" />
    <param name="filter_auto_heading_alignment_selector" value="3"     type="int" />
    <param name="filter_init_reference_frame"            value="2"     type="int" />
    <rosparam param="filter_init_position">   [0.0, 0.0, 0.0]</rosparam>    
    <rosparam param="filter_init_velocity">   [0.0, 0.0, 0.0]</rosparam>
    <rosparam param="filter_init_attitude">   [0.0, 0.0, 0.0]</rosparam>

    <!-- (GQ7 only) Relative Position Configuration
         Reference frame =
         1 - Relative ECEF position
         2 - Relative LLH position

         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)
     -->

    <param name="publish_relative_position"         value="true" type="bool" />
    <param name="filter_relative_position_frame"    value="2"  type="int" />
    <rosparam param="filter_relative_position_ref"> [0, 0, 0.01]</rosparam>    

    <!-- (GQ7 only) Speed Lever Arm Configuration
         Lever Arm - In vehicle reference frame (meters)
     -->

    <rosparam param="filter_speed_lever_arm"> [0.0, 0.0, 0.0]</rosparam>    

    <!-- (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
     -->

    <param name="filter_enable_wheeled_vehicle_constraint" value="false"  type="bool" />

    <!-- (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.
     -->

    <param name="filter_enable_vertical_gyro_constraint" value="false"  type="bool" />

    <!-- (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.
     -->

    <!-- (GQ7 only) PPS Source
          PPS Source =
          0 - Disabled
          1 - Reciever 1 (default)
          2 - Reciever 2
          3 - Generated from system oscillator
          4 - GPIO 1 (provided by external source if supported)
          5 - GPIO 2 (provided by external source if supported)
          6 - GPIO 3 (provided by external source if supported)
          7 - GPIO 4 (provided by external source if supported)
     -->
    <param name="filter_pps_source" value="1" type="int" />

    <param name="filter_enable_gnss_antenna_cal"     value="false" type="bool" />
    <param name="filter_gnss_antenna_cal_max_offset" value="0.1"   type="double" />
  </node>

  <!-- ****************************************************************** -->
  <!-- Diagnostics -->
  <!-- ****************************************************************** -->

  <group if="$(arg diagnostics)">

    <!-- Diagnostic Aggregator for robot monitor usage -->
    <node pkg="diagnostic_aggregator" type="aggregator_node" name="imu_diagnostic_aggregator">
      <rosparam command="load" file="$(find ros_mscl)/config/diagnostic_analyzers.yaml" />
    </node>
  </group>

  <!-- ****************************************************************** -->
  <!-- Set the debug level to debug -->
  <!-- ****************************************************************** -->

  <group if="$(arg debug)">
    <env name="ROSCONSOLE_CONFIG_FILE"
       value="$(find ros_mscl)/config/custom_rosconsole.conf"/>
  </group>
</launch>
nathanmillermicrostrain commented 3 years ago

Ok... I thought that the following setting might be the case:

<param name="raw_file_include_support_data" value=**"true"** type="bool" /> So, this has a couple of effects on the datastream that need some discussion:

1) More data is added to the filter (nav) data packets and the underlaying MIP binary protocol has a size limitation of 261 bytes per packet. The additional data elements cause the filter data to be split across 2 MIP packets. That means, the driver will process 2 MIP packets per 10 mS interval. The effect is you will get back-to-back sequence numbers (and minimal dt in both plots that you show above.) This doesn't totally explain what you are seeing above, but part of it.

2) There is a known bug in the GQ7 firmware that requires: the device to be connected via UART, a lot of satellites in view, and the support data enabled. In this configuration, you may see dropped/missing/delayed data due to the GQ7 UART output buffer size being insufficient for the amount of data each GNSS sends. There is a fix for this issue and we are currently testing it with a release target date of 6/1/21. This would explain the band of packets above 0.04s dt in the rosbag image above.

I would suggest not including the support data unless you need to to get simulation/data analysis support from us..... especially when doing real-time control with the device. The reason is, there is a significant amount of additional data (all of the raw satellite ranging and ephemeris info) that comes over when enabled. This comes out all at once at 2 hz for each of the GNSS modules (plus the RTK, if receiving corrections)... and it is enough data to push around the reception time of the other data (IMU and NAV.)

I also noticed that in your first image "Node Timer Callback" seems to be called at 100 Hz. The spin rate of the node should be 200 Hz. This is something I'll need to check on tomorrow (I'm away from the office today.)

My guess is that if you disable "raw_file_include_support_data", you will see your timing issues mostly clear up, if not clear up completely. I'll take a deeper look at it tomorrow as I will be finishing up the addition to the driver to support your other request wrt RTK status.

Let me know if that helps,

Nathan

JTomasi commented 3 years ago

Hi Nathan,

Apologies but I doubled checked and I had the incorrect version of the launch file. The setting you are referring to was actually set to false when I collected this data. The above information is very useful however.

I have posted the correct launch file below. One issue that I discovered was that I had incorrectly set the following parameter: <param name="filter_auto_heading_alignment_selector" value="0" type="int" />

I missed the comment indicating that this was a bitfield and not an enum - I have since update this but perhaps this also contributed. Please let me know if there is anything else that you notice.

<?xml version="1.1"?>
<launch>

  <!-- Standalone example launch 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 -->

  <!-- Declare arguments with default values -->
  <arg name="name"        default = "gq7" />
  <arg name="port"        default = "/dev/ttyS0" />
  <arg name="baudrate"    default = "115200" />
  <arg name="debug"       default = "false" />
  <arg name="diagnostics" default = "true" />

  <!-- ****************************************************************** -->
  <!-- Microstrain sensor node -->
  <!-- ****************************************************************** -->

  <node name="ros_mscl_node" pkg="ros_mscl" type="ros_mscl_node" output="screen" ns="$(arg name)">

    <!-- ****************************************************************** -->
    <!-- General Settings -->
    <!-- ****************************************************************** -->

    <param name="port"     value="$(arg port)"     type="str" />
    <param name="baudrate" value="$(arg baudrate)" type="int" />

    <!-- 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
     -->

    <param name="device_setup" value="true" type="bool" />

    <!-- 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
     -->

    <param name="save_settings" value="false" type="bool" />

    <!-- 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
     -->

    <param name="raw_file_enable" value="false" type="bool" />

    <!-- (GQ7 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

          Notes: Including this feature increases communication bandwidth requirements... for serial data connections,
                 please ensure the baudrate is sufficient for the added data channels.
     -->

    <param name="raw_file_include_support_data" value="false" type="bool" />

    <!-- The directory to store the raw data file (no trailing '/')-->

    <param name="raw_file_directory" value="/home/your_name" type="str" />

    <!-- ****************************************************************** -->
    <!-- IMU Settings -->
    <!-- ****************************************************************** -->

    <param name="publish_imu" value="true" type="bool" />

    <param name="imu_data_rate" value="100"      type="int" />
    <param name="imu_frame_id"  value="gq7_link" type="str" />

    <!-- Static IMU message covariance values (the device does not generate these) -->
    <!-- Since internally these are std::vector we need to use the rosparam tags -->
    <rosparam param="imu_orientation_cov"> [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>
    <rosparam param="imu_linear_cov">      [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>
    <rosparam param="imu_angular_cov">     [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]</rosparam>

    <!-- If enabled, this message can be used to validate time IMU time syncronzation with gps -->
    <!-- It is most useful when using an external timesource and external PPS-->
    <!-- (see: filter_enable_external_gps_time_update) -->
    <param name="publish_gps_corr"  value="false" type="bool" />

    <!-- ****************************************************************** -->
    <!-- GNSS Settings (only applicable for devices with GNSS) -->
    <!-- ****************************************************************** -->

    <param name="publish_gnss1"   value="true" type="bool" />
    <param name="gnss1_data_rate" value="2"    type="int" />
    <param name="gnss1_frame_id"  value="gnss1_antenna_wgs84" type="str" />

    <!-- 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 -->
     -->
    <rosparam param="gnss1_antenna_offset"> [0.1479, -0.01561, 0.2088]</rosparam>

    <!-- GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) -->
    <param name="publish_gnss2"   value="true" type="bool" />
    <param name="gnss2_data_rate" value="2"    type="int" />
    <param name="gnss2_frame_id"  value="gnss2_antenna_wgs84" type="str" />

    <!-- Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )-->
    <!--Note: Make this as accurate as possible for good performance -->

    <rosparam param="gnss2_antenna_offset"> [1.2859, -0.01561, 0.2088]</rosparam>

    <!-- (GQ7 Only) Enable RTK dongle interface -->
    <param name="rtk_dongle_enable" value="true"    type="bool" />

    <!-- ****************************************************************** -->
    <!-- RTK Settings (only applicable for devices with RTK) -->
    <!-- ****************************************************************** -->

    <param name="publish_rtk"   value="true" type="bool" />
    <param name="rtk_data_rate" value="1"    type="int" />

    <!-- ****************************************************************** -->
    <!-- Kalman Filter Settings (only applicable for devices with a Kalman Filter) -->
    <!-- ****************************************************************** -->

    <param name="publish_filter" value="true" type="bool" />

    <param name="filter_data_rate"           value="100" type="int" />
    <param name="filter_odom_frame_id"       value="sensor_wgs84" type="str" />
    <param name="filter_odom_child_frame_id" value="sensor_ned" type="str" />

    <!-- 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.
                Quaternion order is [i, j, k, w]
     -->
    <param name="filter_sensor2vehicle_frame_selector" value="0" type="int" />

    <rosparam param="filter_sensor2vehicle_frame_transformation_euler">      [0.0, 0.0, 0.0]</rosparam>
    <rosparam param="filter_sensor2vehicle_frame_transformation_matrix">     [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
    <rosparam param="filter_sensor2vehicle_frame_transformation_quaternion"> [0.0, 0.0, 0.0, 1.0]</rosparam>

     <!-- Controls if the Kalman filter is reset after the settings are configured-->
    <param name="filter_reset_after_config" value="true" type="bool" />

     <!-- Controls if the Kalman filter will auto-init or requires manual initialization-->
    <param name="filter_auto_init" value="true" type="bool" />

    <!-- (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual -->
    <param name="filter_declination_source" value="2" type="int" />
    <param name="filter_declination"        value="0.23" type="double" />

    <!-- (All, except GQ7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)  -->
    <param name="filter_heading_source" value="2" type="int" />

    <!-- (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) -->
    <param name="filter_dynamics_mode" value="3" type="int" />>

    <!-- ZUPT control -->
    <param name="filter_velocity_zupt_topic" value="/moving_vel" type="str" />
    <param name="filter_angular_zupt_topic"  value="/moving_ang" type="str" />
    <param name="filter_velocity_zupt"       value="true" type="bool" />
    <param name="filter_angular_zupt"        value="true" type="bool" />

    <!-- (GQ7 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 
     -->

    <param name="filter_adaptive_level"         value="2" type="int" />
    <param name="filter_adaptive_time_limit_ms" value="15000" type="int" />

    <!-- (GQ7 only) Aiding measurement control -->
    <param name="filter_enable_gnss_pos_vel_aiding"     value="true"  type="bool" />
    <param name="filter_enable_gnss_heading_aiding"     value="true"  type="bool" />
    <param name="filter_enable_altimeter_aiding"        value="false" type="bool" />
    <param name="filter_enable_odometer_aiding"         value="false" type="bool" />
    <param name="filter_enable_magnetometer_aiding"     value="false" type="bool" />
    <param name="filter_enable_external_heading_aiding" value="false" type="bool" />

    <!--  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.
    -->
    <param name="filter_enable_external_gps_time_update"    value="false" type="bool" />
    <param name="filter_external_gps_time_topic"            value="/external_gps_time" type="str" />
    <param name="gps_leap_seconds"                          value="18" type="int" />

    <!--  (GQ7 only) GPIO Configuration
          Notes:    For information on possible configurations and specific pin options, 
                    refer to the MSCL MipNodeFeatures command, supportedGpioConfigurations.

          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 

          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
    -->
    <param name="gpio1_feature"     value="0" type="int" />
    <param name="gpio1_behavior"    value="0" type="int" />
    <param name="gpio1_pin_mode"    value="0" type="int" />

    <param name="gpio2_feature"     value="0" type="int" />
    <param name="gpio2_behavior"    value="0" type="int" />
    <param name="gpio2_pin_mode"    value="0" type="int" />

    <param name="gpio3_feature"     value="0" type="int" />
    <param name="gpio3_behavior"    value="0" type="int" />
    <param name="gpio3_pin_mode"    value="0" type="int" />

    <param name="gpio4_feature"     value="0" type="int" />
    <param name="gpio4_behavior"    value="0" type="int" />
    <param name="gpio4_pin_mode"    value="0" type="int" />

    <param name="gpio_config"       value="false" type="bool" />

    <!-- (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
     -->

    <param name="filter_init_condition_src"              value="0"     type="int" />
    <param name="filter_auto_heading_alignment_selector" value="0"     type="int" />
    <param name="filter_init_reference_frame"            value="2"     type="int" />
    <rosparam param="filter_init_position">   [0.0, 0.0, 0.0]</rosparam>    
    <rosparam param="filter_init_velocity">   [0.0, 0.0, 0.0]</rosparam>
    <rosparam param="filter_init_attitude">   [0.0, 0.0, 0.0]</rosparam>

    <!-- (GQ7 only) Relative Position Configuration
         Reference frame =
         1 - Relative ECEF position
         2 - Relative LLH position

         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)
     -->

    <param name="publish_relative_position"         value="false" type="bool" />
    <param name="filter_relative_position_frame"    value="2"  type="int" />
    <rosparam param="filter_relative_position_ref"> [0, 0, 0.01]</rosparam>    

    <!-- (GQ7 only) Speed Lever Arm Configuration
         Lever Arm - In vehicle reference frame (meters)
     -->

    <rosparam param="filter_speed_lever_arm"> [0.0, 0.0, 0.0]</rosparam>    

    <!-- (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
     -->

    <param name="filter_enable_wheeled_vehicle_constraint" value="false"  type="bool" />

    <!-- (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.
     -->

    <param name="filter_enable_vertical_gyro_constraint" value="false"  type="bool" />

    <!-- (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.
     -->

    <!-- (GQ7 only) PPS Source
          PPS Source =
          0 - Disabled
          1 - Reciever 1 (default)
          2 - Reciever 2
          3 - Generated from system oscillator
          4 - GPIO 1 (provided by external source if supported)
          5 - GPIO 2 (provided by external source if supported)
          6 - GPIO 3 (provided by external source if supported)
          7 - GPIO 4 (provided by external source if supported)
     -->
    <param name="filter_pps_source" value="1" type="int" />

    <param name="filter_enable_gnss_antenna_cal"     value="false" type="bool" />
    <param name="filter_gnss_antenna_cal_max_offset" value="0.1"   type="double" />
  </node>

  <!-- ****************************************************************** -->
  <!-- Diagnostics -->
  <!-- ****************************************************************** -->

  <group if="$(arg diagnostics)">

    <!-- Diagnostic Aggregator for robot monitor usage -->
    <node pkg="diagnostic_aggregator" type="aggregator_node" name="imu_diagnostic_aggregator">
      <rosparam command="load" file="$(find ros_mscl)/config/diagnostic_analyzers.yaml" />
    </node>
  </group>

  <!-- ****************************************************************** -->
  <!-- Set the debug level to debug -->
  <!-- ****************************************************************** -->

  <group if="$(arg debug)">
    <env name="ROSCONSOLE_CONFIG_FILE"
       value="$(find ros_mscl)/config/custom_rosconsole.conf"/>
  </group>
</launch>
nathanmillermicrostrain commented 3 years ago

Ok... well, let me take a look at this issue when I'm in the office tomorrow. The filter_auto_heading_alignment_selector shouldn't affect this issue, but yes, that should be a "1" if you are wanting to initialize with dual antenna.

nathanmillermicrostrain commented 3 years ago

Ok, so I did a somewhat similar test with the exception that my nav data is at 10 hz (my baudrate is limited to 115200 in the setup that I'm using.) I get what I expect would be decent results (shown below.) The time that I am plotting is the arrival time of the packet. The computer (actually a virtual machine) isn't doing any other calculations. In your test was there something that could have been interrupting the processing of the GQ7 data? Note: there are 2 nav packets arriving during each 0.1 sec interval.

image

JTomasi commented 3 years ago

Hi Nathan,

I performed an additional test and am still getting the same result. Here, I collected both the IMU and nav/odom data at 200 Hz. I am running my node loop at 100Hz to emphasize the bursts in the nav messages. Note that the dt value is calculated based on the message header timestamp, which seems to be correct for both the IMU data and the nav/odom data. The same issue with how the nav data is passed through the driver can been - the nav data seems to be read in bursts.

As an aside, I am currently using the vrsion of the ROS MSCL driver found here. Is there any chance that this could be the cause of the issues? I plan on updating to the newest driver once the version with RTK that you are working on is completed.

image

nathanmillermicrostrain commented 3 years ago

The newest version of the driver was committed last night and supports the RTK info that you need (commented on the other support thread.) Sorry, forgot to push the commit last night!

I don't believe anything changed in parsing of the MIP packets between versions. Our devices interleave the IMU/NAV/GNSS MIP packets and the MSCL library just queues them up after verifying the checksum. The driver picks up these queued results and distributes them as you've seen in the driver code you've edited.

Can you share the code that you edited to generate the "Filtered nav message ####, ...." info message?
Also, your launch file says your running at 115200, but given your setup (if I remember correctly), you are really running at 921600, right?

JTomasi commented 3 years ago

Below is the beginning of the nav message callback function. I simply take the message, a subtract the timestamp from the previous timestamp.

You are correct, we are running at 921600 but our particular setup requires the config to be set to 115200.

void StateEstimationNodelet::navFilteredCB(
    const nav_msgs::OdometryConstPtr &msg){
  ROS_INFO_STREAM("\tFiltered nav message " << msg->header.seq << ", dt: " << (msg->header.stamp-temp_prev_time_nav).toSec());
  temp_prev_time_nav_ = msg->header.stamp;
nathanmillermicrostrain commented 3 years ago

Is it possible this is a consumer-side issue? I thought you were printing the info from our driver, not a different node listening to the topic. Have you tried checking our driver side, something like the following:

Starting at line 1722 in the latest driver:

//Handle time uint64_t time = packet.collectedTimestamp().nanoseconds();

//Check if the user wants to use the device timestamp instead of PC collected time

if(packet.hasDeviceTime() && m_use_device_timestamp)
{
time = packet.deviceTimestamp().nanoseconds();
}

//TEST CODE START static uint64_t prev_time = 0;

ROS_INFO("NAV Packet id = %d, dt = %f", m_filter_valid_packet_count, ((double)time-(double)prevt_time)/1.0e9); prev_time = time;

//TEST CODE END

//Filtered IMU timestamp and frame m_filtered_imu_msg.header.seq = m_filter_valid_packet_count; m_filtered_imu_msg.header.stamp = ros::Time().fromNSec(time);
m_filtered_imu_msg.header.frame_id = m_filter_frame_id;

JTomasi commented 3 years ago

Closing this issue as the recent driver updates have rectified this issue.

edwardribbit commented 3 years ago

Adding to the knowledge base here, this issue was related to ROS1 tcp message transport. Adding transport hint tcpNoDelay when initialising a subscriber causes the subscriber callback to be called at a constant frequency.

Aussie25 commented 3 years ago

Hello,

I am inquiring about how the data that the 3DM-GQ7 generates is processed by the ROS driver. It seems that the data is processed/received in ROS in bursts. I wanted to confirm if this is intended behaviour or if there is perhaps something incorrect with my configuration. My exact issue is outlined below:

I have collected a large rosbag containing several topics including:

* /gq7/imu/data

* /gq7/nav/odom

Both of these topics were set to a data rate of 100 Hz. I am using the rosbag for testing a simple node that is running at 100Hz and noticed that the messages on the /gq7/nav/odom/ topic are quite "bursty". To verify this, I wrote a very simple node that simply loops at 100Hz and prints to the terminal during each loop. The node also has two callback functions for each topic. During each callback, I simply print the message header seq to the terminal. The output of this node is below:

image

As you can see, the IMU data is read in at the expected 100Hz, but the filtered nav data seems to be processed by the ROS driver in bursts, i.e., there is no new nav message during some node loops, while there are a large number during others.

As an additional check, I plotted the difference in time between the /gq7/nav/odom header timestamps that were collected at 100 Hz. The nominal difference should be around 0.01s:

nav_message_stamp_time

Seen here, the average time difference between messages is around 0.01s but there is quite a bit of variation, which may be causing the above timing issues.

Please let me know if you have any insights or can confirm if the messages that the ROS driver sends are intended to be sent in bursts. Thanks for your help.

Hi @JTomasi , Were you able to resolve the burst issue and did you try higher imu data frequency. And may I know how to plot the difference in time between the message header timestamps. Thanks