diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0e10c40..358c3d4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package xsens_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.0 (2022-09-30) +------------------ +* Migrated to ROS 2 (humble) +* Contributors: Dominic Baril (Norlab), Simon-Pierre Deschênes (Norlab) + 2.2.2 (2018-08-02) ------------------ * fix exception while closing node diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 64a67ee..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,166 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(xsens_driver) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy std_msgs tf sensor_msgs geometry_msgs diagnostic_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES test_py - CATKIN_DEPENDS rospy std_msgs tf sensor_msgs geometry_msgs diagnostic_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(test_py -# src/${PROJECT_NAME}/test_py.cpp -# ) - -## Declare a cpp executable -# add_executable(test_py_node src/test_py_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(test_py_node test_py_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(test_py_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -install(PROGRAMS - nodes/mtnode.py - nodes/mtdef.py - nodes/mtdevice.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark executables and/or libraries for installation -# install(TARGETS test_py test_py_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install( - DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_test_py.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/README b/README index b0cdb01..ce34f54 100644 --- a/README +++ b/README @@ -1,3 +1,11 @@ -ROS Driver for XSens MT/MTi/MTi-G devices. +ROS 2 driver for XSens MT/MTi/MTi-G devices. See: http://ros.org/wiki/ethzasl_xsens_driver + +** Some useful commands to enable serial communication + +``` +sudo adduser $USER dialout +``` + +where `$USER` is the username. diff --git a/launch/xsens_driver.launch b/launch/xsens_driver.launch deleted file mode 100644 index c0478f5..0000000 --- a/launch/xsens_driver.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - $(arg angular_velocity_covariance_diagonal) - $(arg linear_acceleration_covariance_diagonal) - $(arg orientation_covariance_diagonal) - - diff --git a/launch/xsens_driver.launch.xml b/launch/xsens_driver.launch.xml new file mode 100644 index 0000000..01c6dc4 --- /dev/null +++ b/launch/xsens_driver.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mainpage.dox b/mainpage.dox index 9ce7a6d..6b97375 100644 --- a/mainpage.dox +++ b/mainpage.dox @@ -2,11 +2,11 @@ \mainpage \htmlinclude manifest.html -The \b xsens_driver package provides \b mtnode.py, a generic ROS node publishing the data streamed by an XSens motion tracker (MT, MTi, MTi-G...) from third, fourth or fifth generation. +The \b xsens_driver package provides \b mtnode.py, a generic ROS 2 node publishing the data streamed by an XSens motion tracker (MT, MTi, MTi-G...) from third, fourth or fifth generation. \section topics Topics -The ROS node is a wrapper around the \b mtdevice::MTDevice class. It can publish the following topics, depending on the configuration of the device: +The ROS 2 node is a wrapper around the \b mtdevice::MTDevice class. It can publish the following topics, depending on the configuration of the device: - \c /imu/data (sensor_msgs::Imu): orientation, angular velocity, linear acceleration, - \c /fix (sensor_msgs::NavSatFix): longitude, latitude, altitude (from filtered/fused GPS and IMU), - \c /raw_fix (sensor_msgs::NavSatFix): longitude, latitude, altitude (from ONLY GPS), @@ -29,16 +29,16 @@ The covariance information in the + + xsens_driver - 2.2.2 - - ROS Driver for XSens MT/MTi/MTi-G devices. - + 3.0.0 + ROS 2 driver for XSens MT/MTi/MTi-G devices. Francis Colas BSD - catkin + rclpy + std_msgs + sensor_msgs + geometry_msgs + diagnostic_msgs + python-serial + tf_transformations - rospy - std_msgs - tf - sensor_msgs - geometry_msgs - diagnostic_msgs - - rospy - std_msgs - tf - sensor_msgs - geometry_msgs - diagnostic_msgs - python-serial + + ament_python + - - diff --git a/resource/xsens_driver b/resource/xsens_driver new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..49b2a2a --- /dev/null +++ b/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/xsens_driver +[install] +install_scripts=$base/lib/xsens_driver + diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..2d50be0 --- /dev/null +++ b/setup.py @@ -0,0 +1,28 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'xsens_driver' + +setup( + name='xsens_driver', + version='3.0.0', + packages=[package_name], + data_files=[ + ('share/' + package_name, ['package.xml']), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Francis Colas', + maintainer_email='francis.colas@mavt.ethz.ch', + description='ROS 2 driver for XSens MT/MTi/MTi-G devices.', + license='BSD', + entry_points={ + 'console_scripts': [ + 'mtnode.py = xsens_driver.mtnode:main', + ], + }, +) + diff --git a/xsens_driver/__init__.py b/xsens_driver/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/nodes/mtdef.py b/xsens_driver/mtdef.py similarity index 99% rename from nodes/mtdef.py rename to xsens_driver/mtdef.py index 0b2cde0..b1ef1c7 100644 --- a/nodes/mtdef.py +++ b/xsens_driver/mtdef.py @@ -166,7 +166,7 @@ class DeprecatedMID: def getName(cls, value): '''Return the name of the first found member of class cls with given value.''' - for k, v in cls.__dict__.iteritems(): + for k, v in cls.__dict__.items(): if v == value: return k return '' diff --git a/nodes/mtdevice.py b/xsens_driver/mtdevice.py similarity index 94% rename from nodes/mtdevice.py rename to xsens_driver/mtdevice.py index d3c6092..e10ec43 100755 --- a/nodes/mtdevice.py +++ b/xsens_driver/mtdevice.py @@ -9,7 +9,7 @@ import re import pprint -from mtdef import MID, OutputMode, OutputSettings, MTException, Baudrates, \ +from xsens_driver.mtdef import MID, OutputMode, OutputSettings, MTException, Baudrates, \ XDIGroup, getMIDName, DeviceState, DeprecatedMID, MTErrorMessage, \ MTTimeoutException @@ -65,7 +65,7 @@ def write_msg(self, mid, data=b''): else: lendat = struct.pack('!B', length) packet = b'\xFA\xFF' + struct.pack('!B', mid) + lendat + data - packet += struct.pack('!B', 0xFF & (-(sum(map(ord, packet[1:]))))) + packet += struct.pack('!B', 0xFF & (-(sum(packet[1:])))) msg = packet start = time.time() while ((time.time()-start) < self.timeout) and self.device.read(): @@ -75,9 +75,9 @@ def write_msg(self, mid, data=b''): except serial.serialutil.SerialTimeoutException: raise MTTimeoutException("writing message") if self.verbose: - print "MT: Write message id 0x%02X (%s) with %d data bytes: "\ + print("MT: Write message id 0x%02X (%s) with %d data bytes: "\ "[%s]" % (mid, getMIDName(mid), length, - ' '.join("%02X" % ord(v) for v in data)) + ' '.join("%02X" % ord(v) for v in data))) def waitfor(self, size=1): """Get a given amount of data.""" @@ -87,8 +87,8 @@ def waitfor(self, size=1): if len(buf) == size: return buf if self.verbose: - print "waiting for %d bytes, got %d so far: [%s]" % \ - (size, len(buf), ' '.join('%02X' % v for v in buf)) + print("waiting for %d bytes, got %d so far: [%s]" % \ + (size, len(buf), ' '.join('%02X' % v for v in buf))) raise MTTimeoutException("waiting for message") def read_data_msg(self, buf=bytearray()): @@ -153,9 +153,9 @@ def read_msg(self): "waiting for next message.\n") continue if self.verbose: - print "MT: Got message id 0x%02X (%s) with %d data bytes: "\ + print("MT: Got message id 0x%02X (%s) with %d data bytes: "\ "[%s]" % (mid, getMIDName(mid), length, - ' '.join("%02X" % v for v in data)) + ' '.join("%02X" % v for v in data))) if mid == MID.Error: raise MTErrorMessage(data[0]) return (mid, buf[:-1]) @@ -171,8 +171,8 @@ def write_ack(self, mid, data=b'', n_resend=30, n_read=25): if mid_ack == (mid+1): break elif self.verbose: - print "ack (0x%02X) expected, got 0x%02X instead" % \ - (mid+1, mid_ack) + print("ack (0x%02X) expected, got 0x%02X instead" % \ + (mid+1, mid_ack)) else: # inner look not broken continue # retry (send+wait) break # still no luck @@ -674,7 +674,7 @@ def configure_legacy(self, mode, settings, period=None, skipfactor=None): raise except MTException as e: if self.verbose: - print "no ack received while switching from MTData2 to MTData." + print("no ack received while switching from MTData2 to MTData.") pass # no ack??? self.SetOutputMode(mode) self.SetOutputSettings(settings) @@ -1151,7 +1151,7 @@ def parse_MTData(self, data, mode=None, settings=None): data = data[12:] output['Timestamp'] = o # TODO at that point data should be empty - except struct.error, e: + except struct.error as e: raise MTException("could not parse MTData message.") if data != '': raise MTException("could not parse MTData message (too long).") @@ -1177,7 +1177,7 @@ def find_devices(timeout=0.002, verbose=False, initial_wait=0.1): mtdev_list = [] for port in glob.glob("/dev/tty*S*"): if verbose: - print "Trying '%s'" % port + print("Trying '%s'" % port) try: br = find_baudrate(port, timeout, verbose, initial_wait) if br: @@ -1194,31 +1194,31 @@ def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1): baudrates = (115200, 460800, 921600, 230400, 57600, 38400, 19200, 9600) for br in baudrates: if verbose: - print "Trying %d bd:" % br, - sys.stdout.flush() + print("Trying %d bd:" % br, + sys.stdout.flush()) try: mt = MTDevice(port, br, timeout=timeout, verbose=verbose, initial_wait=initial_wait) except serial.SerialException: if verbose: - print "fail: unable to open device." + print("fail: unable to open device.") raise MTException("unable to open %s" % port) try: mt.GoToConfig() mt.GoToMeasurement() if verbose: - print "ok." + print("ok.") return br except MTException: if verbose: - print "fail." + print("fail.") ################################################################ # Documentation for stand alone usage ################################################################ def usage(): - print """MT device driver. + print("""MT device driver.) Usage: ./mtdevice.py [commands] [opts] @@ -1522,7 +1522,7 @@ def usage(): 115200/(PERIOD * (SKIPFACTOR + 1)) If the value is 0xffff, no data is send unless a ReqData request is made. -""" +""") ################################################################ @@ -1539,10 +1539,10 @@ def main(): 'option-flags=', 'icc-command=', 'timeout=', 'initial-wait='] try: opts, args = getopt.gnu_getopt(sys.argv[1:], shopts, lopts) - except getopt.GetoptError, e: - print e + except getopt.GetoptError as e: + print(e) usage() - return 1 + # return 1 # default values device = '/dev/ttyUSB0' baudrate = 115200 @@ -1569,7 +1569,7 @@ def main(): try: new_baudrate = int(a) except ValueError: - print "change-baudrate argument must be integer." + print("change-baudrate argument must be integer.") return 1 actions.append('change-baudrate') elif o in ('-c', '--configure'): @@ -1587,7 +1587,7 @@ def main(): try: new_xkf = int(a) except ValueError: - print "xkf-scenario argument must be integer." + print("xkf-scenario argument must be integer.") return 1 actions.append('xkf-scenario') elif o in ('-y', '--synchronization'): @@ -1607,7 +1607,7 @@ def main(): try: baudrate = int(a) except ValueError: - print "baudrate argument must be integer." + print("baudrate argument must be integer.") return 1 elif o in ('-m', '--output-mode'): mode = get_mode(a) @@ -1621,13 +1621,13 @@ def main(): try: period = int(a) except ValueError: - print "period argument must be integer." + print("period argument must be integer.") return 1 elif o in ('-f', '--deprecated-skip-factor'): try: skipfactor = int(a) except ValueError: - print "skip-factor argument must be integer." + print("skip-factor argument must be integer.") return 1 elif o in ('-v', '--verbose'): verbose = True @@ -1650,13 +1650,13 @@ def main(): try: timeout = float(a) except ValueError: - print "timeout argument must be a floating number." + print("timeout argument must be a floating number.") return 1 elif o in ('-w', '--initial-wait'): try: initial_wait = float(a) except ValueError: - print "initial-wait argument must be a floating number." + print("initial-wait argument must be a floating number.") return 1 # if nothing else: echo @@ -1667,19 +1667,19 @@ def main(): devs = find_devices(timeout=timeout, verbose=verbose, initial_wait=initial_wait) if devs: - print "Detected devices:", "".join('\n\t%s @ %d' % (d, p) - for d, p in devs) - print "Using %s @ %d" % devs[0] + print("Detected devices:", "".join('\n\t%s @ %d' % (d, p) + for d, p in devs)) + print("Using %s @ %d" % devs[0]) device, baudrate = devs[0] else: - print "No suitable device found." + print("No suitable device found.") return 1 # find baudrate if not baudrate: baudrate = find_baudrate(device, timeout=timeout, verbose=verbose, initial_wait=initial_wait) if not baudrate: - print "No suitable baudrate found." + print("No suitable baudrate found.") return 1 # open device try: @@ -1691,29 +1691,29 @@ def main(): if 'inspect' in actions: inspect(mt, device, baudrate) if 'change-baudrate' in actions: - print "Changing baudrate from %d to %d:" % (baudrate, + print("Changing baudrate from %d to %d:") % (baudrate, new_baudrate), sys.stdout.flush() mt.ChangeBaudrate(new_baudrate) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'reset' in actions: - print "Restoring factory defaults", - sys.stdout.flush() + print("Restoring factory defaults", + sys.stdout.flush()) mt.RestoreFactoryDefaults() - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'configure' in actions: - print "Changing output configuration", - sys.stdout.flush() + print("Changing output configuration", + sys.stdout.flush()) mt.SetOutputConfiguration(output_config) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'synchronization' in actions: - print "Changing synchronization settings", - sys.stdout.flush() + print("Changing synchronization settings", + sys.stdout.flush()) mt.SetSyncSettings(sync_settings) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'setUTCtime' in actions: - print "Setting UTC time in the device", - sys.stdout.flush() + print("Setting UTC time in the device", + sys.stdout.flush()) mt.SetUTCTime(UTCtime_settings[6], UTCtime_settings[0], UTCtime_settings[1], @@ -1722,68 +1722,68 @@ def main(): UTCtime_settings[4], UTCtime_settings[5], UTCtime_settings[7]) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'gnss-platform' in actions: - print "Setting GNSS platform", - sys.stdout.flush() + print("Setting GNSS platform", + sys.stdout.flush()) mt.SetGnssPlatform(platform) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'option-flags' in actions: - print "Setting option flags", - sys.stdout.flush() + print("Setting option flags", + sys.stdout.flush()) mt.SetOptionFlags(*flag_tuple) - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? if 'icc-command' in actions: icc_command_names = { 0: 'start representative motion', 1: 'stop representative motion', 2: 'store ICC results', 3: 'representative motion state'} - print "Sending ICC command 0x%02X (%s):" % ( + print("Sending ICC command 0x%02X (%s):" % ( icc_command, icc_command_names[icc_command]), - sys.stdout.flush() + sys.stdout.flush()) res = mt.IccCommand(icc_command) if icc_command == 0x00: - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? elif icc_command == 0x01: - print res + print(res) elif icc_command == 0x02: - print " Ok" # should we test that it was actually ok? + print(" Ok") # should we test that it was actually ok? elif icc_command == 0x03: res_string = {0: 'representative motion inactive', 1: 'representation motion active'} - print "0x02X (%s)" % (res, res_string.get(res, 'unknown')) + print("0x02X (%s)" % (res, res_string.get(res, 'unknown'))) if 'legacy-configure' in actions: if mode is None: - print "output-mode is require to configure the device in "\ - "legacy mode." + print("output-mode is require to configure the device in "\ + "legacy mode.") return 1 if settings is None: - print "output-settings is required to configure the device "\ - "in legacy mode." + print("output-settings is required to configure the device "\ + "in legacy mode.") return 1 - print "Configuring in legacy mode", - sys.stdout.flush() + print("Configuring in legacy mode", + sys.stdout.flush()) mt.configure_legacy(mode, settings, period, skipfactor) - print " Ok" # should we test it was actually ok? + print(" Ok") # should we test it was actually ok? if 'xkf-scenario' in actions: - print "Changing XKF scenario", - sys.stdout.flush() + print("Changing XKF scenario", + sys.stdout.flush()) mt.SetCurrentScenario(new_xkf) - print "Ok" + print("Ok") if 'echo' in actions: # if (mode is None) or (settings is None): # mode, settings, length = mt.auto_config() # print mode, settings, length try: while True: - print mt.read_measurement(mode, settings) + print(mt.read_measurement(mode, settings)) except KeyboardInterrupt: pass except MTErrorMessage as e: - print "MTErrorMessage:", e + print("MTErrorMessage:", e) except MTException as e: - print "MTException:", e + print("MTException:", e) def inspect(mt, device, baudrate): @@ -1810,20 +1810,20 @@ def sync_fmt(settings): for s in settings) def try_message(m, f, formater=None, *args, **kwargs): - print ' %s ' % m, + print(' %s ' % m), try: if formater is not None: - print formater(f(*args, **kwargs)) + print(formater(f(*args, **kwargs))) else: pprint.pprint(f(*args, **kwargs), indent=4) except MTTimeoutException as e: - print 'timeout: might be unsupported by your device.' + print('timeout: might be unsupported by your device.') except MTErrorMessage as e: if e.code == 0x04: - print 'message unsupported by your device.' + print('message unsupported by your device.') else: raise e - print "Device: %s at %d Bd:" % (device, baudrate) + print("Device: %s at %d Bd:" % (device, baudrate)) try_message("device ID:", mt.GetDeviceID, hex_fmt(4)) try_message("product code:", mt.GetProductCode) try_message("hardware version:", mt.GetHardwareVersion) @@ -1910,7 +1910,7 @@ def get_output_config(config_arg): output_configuration.append((code, frequency)) return output_configuration except (IndexError, KeyError): - print 'could not parse output specification "%s"' % item + print('could not parse output specification "%s"' % item) return @@ -1954,7 +1954,7 @@ def get_mode(arg): elif c == 'r': mode |= OutputMode.RAW else: - print "Unknown output-mode specifier: '%s'" % c + print("Unknown output-mode specifier: '%s'" % c) return return mode @@ -2008,7 +2008,7 @@ def get_settings(arg): elif c == 'N': NED = OutputSettings.Coordinates_NED else: - print "Unknown output-settings specifier: '%s'" % c + print("Unknown output-settings specifier: '%s'" % c) return settings = timestamp | orient_mode | calib_mode | NED return settings @@ -2026,7 +2026,7 @@ def get_synchronization_settings(arg): # convert string to int sync_settings = tuple([int(i) for i in sync_settings]) except ValueError: - print "Synchronization sync_settings must be integers." + print("Synchronization sync_settings must be integers.") return # check synchronization sync_settings if sync_settings[0] in (3, 4, 8, 9, 11) and \ @@ -2035,7 +2035,7 @@ def get_synchronization_settings(arg): sync_settings[3] in (0, 1): return sync_settings else: - print "Invalid synchronization settings." + print("Invalid synchronization settings.") return @@ -2061,7 +2061,7 @@ def get_UTCtime(arg): try: time_settings = [int(i) for i in time_settings] except ValueError: - print "UTCtime settings must be integers." + print("UTCtime settings must be integers.") return # check UTCtime settings @@ -2074,7 +2074,7 @@ def get_UTCtime(arg): 0 <= time_settings[6] <= 1000000000: return time_settings else: - print "Invalid UTCtime settings." + print("Invalid UTCtime settings.") return @@ -2083,12 +2083,12 @@ def get_gnss_platform(arg): try: platform = int(arg) except ValueError: - print "GNSS platform must be an integer." + print("GNSS platform must be an integer.") return if platform in (0, 8): return platform else: - print "Invalid GNSS platform argument (excepted 0 or 8)." + print("Invalid GNSS platform argument (excepted 0 or 8).") return @@ -2099,8 +2099,8 @@ def get_option_flags(arg): arg.split(',')) return (set_flag, clear_flag) except ValueError: - print 'incorrect option flags specification (expected a pair of '\ - 'values)' + print('incorrect option flags specification (expected a pair of '\ + 'values)') return @@ -2112,7 +2112,7 @@ def get_icc_command(arg): raise ValueError return icc_command except ValueError: - print 'invalid ICC command "%s"; expected 0, 1, 2, or 3.' % arg + print('invalid ICC command "%s"; expected 0, 1, 2, or 3.' % arg) return diff --git a/nodes/mtnode.py b/xsens_driver/mtnode.py similarity index 78% rename from nodes/mtnode.py rename to xsens_driver/mtnode.py index 6be262d..c692eb3 100755 --- a/nodes/mtnode.py +++ b/xsens_driver/mtnode.py @@ -1,15 +1,14 @@ #!/usr/bin/env python -import roslib; roslib.load_manifest('xsens_driver') -import rospy +import rclpy +import rclpy.node import select import sys -import mtdevice -import mtdef +from xsens_driver import mtdevice +from xsens_driver import mtdef from std_msgs.msg import Header, String, UInt16 -from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, MagneticField,\ - FluidPressure, Temperature, TimeReference +from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, MagneticField, FluidPressure, Temperature, TimeReference from geometry_msgs.msg import TwistStamped, PointStamped from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus import time @@ -17,99 +16,55 @@ import calendar import serial -# transform Euler angles or matrix into quaternions from math import radians, sqrt, atan2 -from tf.transformations import quaternion_from_matrix, quaternion_from_euler,\ - identity_matrix - - -def get_param(name, default): - try: - v = rospy.get_param(name) - rospy.loginfo("Found parameter: %s, value: %s" % (name, str(v))) - except KeyError: - v = default - rospy.logwarn("Cannot find value for parameter: %s, assigning " - "default: %s" % (name, str(v))) - return v - - -def get_param_list(name, default): - value = get_param(name, default) - if len(default) != len(value): - rospy.logfatal("Parameter %s should be a list of size %d", name, len(default)) - sys.exit(1) - return value - - -def matrix_from_diagonal(diagonal): - n = len(diagonal) - matrix = [0] * n * n - for i in range(0, n): - matrix[i*n + i] = diagonal[i] - return tuple(matrix) - +# transform Euler angles or matrix into quaternions +from tf_transformations import quaternion_from_matrix, quaternion_from_euler, identity_matrix -class XSensDriver(object): +class XSensDriver(rclpy.node.Node): def __init__(self): - device = get_param('~device', 'auto') - baudrate = get_param('~baudrate', 0) - timeout = get_param('~timeout', 0.002) - initial_wait = get_param('~initial_wait', 0.1) + super().__init__("xsens_driver") + device = self.get_param('device', 'auto') + baudrate = self.get_param('baudrate', 0) + timeout = self.get_param('timeout', 0.002) + initial_wait = self.get_param('initial_wait', 0.1) if device == 'auto': - devs = mtdevice.find_devices(timeout=timeout, - initial_wait=initial_wait) + devs = mtdevice.find_devices(timeout=timeout, initial_wait=initial_wait) if devs: device, baudrate = devs[0] - rospy.loginfo("Detected MT device on port %s @ %d bps" - % (device, baudrate)) + self.get_logger().info("Detected MT device on port %s @ %d bps" % (device, baudrate)) else: - rospy.logerr("Fatal: could not find proper MT device.") - rospy.signal_shutdown("Could not find proper MT device.") - return + self.get_logger().error("Fatal: could not find proper MT device.") + sys.exit(1) if not baudrate: - baudrate = mtdevice.find_baudrate(device, timeout=timeout, - initial_wait=initial_wait) + baudrate = mtdevice.find_baudrate(device, timeout=timeout, initial_wait=initial_wait) if not baudrate: - rospy.logerr("Fatal: could not find proper baudrate.") - rospy.signal_shutdown("Could not find proper baudrate.") - return + self.get_logger().error("Fatal: could not find proper baudrate.") + sys.exit(1) - rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate)) - self.mt = mtdevice.MTDevice(device, baudrate, timeout, - initial_wait=initial_wait) + self.get_logger().info("MT node interface: %s at %d bd." % (device, baudrate)) + self.mt = mtdevice.MTDevice(device, baudrate, timeout, initial_wait=initial_wait) # optional no rotation procedure for internal calibration of biases # (only mark iv devices) - no_rotation_duration = get_param('~no_rotation_duration', 0) + no_rotation_duration = self.get_param('no_rotation_duration', 0) if no_rotation_duration: - rospy.loginfo("Starting the no-rotation procedure to estimate the " - "gyroscope biases for %d s. Please don't move the " - "IMU during this time." % no_rotation_duration) + self.get_logger().info("Starting the no-rotation procedure to estimate the gyroscope biases for %d s. Please don't move the IMU during this time." + % no_rotation_duration) self.mt.SetNoRotation(no_rotation_duration) - self.frame_id = get_param('~frame_id', '/base_imu') + self.frame_id = self.get_param('frame_id', 'base_imu') - self.frame_local = get_param('~frame_local', 'ENU') + self.frame_local = self.get_param('frame_local', 'ENU') - self.angular_velocity_covariance = matrix_from_diagonal( - get_param_list('~angular_velocity_covariance_diagonal', [radians(0.025)] * 3) - ) - self.linear_acceleration_covariance = matrix_from_diagonal( - get_param_list('~linear_acceleration_covariance_diagonal', [0.0004] * 3) - ) - self.orientation_covariance = matrix_from_diagonal( - get_param_list("~orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)]) - ) + self.angular_velocity_covariance = self.matrix_from_diagonal(self.get_param('angular_velocity_covariance_diagonal', [radians(0.025)] * 3)) + self.linear_acceleration_covariance = self.matrix_from_diagonal(self.get_param('linear_acceleration_covariance_diagonal', [0.0004] * 3)) + self.orientation_covariance = self.matrix_from_diagonal(self.get_param("orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)])) self.diag_msg = DiagnosticArray() - self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1, - message='No status information') - self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1, - message='No status information') - self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1, - message='No status information') + self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=bytes([1]), message='No status information') + self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=bytes([1]), message='No status information') + self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=bytes([1]), message='No status information') self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat] # publishers created at first use to reduce topic clutter @@ -129,10 +84,25 @@ def __init__(self): self.old_bGPS = 256 # publish GPS only if new # publish a string version of all data; to be parsed by clients - self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10) + self.str_pub = self.create_publisher(String, 'imu_data_str', 10) self.last_delta_q_time = None self.delta_q_rate = None + def get_param(self, name, default): + self.declare_parameter(name, default) + parameter = self.get_parameter(name).get_parameter_value() + parameter_fields = list(parameter.get_fields_and_field_types().keys()) + parameter_value = getattr(parameter, parameter_fields[parameter.type]) + self.get_logger().info("Parameter: %s, value: %s" % (name, str(parameter_value))) + return parameter_value + + def matrix_from_diagonal(self, diagonal): + n = len(diagonal) + matrix = [0.0] * n * n + for i in range(0, n): + matrix[i * n + i] = diagonal[i] + return tuple(matrix) + def reset_vars(self): self.imu_msg = Imu() self.imu_msg.orientation_covariance = (-1., )*9 @@ -146,7 +116,7 @@ def reset_vars(self): self.vel_msg = TwistStamped() self.pub_vel = False self.mag_msg = MagneticField() - self.mag_msg.magnetic_field_covariance = (0, )*9 + self.mag_msg.magnetic_field_covariance = (0.0, )*9 self.pub_mag = False self.temp_msg = Temperature() self.temp_msg.variance = 0. @@ -164,7 +134,7 @@ def reset_vars(self): def spin(self): try: - while not rospy.is_shutdown(): + while rclpy.ok(): self.spin_once() self.reset_vars() # Ctrl-C signal interferes with select with the ROS signal handler @@ -173,7 +143,7 @@ def spin(self): pass def spin_once(self): - '''Read data from device and publishes ROS messages.''' + """Read data from device and publishes ROS messages.""" def convert_coords(x, y, z, source, dest=self.frame_local): """Convert the coordinates between ENU, NED, and NWU.""" if source == dest: @@ -234,12 +204,11 @@ def publish_time_ref(secs, nsecs, source): # Doesn't follow the standard publishing pattern since several time # refs could be published simultaneously if self.time_ref_pub is None: - self.time_ref_pub = rospy.Publisher( - 'time_reference', TimeReference, queue_size=10) + self.time_ref_pub = self.create_publisher(TimeReference, 'time_reference', 10) time_ref_msg = TimeReference() time_ref_msg.header = self.h - time_ref_msg.time_ref.secs = secs - time_ref_msg.time_ref.nsecs = nsecs + time_ref_msg.time_ref.sec = secs + time_ref_msg.time_ref.nanosec = int(nsecs) time_ref_msg.source = source self.time_ref_pub.publish(time_ref_msg) @@ -270,13 +239,13 @@ def stamp_from_itow(itow, y=None, m=None, d=None, ns=0, week=None): # MTData def fill_from_RAW(raw_data): - '''Fill messages with information from 'raw' MTData block.''' + """Fill messages with information from 'raw' MTData block.""" # don't publish raw imu data anymore # TODO find what to do with that - rospy.loginfo("Got MTi data packet: 'RAW', ignored!") + self.get_logger().info("Got MTi data packet: 'RAW', ignored!") def fill_from_RAWGPS(rawgps_data): - '''Fill messages with information from 'rawgps' MTData block.''' + """Fill messages with information from 'rawgps' MTData block.""" if rawgps_data['bGPS'] < self.old_bGPS: self.pub_raw_gps = True # LLA @@ -287,14 +256,14 @@ def fill_from_RAWGPS(rawgps_data): self.old_bGPS = rawgps_data['bGPS'] def fill_from_Temp(temp): - '''Fill messages with information from 'temperature' MTData block. - ''' + """Fill messages with information from 'temperature' MTData block. + """ self.pub_temp = True self.temp_msg.temperature = temp def fill_from_Calib(imu_data): - '''Fill messages with information from 'calibrated' MTData block. - ''' + """Fill messages with information from 'calibrated' MTData block. + """ try: self.pub_imu = True x, y, z = imu_data['gyrX'], imu_data['gyrY'], imu_data['gyrZ'] @@ -327,8 +296,8 @@ def fill_from_Calib(imu_data): pass def fill_from_Orient(orient_data): - '''Fill messages with information from 'orientation' MTData block. - ''' + """Fill messages with information from 'orientation' MTData block. + """ self.pub_imu = True if 'quaternion' in orient_data: w, x, y, z = orient_data['quaternion'] @@ -349,7 +318,7 @@ def fill_from_Orient(orient_data): self.imu_msg.orientation_covariance = self.orientation_covariance def fill_from_Auxiliary(aux_data): - '''Fill messages with information from 'Auxiliary' MTData block.''' + """Fill messages with information from 'Auxiliary' MTData block.""" try: self.anin1_msg.data = o['Ain_1'] self.pub_anin1 = True @@ -362,14 +331,14 @@ def fill_from_Auxiliary(aux_data): pass def fill_from_Pos(position_data): - '''Fill messages with information from 'position' MTData block.''' + """Fill messages with information from 'position' MTData block.""" self.pub_pos_gps = True self.pos_gps_msg.latitude = position_data['Lat'] self.pos_gps_msg.longitude = position_data['Lon'] self.pos_gps_msg.altitude = position_data['Alt'] def fill_from_Vel(velocity_data): - '''Fill messages with information from 'velocity' MTData block.''' + """Fill messages with information from 'velocity' MTData block.""" self.pub_vel = True x, y, z = convert_coords( velocity_data['Vel_X'], velocity_data['Vel_Y'], @@ -379,7 +348,7 @@ def fill_from_Vel(velocity_data): self.vel_msg.twist.linear.z = z def fill_from_Stat(status): - '''Fill messages with information from 'status' MTData block.''' + """Fill messages with information from 'status' MTData block.""" self.pub_diag = True if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK @@ -411,19 +380,19 @@ def fill_from_Stat(status): self.pos_gps_msg.status.service = 0 def fill_from_Sample(ts): - '''Catch 'Sample' MTData blocks.''' + """Catch 'Sample' MTData blocks.""" self.h.seq = ts # MTData2 def fill_from_Temperature(o): - '''Fill messages with information from 'Temperature' MTData2 block. - ''' + """Fill messages with information from 'Temperature' MTData2 block. + """ self.pub_temp = True self.temp_msg.temperature = o['Temp'] def fill_from_Timestamp(o): - '''Fill messages with information from 'Timestamp' MTData2 block. - ''' + """Fill messages with information from 'Timestamp' MTData2 block. + """ try: # put timestamp from gps UTC time if available y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\ @@ -456,8 +425,8 @@ def fill_from_Timestamp(o): pass def fill_from_Orientation_Data(o): - '''Fill messages with information from 'Orientation Data' MTData2 - block.''' + """Fill messages with information from 'Orientation Data' MTData2 + block.""" self.pub_imu = True try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] @@ -485,13 +454,13 @@ def fill_from_Orientation_Data(o): self.imu_msg.orientation_covariance = self.orientation_covariance def fill_from_Pressure(o): - '''Fill messages with information from 'Pressure' MTData2 block.''' + """Fill messages with information from 'Pressure' MTData2 block.""" self.press_msg.fluid_pressure = o['Pressure'] self.pub_press = True def fill_from_Acceleration(o): - '''Fill messages with information from 'Acceleration' MTData2 - block.''' + """Fill messages with information from 'Acceleration' MTData2 + block.""" self.pub_imu = True # FIXME not sure we should treat all in that same way @@ -513,7 +482,7 @@ def fill_from_Acceleration(o): self.imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance def fill_from_Position(o): - '''Fill messages with information from 'Position' MTData2 block.''' + """Fill messages with information from 'Position' MTData2 block.""" try: self.pos_gps_msg.latitude = o['lat'] self.pos_gps_msg.longitude = o['lon'] @@ -534,7 +503,7 @@ def fill_from_Position(o): pass def fill_from_GNSS(o): - '''Fill messages with information from 'GNSS' MTData2 block.''' + """Fill messages with information from 'GNSS' MTData2 block.""" try: # PVT # time block itow, y, m, d, ns, f = o['itow'], o['year'], o['month'],\ @@ -566,17 +535,17 @@ def fill_from_GNSS(o): # TODO publish Sat Info def fill_from_Angular_Velocity(o): - '''Fill messages with information from 'Angular Velocity' MTData2 - block.''' + """Fill messages with information from 'Angular Velocity' MTData2 + block.""" try: dqw, dqx, dqy, dqz = (o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3']) - now = rospy.Time.now() + now = self.get_clock().now() if self.last_delta_q_time is None: self.last_delta_q_time = now else: # update rate (filtering so as to account for lag variance) - delta_t = (now - self.last_delta_q_time).to_sec() + delta_t = (now - self.last_delta_q_time).nanoseconds/1e+9 if self.delta_q_rate is None: self.delta_q_rate = 1./delta_t delta_t_filtered = .95/self.delta_q_rate + .05*delta_t @@ -622,7 +591,7 @@ def fill_from_Angular_Velocity(o): pass def fill_from_GPS(o): - '''Fill messages with information from 'GPS' MTData2 block.''' + """Fill messages with information from 'GPS' MTData2 block.""" # TODO DOP try: # SOL x, y, z = o['ecefX'], o['ecefY'], o['ecefZ'] @@ -654,13 +623,13 @@ def fill_from_GPS(o): # TODO publish SV Info def fill_from_SCR(o): - '''Fill messages with information from 'SCR' MTData2 block.''' + """Fill messages with information from 'SCR' MTData2 block.""" # TODO that's raw information pass def fill_from_Analog_In(o): - '''Fill messages with information from 'Analog In' MTData2 block. - ''' + """Fill messages with information from 'Analog In' MTData2 block. + """ try: self.anin1_msg.data = o['analogIn1'] self.pub_anin1 = True @@ -673,7 +642,7 @@ def fill_from_Analog_In(o): pass def fill_from_Magnetic(o): - '''Fill messages with information from 'Magnetic' MTData2 block.''' + """Fill messages with information from 'Magnetic' MTData2 block.""" x, y, z = o['magX'], o['magY'], o['magZ'] self.mag_msg.magnetic_field.x = x self.mag_msg.magnetic_field.y = y @@ -681,7 +650,7 @@ def fill_from_Magnetic(o): self.pub_mag = True def fill_from_Velocity(o): - '''Fill messages with information from 'Velocity' MTData2 block.''' + """Fill messages with information from 'Velocity' MTData2 block.""" x, y, z = convert_coords(o['velX'], o['velY'], o['velZ'], o['frame']) self.vel_msg.twist.linear.x = x @@ -690,7 +659,7 @@ def fill_from_Velocity(o): self.pub_vel = True def fill_from_Status(o): - '''Fill messages with information from 'Status' MTData2 block.''' + """Fill messages with information from 'Status' MTData2 block.""" try: status = o['StatusByte'] fill_from_Stat(status) @@ -713,7 +682,7 @@ def find_handler_name(name): return # common header self.h = Header() - self.h.stamp = rospy.Time.now() + self.h.stamp = self.get_clock().now().to_msg() self.h.frame_id = self.frame_id # set default values @@ -724,79 +693,74 @@ def find_handler_name(name): try: locals()[find_handler_name(n)](o) except KeyError: - rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n) + self.get_logger().warn("Unknown MTi data packet: '%s', ignoring." % n) # publish available information if self.pub_imu: self.imu_msg.header = self.h if self.imu_pub is None: - self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10) + self.imu_pub = self.create_publisher(Imu, 'imu/data', 10) self.imu_pub.publish(self.imu_msg) if self.pub_raw_gps: self.raw_gps_msg.header = self.h if self.raw_gps_pub is None: - self.raw_gps_pub = rospy.Publisher('raw_fix', NavSatFix, queue_size=10) + self.raw_gps_pub = self.create_publisher(NavSatFix, 'raw_fix', 10) self.raw_gps_pub.publish(self.raw_gps_msg) if self.pub_pos_gps: self.pos_gps_msg.header = self.h if self.pos_gps_pub is None: - self.pos_gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10) + self.pos_gps_pub = self.create_publisher(NavSatFix, 'fix', 10) self.pos_gps_pub.publish(self.pos_gps_msg) if self.pub_vel: self.vel_msg.header = self.h if self.vel_pub is None: - self.vel_pub = rospy.Publisher('velocity', TwistStamped, - queue_size=10) + self.vel_pub = self.create_publisher(TwistStamped, 'velocity', 10) self.vel_pub.publish(self.vel_msg) if self.pub_mag: self.mag_msg.header = self.h if self.mag_pub is None: - self.mag_pub = rospy.Publisher('imu/mag', MagneticField, - queue_size=10) + self.mag_pub = self.create_publisher(MagneticField, 'imu/mag', 10) self.mag_pub.publish(self.mag_msg) if self.pub_temp: self.temp_msg.header = self.h if self.temp_pub is None: - self.temp_pub = rospy.Publisher('temperature', Temperature, - queue_size=10) + self.temp_pub = self.create_publisher(Temperature, 'temperature', 10) self.temp_pub.publish(self.temp_msg) if self.pub_press: self.press_msg.header = self.h if self.press_pub is None: - self.press_pub = rospy.Publisher('pressure', FluidPressure, - queue_size=10) + self.press_pub = self.create_publisher(FluidPressure, 'pressure', 10) self.press_pub.publish(self.press_msg) if self.pub_anin1: if self.analog_in1_pub is None: - self.analog_in1_pub = rospy.Publisher('analog_in1', - UInt16, queue_size=10) + self.analog_in1_pub = self.create_publisher(UInt16, 'analog_in1', 10) self.analog_in1_pub.publish(self.anin1_msg) if self.pub_anin2: if self.analog_in2_pub is None: - self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16, - queue_size=10) + self.analog_in2_pub = self.create_publisher(UInt16, 'analog_in2', 10) self.analog_in2_pub.publish(self.anin2_msg) if self.pub_ecef: self.ecef_msg.header = self.h if self.ecef_pub is None: - self.ecef_pub = rospy.Publisher('ecef', PointStamped, - queue_size=10) + self.ecef_pub = self.create_publisher(PointStamped, 'ecef', 10) self.ecef_pub.publish(self.ecef_msg) if self.pub_diag: self.diag_msg.header = self.h if self.diag_pub is None: - self.diag_pub = rospy.Publisher('/diagnostics', - DiagnosticArray, queue_size=10) + self.diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) self.diag_pub.publish(self.diag_msg) # publish string representation - self.str_pub.publish(str(data)) + msg = String() + msg.data = str(data) + self.str_pub.publish(msg) -def main(): - '''Create a ROS node and instantiate the class.''' - rospy.init_node('xsens_driver') +def main(args=None): + """Create a ROS node and instantiate the class.""" + rclpy.init(args=args) driver = XSensDriver() driver.spin() + rclpy.shutdown() if __name__ == '__main__':