ethz-asl · simonpierredeschenes · Sep 28, 2022 · Sep 29, 2022 · Sep 29, 2022 · Sep 29, 2022
diff --git 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
diff --git 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
diff --git a/launch/xsens_driver.launch.xml b/launch/xsens_driver.launch.xml
@@ -0,0 +1,27 @@
+<launch>
+    <!-- parameters -->
+    <arg name="device" default="auto"/>
+    <arg name="baudrate" default="0"/>
+    <arg name="timeout" default="0.002"/>
+    <arg name="initial_wait" default="0.1"/>
+    <arg name="frame_id" default="/imu"/>
+    <arg name="frame_local" default="ENU"/>
+    <arg name="no_rotation_duration" default="0"/>
+    <arg name="angular_velocity_covariance_diagonal" default="[0.0004, 0.0004, 0.0004]"/>
+    <arg name="linear_acceleration_covariance_diagonal" default="[0.0004, 0.0004, 0.0004]"/>
+    <arg name="orientation_covariance_diagonal" default="[0.01745, 0.01745, 0.15708]"/>
+
+    <!-- node -->
+	<node pkg="xsens_driver" exec="mtnode.py" name="xsens_driver" output="screen" >
+		<param name="device" value="$(var device)"/>
+		<param name="baudrate" value="$(var baudrate)"/>
+		<param name="timeout" value="$(var timeout)"/>
+		<param name="initial_wait" value="$(var initial_wait)"/>
+		<param name="frame_id" value="$(var frame_id)"/>
+		<param name="frame_local" value="$(var frame_local)"/>
+		<param name="no_rotation_duration" value="$(var no_rotation_duration)"/>
+		<param name="angular_velocity_covariance_diagonal" value="$(var angular_velocity_covariance_diagonal)"/>
+		<param name="linear_acceleration_covariance_diagonal" value="$(var linear_acceleration_covariance_diagonal)"/>
+		<param name="orientation_covariance_diagonal" value="$(var orientation_covariance_diagonal)"/>
+	</node>
+</launch>
diff --git 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 (<a href="http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html">sensor_msgs::Imu</a>): orientation, angular velocity, linear acceleration,
 - \c /fix (<a href="http://www.ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html">sensor_msgs::NavSatFix</a>): longitude, latitude, altitude (from filtered/fused GPS and IMU),
 - \c /raw_fix (<a href="http://www.ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html">sensor_msgs::NavSatFix</a>): longitude, latitude, altitude (from ONLY GPS),
@@ -29,16 +29,16 @@ The covariance information in the <a href="http://www.ros.org/doc/api/sensor_msg
 \section params Parameters
 
 The nodes can take the following parameters:
-- \a ~device (\c auto): the path of the device file to connect to the imu; \c auto will look through all serial devices to find the first one.
-- \a ~baudrate (0): the baudrate of the IMU (unused for \c auto device); 0 will try to auto-detect baudrate.
-- \a ~timeout (0.002): the timeout for communication with the IMU.
-- \a ~initial_wait (0.1): initial wait to allow the device to come up before trying to communicate.
-- \a ~frame_id (\c /base_imu): the frame id of the IMU.
-- \a ~frame_local (\c ENU): the desired frame orientation (\c ENU, \c NED, or \c NWU).
-- \a ~no_rotation_duration (0): the int duration in seconds of the no-rotation calibration procedure at the start of the node (only for mark iv devices). If 0, the procedure is not launched.
-- \a ~angular_velocity_covariance_diagonal ([0.0004, 0.0004, 0.0004]) diagonal elements of the covariance matrix on the angular velocity published on the \c /imu/data topic.
-- \a ~linear_acceleration_covariance_diagonal ([0.0004, 0.0004, 0.0004]) diagonal elements of the covariance matrix on the linear acceleration published on the \c /imu/data topic.
-- \a ~orientation_covariance_diagonal ([0.01745, 0.01745, 0.15708]) diagonal elements of the covariance matrix on the orientation published on the \c /imu/data topic.
+- \a device (\c auto): the path of the device file to connect to the imu; \c auto will look through all serial devices to find the first one.
+- \a baudrate (0): the baudrate of the IMU (unused for \c auto device); 0 will try to auto-detect baudrate.
+- \a timeout (0.002): the timeout for communication with the IMU.
+- \a initial_wait (0.1): initial wait to allow the device to come up before trying to communicate.
+- \a frame_id (\c /base_imu): the frame id of the IMU.
+- \a frame_local (\c ENU): the desired frame orientation (\c ENU, \c NED, or \c NWU).
+- \a no_rotation_duration (0): the int duration in seconds of the no-rotation calibration procedure at the start of the node (only for mark iv devices). If 0, the procedure is not launched.
+- \a angular_velocity_covariance_diagonal ([0.0004, 0.0004, 0.0004]) diagonal elements of the covariance matrix on the angular velocity published on the \c /imu/data topic.
+- \a linear_acceleration_covariance_diagonal ([0.0004, 0.0004, 0.0004]) diagonal elements of the covariance matrix on the linear acceleration published on the \c /imu/data topic.
+- \a orientation_covariance_diagonal ([0.01745, 0.01745, 0.15708]) diagonal elements of the covariance matrix on the orientation published on the \c /imu/data topic.
 
 \section dialout Permissions
 

diff --git a/package.xml b/package.xml
@@ -1,28 +1,21 @@
-<package>
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>xsens_driver</name>
-  <version>2.2.2</version>
-  <description>
-  ROS Driver for XSens MT/MTi/MTi-G devices.
-  </description>
+  <version>3.0.0</version>
+  <description>ROS 2 driver for XSens MT/MTi/MTi-G devices.</description>
   <maintainer email="[email protected]">Francis Colas</maintainer>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>python-serial</depend>
+  <depend>tf_transformations</depend>
 
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>diagnostic_msgs</build_depend>
-
-  <run_depend>rospy</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>diagnostic_msgs</run_depend>
-  <run_depend>python-serial</run_depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 </package>
-
-
diff --git a/resource/xsens_driver b/resource/xsens_driver
diff --git a/setup.cfg 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
@@ -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='[email protected]',
+    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
diff --git a/nodes/mtdef.py → xsens_driver/mtdef.py b/nodes/mtdef.py → 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 ''