From e31dcf4a49f311763e11a8344c7ff547dcff9a1b Mon Sep 17 00:00:00 2001
From: Angelo Mastroberardino <angelo@parkopedia.com>
Date: Mon, 20 Aug 2018 18:10:33 +0100
Subject: [PATCH 1/3] added output_config param

---
 launch/xsens_driver.launch | 26 ++++++++++++++------------
 nodes/mtnode.py            |  5 +++++
 2 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/launch/xsens_driver.launch b/launch/xsens_driver.launch
index c0478f5..e5cd593 100644
--- a/launch/xsens_driver.launch
+++ b/launch/xsens_driver.launch
@@ -4,6 +4,7 @@
     <arg name="baudrate" default="0" doc="baudrate of the IMU"/>
     <arg name="timeout" default="0.002" doc="timeout for the IMU communication"/>
     <arg name="initial_wait" default="0.1" doc="initial wait to allow device to come up"/>
+    <arg name="output_config" default="oq,ad,wd,mf" doc="OUTPUT format (as in mtdevice.py --help)"/>
     <arg name="frame_id" default="/imu" doc="frame id of the IMU"/>
     <arg name="frame_local" default="ENU" doc="desired frame orientation (ENU, NED or NWU)"/>
     <arg name="no_rotation_duration" default="0" doc="duration (int in seconds) of the no-rotation calibration procedure"/>
@@ -12,16 +13,17 @@
     <arg name="orientation_covariance_diagonal" default="[0.01745, 0.01745, 0.15708]" doc="Diagonal elements of the orientation covariance matrix"/>
 
     <!-- node -->
-	<node pkg="xsens_driver" type="mtnode.py" name="xsens_driver" output="screen" >
-		<param name="device" value="$(arg device)"/>
-		<param name="baudrate" value="$(arg baudrate)"/>
-		<param name="timeout" value="$(arg timeout)"/>
-		<param name="initial_wait" value="$(arg initial_wait)"/>
-		<param name="frame_id" value="$(arg frame_id)"/>
-		<param name="frame_local" value="$(arg frame_local)"/>
-		<param name="no_rotation_duration" value="$(arg no_rotation_duration)"/>
-		<rosparam param="angular_velocity_covariance_diagonal" subst_value="True">$(arg angular_velocity_covariance_diagonal)</rosparam>
-		<rosparam param="linear_acceleration_covariance_diagonal" subst_value="True">$(arg linear_acceleration_covariance_diagonal)</rosparam>
-		<rosparam param="orientation_covariance_diagonal" subst_value="True">$(arg orientation_covariance_diagonal)</rosparam>
-	</node>
+    <node pkg="xsens_driver" type="mtnode.py" name="xsens_driver" output="screen" >
+        <param name="device" value="$(arg device)"/>
+        <param name="baudrate" value="$(arg baudrate)"/>
+        <param name="timeout" value="$(arg timeout)"/>
+        <param name="initial_wait" value="$(arg initial_wait)"/>
+        <param name="output_config" value="$(arg output_config)" />
+        <param name="frame_id" value="$(arg frame_id)"/>
+        <param name="frame_local" value="$(arg frame_local)"/>
+        <param name="no_rotation_duration" value="$(arg no_rotation_duration)"/>
+        <rosparam param="angular_velocity_covariance_diagonal" subst_value="True">$(arg angular_velocity_covariance_diagonal)</rosparam>
+        <rosparam param="linear_acceleration_covariance_diagonal" subst_value="True">$(arg linear_acceleration_covariance_diagonal)</rosparam>
+        <rosparam param="orientation_covariance_diagonal" subst_value="True">$(arg orientation_covariance_diagonal)</rosparam>
+    </node>
 </launch>
diff --git a/nodes/mtnode.py b/nodes/mtnode.py
index 2d21c99..14a1a77 100755
--- a/nodes/mtnode.py
+++ b/nodes/mtnode.py
@@ -57,6 +57,7 @@ def __init__(self):
         baudrate = get_param('~baudrate', 0)
         timeout = get_param('~timeout', 0.002)
         initial_wait = get_param('~initial_wait', 0.1)
+        output_config = get_param('~output_config', '')
         if device == 'auto':
             devs = mtdevice.find_devices(timeout=timeout,
                                          initial_wait=initial_wait)
@@ -79,6 +80,10 @@ def __init__(self):
         rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
         self.mt = mtdevice.MTDevice(device, baudrate, timeout,
                                     initial_wait=initial_wait)
+        if output_config:
+            rospy.loginfo("Setting MTDevice OUTPUT to: '%s' " % output_config)
+            oc = mtdevice.get_output_config(output_config)
+            self.mt.SetOutputConfiguration(oc)
 
         # optional no rotation procedure for internal calibration of biases
         # (only mark iv devices)

From 6b89900ba95935adc87982708640f444cf11accf Mon Sep 17 00:00:00 2001
From: Angelo Mastroberardino <angelo@parkopedia.com>
Date: Fri, 24 Aug 2018 15:41:16 +0100
Subject: [PATCH 2/3] GoToConfig before reset the output (following the pdf
 guide)

---
 nodes/mtnode.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/nodes/mtnode.py b/nodes/mtnode.py
index 14a1a77..65fdb08 100755
--- a/nodes/mtnode.py
+++ b/nodes/mtnode.py
@@ -83,7 +83,9 @@ def __init__(self):
         if output_config:
             rospy.loginfo("Setting MTDevice OUTPUT to: '%s' " % output_config)
             oc = mtdevice.get_output_config(output_config)
+            self.mt.GoToConfig()
             self.mt.SetOutputConfiguration(oc)
+            self.mt.GoToMeasurement()
 
         # optional no rotation procedure for internal calibration of biases
         # (only mark iv devices)

From 4efa8f11d087b848605660ca23167b8daf680145 Mon Sep 17 00:00:00 2001
From: Angelo Mastroberardino <angelo@parkopedia.com>
Date: Wed, 29 Aug 2018 10:25:54 +0100
Subject: [PATCH 3/3] default params, removed useless guards

---
 nodes/mtnode.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/nodes/mtnode.py b/nodes/mtnode.py
index 65fdb08..195e6e6 100755
--- a/nodes/mtnode.py
+++ b/nodes/mtnode.py
@@ -57,7 +57,7 @@ def __init__(self):
         baudrate = get_param('~baudrate', 0)
         timeout = get_param('~timeout', 0.002)
         initial_wait = get_param('~initial_wait', 0.1)
-        output_config = get_param('~output_config', '')
+        output_config = get_param('~output_config', 'oq,ad,wd,mf')
         if device == 'auto':
             devs = mtdevice.find_devices(timeout=timeout,
                                          initial_wait=initial_wait)
@@ -83,9 +83,7 @@ def __init__(self):
         if output_config:
             rospy.loginfo("Setting MTDevice OUTPUT to: '%s' " % output_config)
             oc = mtdevice.get_output_config(output_config)
-            self.mt.GoToConfig()
             self.mt.SetOutputConfiguration(oc)
-            self.mt.GoToMeasurement()
 
         # optional no rotation procedure for internal calibration of biases
         # (only mark iv devices)