Implemented a sync out count feature in the IMU topics

CMakeLists.txt

@@ -7,16 +7,19 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   diagnostic_updater
+  trigger_msgs
 )
+
 find_package(Boost REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS include vncpplib/include
   LIBRARIES imu_vn_100
-  CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs
+  CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs 
   DEPENDS Boost
 )
 
+
 include_directories(
   include
   vncpplib/include

README.md

@@ -19,6 +19,8 @@ Note that the official SDK is modified within this package due to some bugs or u
 
 This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compiling a catkin package will work.
 
+You also need to download the [trigger_msgs](https://gitlab.com/neufieldrobotics/trigger_msgs) package to compile it. Clone them in the same workspace.
+
 ```
 cd your_work_space
 catkin_make --pkg imu_vn_100 --cmake-args -DCMAKE_BUILD_TYPE=Release

include/imu_vn_100/imu_vn_100.h

@@ -20,10 +20,12 @@
 #include <ros/ros.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
+#include <std_msgs/Header.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Temperature.h>
+#include <trigger_msgs/sync_trigger.h>
 
 #include "vn100.h"
 
@@ -54,6 +56,7 @@ struct DiagnosedPublisher {
     diag->tick(message.header.stamp);
     pub.publish(message);
   }
+
 };
 
 /**
@@ -122,14 +125,21 @@ class ImuVn100 {
   bool binary_output_ = true;
   int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2;
 
-  bool imu_compensated_ = false;
+  int imu_compensated_ = 0;
 
   bool tf_ned_to_enu_ = false;
 
   bool vpe_enable_ = true;
+  bool use_imu_clock = true; // condition to set variables if we are using IMU clock
+  ros::Time start_of_node_ros_time; // this is the offset time added to the IMU time since startup when 
+  bool is_first_data_point = true;  // this condition used to check whether we have first binary message
+  uint64_t nanoseconds_till_first_data_point = 0; // the variable stores the time since startup that we get when we get our first binary message
   int vpe_heading_mode_ = 1;
   int vpe_filtering_mode_ = 1;
   int vpe_tuning_mode_ = 1;
+  int syncOutCnt_old=0;
+  int syncOutCnt=0;
+
   VnVector3 vpe_mag_base_tuning_;
   VnVector3 vpe_mag_adaptive_tuning_;
   VnVector3 vpe_mag_adaptive_filtering_;
@@ -140,7 +150,8 @@ class ImuVn100 {
   SyncInfo sync_info_;
 
   du::Updater updater_;
-  DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_;
+  DiagnosedPublisher pd_imu_,pd_imu_comp_,pd_imu_uncomp_, pd_mag_,pd_mag_comp_;
+  DiagnosedPublisher pd_mag_uncomp_, pd_pres_, pd_temp_, pd_rpy_, pd_sync_trigger;
 
   void FixImuRate();
   void LoadParameters();

launch/vn_100_cont.launch

@@ -4,19 +4,23 @@
 
     <arg name="port" default="/dev/ttyUSB0" />
     <arg name="frame_id" default="$(arg imu)"/>
-    <arg name="baudrate" default="921600"/>
-    <arg name="imu_rate" default="100"/>
+    <arg name="baudrate" default="460800"/>
+    <arg name="imu_rate" default="400"/>
 
     <!-- Sync out settings -->
     <!-- to disable this just set it to < 0 -->
-    <arg name="sync_rate" default="20"/>
+    <arg name="sync_rate" default="0"/>
     <arg name="sync_pulse_width_us" default="1000"/>
 
+    <!-- Time to use settings -->
+    <arg name="use_imu_clock" default="true"/>
+
     <!-- Binary settings -->
     <arg name="binary_output" default="true"/>
-    <arg name="binary_async_mode" default="2"/>
+    <arg name="binary_async_mode" default="1"/>
 
-    <arg name="imu_compensated" default="false"/>
+    <arg name="imu_compensated" default="2" />
+    <!-- 0 - uncompensated only, 1 - compensated only, 2 - both compensated and uncompensated -->
 
     <!-- VPE settings -->
     <arg name="vpe_enable" default="true"/>
@@ -25,19 +29,20 @@
     <arg name="vpe_tuning_mode" default="1"/>
 
     <!-- Ros Topic settings -->
-    <arg name="enable_mag" default="true"/>
+    <arg name="enable_mag" default="false"/>
     <arg name="enable_pres" default="true"/>
     <arg name="enable_temp" default="true"/>
     <arg name="enable_rpy" default="false"/>
 
     <node pkg="imu_vn_100" name="$(arg imu)" type="imu_vn_100_node" output="$(arg output)">
         <param name="port" type="string" value="$(arg port)"/>
+        <param name="use_imu_clock" value="$(arg use_imu_clock)"/>
         <param name="baudrate" type="int" value="$(arg baudrate)"/>
         <param name="frame_id" type="string" value="$(arg frame_id)"/>
         <param name="imu_rate" type="int" value="$(arg imu_rate)"/>
         <param name="binary_output" type="bool" value="$(arg binary_output)"/>
         <param name="binary_async_mode" type="int" value="$(arg binary_async_mode)"/>
-        <param name="imu_compensated" type="bool" value="$(arg imu_compensated)"/>
+        <param name="imu_compensated" type="int" value="$(arg imu_compensated)"/>
         <param name="enable_mag" type="bool" value="$(arg enable_mag)"/>
         <param name="enable_pres" type="bool" value="$(arg enable_pres)"/>
         <param name="enable_temp" type="bool" value="$(arg enable_temp)"/>

launch/vn_100_cont_ext_trigger.launch

@@ -0,0 +1,56 @@
+<launch>
+    <arg name="output" default="screen"/>
+    <arg name="imu" default="imu"/>
+
+    <arg name="port" default="/dev/ttyUSB0" />
+    <arg name="frame_id" default="$(arg imu)"/>
+    <arg name="baudrate" default="460800"/>
+    <arg name="imu_rate" default="400"/>
+
+    <!-- Sync out settings -->
+    <!-- to disable this just set it to < 0 -->
+    <arg name="sync_rate" default="100"/>
+    <arg name="sync_pulse_width_us" default="1000"/>
+
+    <!-- Time to use settings -->
+    <arg name="use_imu_clock" default="true"/>
+
+    <!-- Binary settings -->
+    <arg name="binary_output" default="true"/>
+    <arg name="binary_async_mode" default="1"/>
+
+    <arg name="imu_compensated" default="1"/>
+
+    <!-- VPE settings -->
+    <arg name="vpe_enable" default="true"/>
+    <arg name="vpe_heading_mode" default="1"/>
+    <arg name="vpe_filtering_mode" default="1"/>
+    <arg name="vpe_tuning_mode" default="1"/>
+
+    <!-- Ros Topic settings -->
+    <arg name="enable_mag" default="true"/>
+    <arg name="enable_pres" default="true"/>
+    <arg name="enable_temp" default="true"/>
+    <arg name="enable_rpy" default="false"/>
+
+    <node pkg="imu_vn_100" name="$(arg imu)" type="imu_vn_100_node" output="$(arg output)">
+        <param name="port" type="string" value="$(arg port)"/>
+        <param name="use_imu_clock" value="$(arg use_imu_clock)"/>
+        <param name="baudrate" type="int" value="$(arg baudrate)"/>
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="imu_rate" type="int" value="$(arg imu_rate)"/>
+        <param name="binary_output" type="bool" value="$(arg binary_output)"/>
+        <param name="binary_async_mode" type="int" value="$(arg binary_async_mode)"/>
+        <param name="imu_compensated" type="int" value="$(arg imu_compensated)"/>
+        <param name="enable_mag" type="bool" value="$(arg enable_mag)"/>
+        <param name="enable_pres" type="bool" value="$(arg enable_pres)"/>
+        <param name="enable_temp" type="bool" value="$(arg enable_temp)"/>
+        <param name="enable_rpy" type="bool" value="$(arg enable_rpy)"/>
+        <param name="sync_rate" type="int" value="$(arg sync_rate)"/>
+        <param name="sync_pulse_width_us" type="int" value="$(arg sync_pulse_width_us)"/>
+        <param name="vpe/enable" type="bool" value="$(arg vpe_enable)"/>
+        <param name="vpe/heading_mode" type="int" value="$(arg vpe_heading_mode)"/>
+        <param name="vpe/filtering_mode" type="int" value="$(arg vpe_filtering_mode)"/>
+        <param name="vpe/tuning_mode" type="int" value="$(arg vpe_tuning_mode)"/>
+    </node>
+</launch>

launch/vn_100_cont_rpy_ex_trig.launch

@@ -0,0 +1,56 @@
+<launch>
+    <arg name="output" default="screen"/>
+    <arg name="imu" default="imu"/>
+
+    <arg name="port" default="/dev/ttyUSB0" />
+    <arg name="frame_id" default="$(arg imu)"/>
+    <arg name="baudrate" default="460800"/>
+    <arg name="imu_rate" default="400"/>
+
+    <!-- Sync out settings -->
+    <!-- to disable this just set it to < 0 -->
+    <arg name="sync_rate" default="100"/>
+    <arg name="sync_pulse_width_us" default="1000"/>
+
+    <!-- Time to use settings -->
+    <arg name="use_imu_clock" default="true"/>
+
+    <!-- Binary settings -->
+    <arg name="binary_output" default="true"/>
+    <arg name="binary_async_mode" default="1"/>
+
+    <arg name="imu_compensated" default="2"/>
+
+    <!-- VPE settings -->
+    <arg name="vpe_enable" default="true"/>
+    <arg name="vpe_heading_mode" default="1"/>
+    <arg name="vpe_filtering_mode" default="1"/>
+    <arg name="vpe_tuning_mode" default="1"/>
+
+    <!-- Ros Topic settings -->
+    <arg name="enable_mag" default="false"/>
+    <arg name="enable_pres" default="true"/>
+    <arg name="enable_temp" default="true"/>
+    <arg name="enable_rpy" default="true"/>
+
+    <node pkg="imu_vn_100" name="$(arg imu)" type="imu_vn_100_node" output="$(arg output)">
+        <param name="port" type="string" value="$(arg port)"/>
+        <param name="use_imu_clock" value="$(arg use_imu_clock)"/>
+        <param name="baudrate" type="int" value="$(arg baudrate)"/>
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="imu_rate" type="int" value="$(arg imu_rate)"/>
+        <param name="binary_output" type="bool" value="$(arg binary_output)"/>
+        <param name="binary_async_mode" type="int" value="$(arg binary_async_mode)"/>
+        <param name="imu_compensated" type="int" value="$(arg imu_compensated)"/>
+        <param name="enable_mag" type="bool" value="$(arg enable_mag)"/>
+        <param name="enable_pres" type="bool" value="$(arg enable_pres)"/>
+        <param name="enable_temp" type="bool" value="$(arg enable_temp)"/>
+        <param name="enable_rpy" type="bool" value="$(arg enable_rpy)"/>
+        <param name="sync_rate" type="int" value="$(arg sync_rate)"/>
+        <param name="sync_pulse_width_us" type="int" value="$(arg sync_pulse_width_us)"/>
+        <param name="vpe/enable" type="bool" value="$(arg vpe_enable)"/>
+        <param name="vpe/heading_mode" type="int" value="$(arg vpe_heading_mode)"/>
+        <param name="vpe/filtering_mode" type="int" value="$(arg vpe_filtering_mode)"/>
+        <param name="vpe/tuning_mode" type="int" value="$(arg vpe_tuning_mode)"/>
+    </node>
+</launch>

package.xml

@@ -10,10 +10,10 @@
   <author email="sunke.polyu@gmail.com">Ke Sun</author>
 
   <buildtool_depend>catkin</buildtool_depend>
-
   <depend>diagnostic_updater</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>trigger_msgs</depend>
 
 
   <export>