From e31dcf4a49f311763e11a8344c7ff547dcff9a1b Mon Sep 17 00:00:00 2001 From: Angelo Mastroberardino Date: Mon, 20 Aug 2018 18:10:33 +0100 Subject: [PATCH 1/4] 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 @@ + @@ -12,16 +13,17 @@ - - - - - - - - - $(arg angular_velocity_covariance_diagonal) - $(arg linear_acceleration_covariance_diagonal) - $(arg orientation_covariance_diagonal) - + + + + + + + + + + $(arg angular_velocity_covariance_diagonal) + $(arg linear_acceleration_covariance_diagonal) + $(arg orientation_covariance_diagonal) + 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 Date: Fri, 24 Aug 2018 15:41:16 +0100 Subject: [PATCH 2/4] 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 5accd3e7a5d45a10697c385b4408ea882286dd94 Mon Sep 17 00:00:00 2001 From: Angelo Mastroberardino Date: Wed, 29 Aug 2018 10:22:38 +0100 Subject: [PATCH 3/4] testing orientation and coordinates --- launch/xsens_driver.launch | 8 ++++++-- nodes/mtdef.py | 11 +++++++++++ nodes/mtdevice.py | 16 ++++++++++++++++ nodes/mtnode.py | 20 ++++++++++++++++++-- 4 files changed, 51 insertions(+), 4 deletions(-) diff --git a/launch/xsens_driver.launch b/launch/xsens_driver.launch index e5cd593..3b91029 100644 --- a/launch/xsens_driver.launch +++ b/launch/xsens_driver.launch @@ -5,8 +5,10 @@ + + - + @@ -19,6 +21,8 @@ + + @@ -26,4 +30,4 @@ $(arg linear_acceleration_covariance_diagonal) $(arg orientation_covariance_diagonal) - + \ No newline at end of file diff --git a/nodes/mtdef.py b/nodes/mtdef.py index 2b7ec54..1b0aba9 100644 --- a/nodes/mtdef.py +++ b/nodes/mtdef.py @@ -93,6 +93,17 @@ class MID: # Filter messages # Reset orientation, 2 bytes ResetOrientation = 0xA4 + ResetOrientationAck = 0xA5 + class ResetCode: + StoreCurrentSettings = 0x0000 + ResetHeading = 0x0001 + # Reserved = 0x0002 + ResetInclination = 0x0003 + ResetAlignment = 0x0004 + DefaultHeading = 0x0005 + DefaultInclination = 0x0006 + DefaultAlignment = 0x0007 + # Request or set UTC time from sensor (MTI-G and MTi-10/100 series) SetUTCTime = 0x60 # Set correction ticks to UTC time diff --git a/nodes/mtdevice.py b/nodes/mtdevice.py index 0bf0213..eef69cb 100755 --- a/nodes/mtdevice.py +++ b/nodes/mtdevice.py @@ -54,6 +54,7 @@ def __init__(self, port, baudrate=115200, timeout=0.002, autoconf=True, if config_mode: self.GoToConfig() + ############################################################ # Low-level communication ############################################################ @@ -140,12 +141,14 @@ def read_msg(self): continue # read message id and length of message mid, length = struct.unpack('!BB', self.waitfor(2)) + print "**** MID 0x%02X **** " % mid if length == 255: # extended length length, = struct.unpack('!H', self.waitfor(2)) # read contents and checksum buf = self.waitfor(length+1) checksum = buf[-1] data = struct.unpack('!%dB' % length, buf[:-1]) + print "**** data %s, data[0]: 0x%02X **** " % (data, data[0] if len(data) else 0) # check message integrity if 0xFF & sum(data, 0xFF+mid+length+checksum): if self.verbose: @@ -573,9 +576,22 @@ def ResetOrientation(self, code): 0x0001: heading reset (NOT supported by MTi-G), 0x0003: object reset. """ + print "msg: 0x%02X, code: 0x%02X " % (MID.ResetOrientation, code) data = struct.pack('!H', code) self.write_ack(MID.ResetOrientation, data) + def SaveCurrentOrientation(self): + self.ResetOrientation(MID.ResetCode.StoreCurrentSettings) + + def ResetHeading(self): + self.ResetOrientation(MID.ResetCode.ResetHeading) + + def ResetInclination(self): + self.ResetOrientation(MID.ResetCode.ResetInclination) + + def ResetAlignment(self): + self.ResetOrientation(MID.ResetCode.ResetAlignment) + def SetNoRotation(self, duration): """Initiate the "no rotation" procedure to estimate gyro biases.""" self._ensure_measurement_state() diff --git a/nodes/mtnode.py b/nodes/mtnode.py index 65fdb08..746a16a 100755 --- a/nodes/mtnode.py +++ b/nodes/mtnode.py @@ -58,6 +58,8 @@ def __init__(self): timeout = get_param('~timeout', 0.002) initial_wait = get_param('~initial_wait', 0.1) output_config = get_param('~output_config', '') + reset_orientation = get_param('~reset_orientation', False) + set_geo_coordinates = get_param('~set_geo_coordinates', '') if device == 'auto': devs = mtdevice.find_devices(timeout=timeout, initial_wait=initial_wait) @@ -80,12 +82,26 @@ 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.GoToConfig() self.mt.SetOutputConfiguration(oc) - self.mt.GoToMeasurement() + + if reset_orientation: + rospy.loginfo("Resetting MTDevice orientation") + #self.mt.ResetHeading() + self.mt.ResetAlignment() + self.mt.ResetInclination() + + if set_geo_coordinates: + coords = set_geo_coordinates.split(",") + if len(coords) != 3: + raise RuntimeError("set_geo_coordinates wrongly formatted: " \ + "can only take 3 values comma separated") + lat, lng, alt = float(coords[0]), float(coords[1]), float(coords[2]) + rospy.loginfo("Setting Lat, Lng, Alt: %f, %f, %f" % (lat, lng, alt)) + self.mt.SetLatLonAlt(lat, lng, alt) # optional no rotation procedure for internal calibration of biases # (only mark iv devices) From fff7663bb857f13465ca2166010135dd387568ec Mon Sep 17 00:00:00 2001 From: Angelo Mastroberardino Date: Wed, 29 Aug 2018 10:40:25 +0100 Subject: [PATCH 4/4] added only set-coordinates option, in isolation --- launch/xsens_driver.launch | 8 ++------ nodes/mtdef.py | 11 ----------- nodes/mtdevice.py | 16 ---------------- nodes/mtnode.py | 15 +-------------- 4 files changed, 3 insertions(+), 47 deletions(-) diff --git a/launch/xsens_driver.launch b/launch/xsens_driver.launch index 3b91029..427fe98 100644 --- a/launch/xsens_driver.launch +++ b/launch/xsens_driver.launch @@ -4,11 +4,9 @@ - - - + - + @@ -20,8 +18,6 @@ - - diff --git a/nodes/mtdef.py b/nodes/mtdef.py index 1b0aba9..2b7ec54 100644 --- a/nodes/mtdef.py +++ b/nodes/mtdef.py @@ -93,17 +93,6 @@ class MID: # Filter messages # Reset orientation, 2 bytes ResetOrientation = 0xA4 - ResetOrientationAck = 0xA5 - class ResetCode: - StoreCurrentSettings = 0x0000 - ResetHeading = 0x0001 - # Reserved = 0x0002 - ResetInclination = 0x0003 - ResetAlignment = 0x0004 - DefaultHeading = 0x0005 - DefaultInclination = 0x0006 - DefaultAlignment = 0x0007 - # Request or set UTC time from sensor (MTI-G and MTi-10/100 series) SetUTCTime = 0x60 # Set correction ticks to UTC time diff --git a/nodes/mtdevice.py b/nodes/mtdevice.py index eef69cb..0bf0213 100755 --- a/nodes/mtdevice.py +++ b/nodes/mtdevice.py @@ -54,7 +54,6 @@ def __init__(self, port, baudrate=115200, timeout=0.002, autoconf=True, if config_mode: self.GoToConfig() - ############################################################ # Low-level communication ############################################################ @@ -141,14 +140,12 @@ def read_msg(self): continue # read message id and length of message mid, length = struct.unpack('!BB', self.waitfor(2)) - print "**** MID 0x%02X **** " % mid if length == 255: # extended length length, = struct.unpack('!H', self.waitfor(2)) # read contents and checksum buf = self.waitfor(length+1) checksum = buf[-1] data = struct.unpack('!%dB' % length, buf[:-1]) - print "**** data %s, data[0]: 0x%02X **** " % (data, data[0] if len(data) else 0) # check message integrity if 0xFF & sum(data, 0xFF+mid+length+checksum): if self.verbose: @@ -576,22 +573,9 @@ def ResetOrientation(self, code): 0x0001: heading reset (NOT supported by MTi-G), 0x0003: object reset. """ - print "msg: 0x%02X, code: 0x%02X " % (MID.ResetOrientation, code) data = struct.pack('!H', code) self.write_ack(MID.ResetOrientation, data) - def SaveCurrentOrientation(self): - self.ResetOrientation(MID.ResetCode.StoreCurrentSettings) - - def ResetHeading(self): - self.ResetOrientation(MID.ResetCode.ResetHeading) - - def ResetInclination(self): - self.ResetOrientation(MID.ResetCode.ResetInclination) - - def ResetAlignment(self): - self.ResetOrientation(MID.ResetCode.ResetAlignment) - def SetNoRotation(self, duration): """Initiate the "no rotation" procedure to estimate gyro biases.""" self._ensure_measurement_state() diff --git a/nodes/mtnode.py b/nodes/mtnode.py index 746a16a..708a024 100755 --- a/nodes/mtnode.py +++ b/nodes/mtnode.py @@ -57,9 +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', '') - reset_orientation = get_param('~reset_orientation', False) - set_geo_coordinates = get_param('~set_geo_coordinates', '') + set_geo_coordinates = get_param('~set_geo_coordinates', '51.5,-0.09,0.0') if device == 'auto': devs = mtdevice.find_devices(timeout=timeout, initial_wait=initial_wait) @@ -83,17 +81,6 @@ def __init__(self): 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) - - if reset_orientation: - rospy.loginfo("Resetting MTDevice orientation") - #self.mt.ResetHeading() - self.mt.ResetAlignment() - self.mt.ResetInclination() - if set_geo_coordinates: coords = set_geo_coordinates.split(",") if len(coords) != 3: