add configuration code, add hr topics, fix timestamp error when optio…

…n is sampletimefine.
xsenssupport · Aug 19, 2024 · f21f1ba · f21f1ba
1 parent ea34b02
commit f21f1ba
Show file tree

Hide file tree

Showing 37 changed files with 1,833 additions and 1,083 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,9 @@
+/log/
+/build/
+/install/
+/.vscode/
+/src/.vscode/
+*.o
+*.dpp
+*.d
+*.a
diff --git a/README.md b/README.md
@@ -1,7 +1,7 @@
 
 # Xsens MTi ROS2 Driver and Ntrip Client
 
-This code was based on the [bluespace_ai_xsens_ros_mti_driver](https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver) and the official Movella 2023 [Open source Xsens Device API](https://base.movella.com/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices) tested on MTi-680 with ROS2 Humble at Ubuntu 22.04.3 LTS .
+This code was based on the [bluespace_ai_xsens_ros_mti_driver](https://github.com/bluespace-ai/bluespace_ai_xsens_ros_mti_driver) and the official Movella 2023 [Open source Xsens Device API](https://base.movella.com/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices) tested on MTi-680G/MTi-8 with ROS2 Humble at Ubuntu 22.04.3 LTS .
 
 ## ROS vs ROS2 Versions
 
@@ -24,17 +24,21 @@ Here are the recommended Output Configurations and Device Settings:
 
 ## Changes made to the MTi ROS Driver:
 
- - add ``xsens_time_handler`` , to give user option to use UTC Time from the MTi sensor.
- - add ``ntrip_util``, to support the /nmea topic
- - add ``xsens_log_handler``, to support the ``should_log`` option in the ``xsens_mti_node.yaml``
- - add ``nmeapublisher.h`` under ``src/messagepublisher`` folder, to send GPGGA message, ``/nmea`` rostopic. The ``/nmea`` messages will be generated by PvtData, but if you have enabled SendLatest Time Synchronization option, you won't be able to get PvtData, even there is trigger, in that case, the ``/nnmea`` messages will be generated by ``packet.UtcTime(), packet.status(), packet.latitudeLongitude()``. The ``/nmea`` messages are published at approximately 1Hz.
- - add ``gnssposepublisher.h`` under ``src/messagepublisher`` folder, to send position+orientation in one message, ``/gnss_pose`` rostopic.
- - add ``utctimepublisher.h`` under ``src/messagepublisher`` folder, to send utctime(if available) , ``/imu/utctime`` rostopic. 
- - add ``XsStatusWord.msg``, used to publish ``/status`` topic
-
-change:
- - ``lib/xspublic/xscontroller/iointerface.h``, line 138, change to ``PO_OneStopBIt`` for PO_XsensDefaults.
- - ``lib/xspublic/xscommon/threading.cpp``, updated to work with glibc 2.35 in ubuntu 22 OS.
+ - Fix the fix_type of the ``/nmea`` GPGGA topic to align with NMEA standards.
+ - Add: 
+    - +Sensor ouput configurations; 
+    - +Sensor Filter Settings;
+    - +Setting baudrate; 
+    - +Setting GNSS Lever Arm for MTi-8/MTi-680(G)
+    - +Setting u-Blox GNSS Platform
+    - +Option Flags Settings(AHS,In-Run Compass, Beidou, OrientationSmoother, PositionVelocitySmoother, ContinousZRU); 
+    - +Manual Gyro Bias Estimation Periodically
+    - +Add ``filter/euler`` and high rate topics for ``imu/acceleration_hr``, ``imu/angular_velocity_hr``
+    - +Add error messages.
+
+- change:
+    - ``lib/xspublic/xscontroller/iointerface.h``, line 138, change to ``PO_OneStopBIt`` for PO_XsensDefaults.
+    - ``lib/xspublic/xscommon/threading.cpp``, updated to work with glibc 2.35.
 
 ## Ntrip_Client
 The Ntrip_client subscribes to the ``/nmea`` rostopic from ``xsens_mti_ros2_driver``, and wait until it gets data for maximum 300 sec, it will send GPGGA to the Ntrip Caster(Server) every 1 second.
@@ -55,7 +59,7 @@ sudo apt install ros-humble-mavros-msgs
 
 change the NTRIP credentials/servers/mountpoint in ``src/ntrip/launch/ntrip_launch.py`` to your own one.
 
-clone the source file to your ``ros2_ws``, and run the code below:
+copy the `src` folder to your ``ros2_ws`` folder, and run the code below:
 ```
 cd ~/ros2_ws
 pushd src/xsens_mti_ros2_driver/lib/xspublic && make && popd
@@ -106,7 +110,8 @@ or ``ros2 topic echo /status`` to check the RTK Fix type, it should be 1(RTK Flo
 | filter/free_acceleration | geometry_msgs/Vector3Stamped    | free acceleration from filter, which is the acceleration in the local earth coordinate system (L) from which<br>the local gravity is deducted | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | filter/positionlla       | geometry_msgs/Vector3Stamped    | filtered position output in latitude (x), longitude (y) and altitude (z) as Vector3, in WGS84 datum                                           | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | filter/quaternion        | geometry_msgs/QuaternionStamped | quaternion from filter                                                                                                                        | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
-| filter/twist             | geometry_msgs/TwistStamped      | velocity and angular velocity                                                                                                                 | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
+| filter/euler        | geometry_msgs/Vector3Stamped | euler(roll,pitch,yaw) from filter                                                                                                                        | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
+| filter/twist             | geometry_msgs/TwistStamped      | filtered velocity and calibrated angular velocity                                                                                                                 | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | filter/velocity          | geometry_msgs/Vector3Stamped    | filtered velocity output as Vector3                                                                                                           | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | gnss                     | sensor_msgs/NavSatFix           | raw 4 Hz latitude, longitude, altitude and status data from GNSS receiver                                                                     | 4Hz                                                                             |
 | gnss_pose                | geometry_msgs/PoseStamped       | filtered position output in latitude (x), longitude (y) and altitude (z) as Vector3 in WGS84 datum, and quaternion from filter                | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
@@ -115,13 +120,23 @@ or ``ros2 topic echo /status`` to check the RTK Fix type, it should be 1(RTK Flo
 | imu/data                 | sensor_msgs/Imu                 | quaternion, calibrated angular velocity and acceleration                                                                                      | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | imu/dq                   | geometry_msgs/QuaternionStamped | integrated angular velocity from sensor (in quaternion representation)                                                                        | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | imu/dv                   | geometry_msgs/Vector3Stamped    | integrated acceleration from sensor                                                                                                           | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
-| imu/mag                  | sensor_msgs/MagneticField     | calibrated magnetic field                                                                                                                     | 1-100Hz                                                                         |
+| imu/mag                  | sensor_msgs/MagneticField    | calibrated magnetic field                                                                                                                     | 1-100Hz                                                                         |
 | imu/time_ref             | sensor_msgs/TimeReference       | SampleTimeFine timestamp from device                                                                                                          | depending on packet                                                             |
 | imu/utctime              | sensor_msgs/TimeReference       | UTC Time from the device                                                                                                                      | depending on packet                                                             |
-| nmea                     | nmea_msgs/Sentence              | 1Hz GPGGA data from GNSS receiver PVTData(if available), otherwise from filtered position, utctime, status packet                             | 1Hz                                                                             |
+| nmea                     | nmea_msgs/Sentence              | 4Hz GPGGA data from GNSS receiver PVTData(if available) and StatusWord                             | 4Hz                                                                             |
 | pressure                 | sensor_msgs/FluidPressure       | barometric pressure from device                                                                                                               | 1-100Hz                                                                         |
-| status                   | xsens_mti_ros2_driver/msg/XsStatusWord | statusWord, 32bit                                                                                                                             | depending on packet                                                             |
+| status                   | xsens_mti_driver/XsStatusWord | statusWord, 32bit                                                                                                                             | depending on packet                                                             |
 | temperature              | sensor_msgs/Temperature         | temperature from device                                                                                                                       | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
 | tf                       | geometry_msgs/TransformStamped  | transformed orientation                                                                                                                       | 1-400Hz(MTi-600 and MTi-100 series), 1-100Hz(MTi-1 series)                      |
+| imu/acceleration_hr         | geometry_msgs/Vector3Stamped    | high rate acceleration                                                                                                                       | see xsens_mti_node.yaml                      |
+| imu/angular_velocity_hr     | geometry_msgs/Vector3Stamped    | high rate angular velocity                                                                                                                   | see xsens_mti_node.yaml                      |
 
 Please refer to [MTi Family Reference Manual](https://mtidocs.movella.com/mti-system-overview) for detailed definition of data. 
+
+
+
+## Troubleshooting
+
+- Refer to the [README.txt](./src/xsens_mti_ros2_driver/README.txt)
+- nVidia Jetson devices, ref to [Interfacing MTi devices with the NVIDIA Jetson](https://base.movella.com/s/article/article/Interfacing-MTi-devices-with-the-NVIDIA-Jetson-1605870420176) 
+- Docs, ref code: [All MTi Related Documentation Links](https://base.movella.com/s/article/All-MTi-Related-Documentation-Links)
diff --git a/src/xsens_mti_ros2_driver/README.txt b/src/xsens_mti_ros2_driver/README.txt
@@ -9,7 +9,7 @@ Prerequisites:
     - C++11
 
 Building:
-    - Copy xsens_ros2_mti_driver folder into your colcon workspace 'src' folder, if you have the RTK GNSS/INS sensor model like MTi-8 or MTi-680/680G, please also copy the ntrip folder.
+    - Copy xsens_ros2_mti_driver folder and ntrip folder(for MTi-680(G)/MTi-8 only) into your colcon workspace 'src' folder, if you have the RTK GNSS/INS sensor model like MTi-8 or MTi-680/680G, please also copy the ntrip folder.
         Make sure the permissions are set to o+rw on your files and directories.
         $ sudo chmod -R o+rw xsens_ros2_mti_driver/
 
@@ -26,7 +26,7 @@ Running:
     - Configure your MTi device to output desired data (e.g. for display example - orientation output)
 
     - Launch the Xsens MTi driver from your colcon workspace:
-            $ ros2 launch xsens_mti_ros2_driver display.launch.py
+            $ ros2 launch xsens_mti_ros2_driver xsens_mti_node.launch.py
 
         After the device has been detected, you can communicate with it from another process / terminal window.
         For example:
@@ -69,36 +69,6 @@ Running:
         compile the module:
 	   $sudo apt-get install linux-headers-$(uname -r)
 
-        For Linux Kernal version >5.3(15 September 2019), check your version `uname -r`, change this line in the `Makefile` of xsens_mt:
-	   ```make
-	   $(MAKE) -C $(KDIR) SUBDIRS=$(PWD) modules
-	   ```
-	   To:
-	   ```make
-	   $(MAKE) -C $(KDIR) M=$(PWD) modules
-	   ```
-	 Then:
-	   $sudo make HAVE_LIBUSB=1
-	   $sudo modprobe usbserial
-	   $sudo insmod ./xsens_mt.ko
-
-    - The MTi-10/20/30 or MTi-100/200/300 or MTi-G-710 is not recognized.
-        $ git clone https://github.com/xsens/xsens_mt.git
-        $ cd xsens_mt
-
-        You may need to install the kernel headers for 
-        your running kernel version to ensure that you have all the necessary files to 
-        compile the module:
-	   $sudo apt-get install linux-headers-$(uname -r)
-
-        For Linux Kernal version >5.3(15 September 2019), check your version `uname -r`, change this line in the `Makefile` of xsens_mt:
-	   ```make
-	   $(MAKE) -C $(KDIR) SUBDIRS=$(PWD) modules
-	   ```
-	   To:
-	   ```make
-	   $(MAKE) -C $(KDIR) M=$(PWD) modules
-	   ```
 	 Then:
 	   $sudo make HAVE_LIBUSB=1
 	   $sudo modprobe usbserial

diff --git a/src/xsens_mti_ros2_driver/include/xdacallback.h b/src/xsens_mti_ros2_driver/include/xdacallback.h
@@ -28,36 +28,7 @@
 //  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
 //  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
 //  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
-//  
-// BSD 3-Clause License
-//
-// Copyright (c) 2021, BlueSpace.ai, Inc.
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// 1. Redistributions of source code must retain the above copyright notice, this
-//    list of conditions and the following disclaimer.
-//
-// 2. Redistributions in binary form must reproduce the above copyright notice,
-//    this list of conditions and the following disclaimer in the documentation
-//    and/or other materials provided with the distribution.
-//
-// 3. Neither the name of the copyright holder nor the names of its
-//    contributors may be used to endorse or promote products derived from
-//    this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
 
 #ifndef XDACALLBACK_H
 #define XDACALLBACK_H
@@ -77,20 +48,21 @@ typedef std::pair<rclcpp::Time, XsDataPacket> RosXsDataPacket;
 class XdaCallback : public XsCallback
 {
 public:
-	XdaCallback(rclcpp::Node& node, size_t maxBufferSize = 5);
+	XdaCallback(rclcpp::Node::SharedPtr node, size_t maxBufferSize = 5);
 	virtual ~XdaCallback() throw();
 
 	RosXsDataPacket next(const std::chrono::milliseconds &timeout);
 
 protected:
 	void onLiveDataAvailable(XsDevice *, const XsDataPacket *packet) override;
+	void onError(XsDevice *, XsResultValue error) override;
 
 private:
 	std::mutex m_mutex;
 	std::condition_variable m_condition;
 	std::list<RosXsDataPacket> m_buffer;
 	size_t m_maxBufferSize;
-	rclcpp::Node& parent_node;
+	rclcpp::Node::SharedPtr parent_node;
 
 	XsensTimeHandler m_timeHandler;
 };

diff --git a/src/xsens_mti_ros2_driver/include/xdainterface.h b/src/xsens_mti_ros2_driver/include/xdainterface.h
@@ -29,35 +29,6 @@
 //  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
 //  
 
-// BSD 3-Clause License
-//
-// Copyright (c) 2021, BlueSpace.ai, Inc.
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// 1. Redistributions of source code must retain the above copyright notice, this
-//    list of conditions and the following disclaimer.
-//
-// 2. Redistributions in binary form must reproduce the above copyright notice,
-//    this list of conditions and the following disclaimer in the documentation
-//    and/or other materials provided with the distribution.
-//
-// 3. Neither the name of the copyright holder nor the names of its
-//    contributors may be used to endorse or promote products derived from
-//    this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifndef XDAINTERFACE_H
 #define XDAINTERFACE_H
@@ -66,19 +37,21 @@
 #include <mavros_msgs/msg/rtcm.hpp>
 #include "xdacallback.h"
 #include <xstypes/xsportinfo.h>
+#include <xstypes/xsstring.h>
 
 #include <chrono>
 
 struct XsControl;
 struct XsDevice;
-
+struct XsString;
+struct XsPortInfo;
 
 class PacketCallback;
 
-class XdaInterface : public rclcpp::Node
+class XdaInterface
 {
 public:
-	explicit XdaInterface(const std::string &node_name, const rclcpp::NodeOptions &options=rclcpp::NodeOptions());
+	explicit XdaInterface(rclcpp::Node::SharedPtr node);
 	~XdaInterface();
 
 	void spinFor(std::chrono::milliseconds timeout);
@@ -89,21 +62,25 @@ class XdaInterface : public rclcpp::Node
 	bool prepare();
 	void close();
 
-	void callbackGyroBiasEstimation();
-	bool manualGyroBiasEstimation(uint16_t duration);
-
-	const int XS_DEFAULT_BAUDRATE = 115200;
+	void setupManualGyroBiasEstimation();
 
 private:
 	void registerCallback(PacketCallback *cb);
 	bool handleError(std::string error);
 	void declareCommonParameters();
+	bool configureSensorSettings();
+	bool manualGyroBiasEstimation(uint16_t duration);
 
 	XsControl *m_control;
 	XsDevice *m_device;
+	XsString m_productCode;
 	XsPortInfo m_port;
 	XdaCallback m_xdaCallback;
 	std::list<PacketCallback *> m_callbacks;
+	rclcpp::Node::SharedPtr m_node; 
+	// Timer for Manual Gyro Bias Estimation
+	rclcpp::TimerBase::SharedPtr m_manualGyroBiasTimer;
+	rclcpp::Subscription<mavros_msgs::msg::RTCM>::SharedPtr m_rtcmSubscription;
 };
 
-#endif
+#endif