Merge pull request #26 from gsokoll/ros2

Added timeout parameter

Author: twdragon

.gitignore

@@ -26,6 +26,7 @@ documentation
 Documentation
 .idea
 # Visual Studio files
+.vscode
 .vs
 *.db
 *.pdb

README.md

@@ -35,6 +35,7 @@ Configuration of the node is done by default via the configuration YAML file [`c
 - `port` - the virtual kernel device name for a port, `ttyUSB0` by default
 - `baud_rate` - port rate value to be used by the library for opening the port, _9600 baud_ by default
 - `polling_interval` - the sensor polling interval in milliseconds. If this parameter is omitted, the default value is set up by the library (50 ms).
+- `timeout_ms` - the sensor timeout period in milliseconds.  If no data is received from the sensor after this period, then an error is raised and the node terminates. If this parameter is omitted, a default value of 3 times the polling interval is used.  If this parameter is zero, the timeout check is disabled.
 - `restart_service_name` - the service name used to restart the sensor connection after an error.
 - `imu_publisher:`
     - `topic_name` - the topic name for IMU data publisher, `imu` in the node's namespace by default

config/config.yml

@@ -3,6 +3,7 @@ witmotion:
     port: ttyUSB0
     baud_rate: 9600 # baud
     polling_interval: 50 # ms
+    timeout_ms: 150 # ms
     restart_service_name: /restart_imu
     imu_publisher:
       topic_name: /imu

config/wt31n.yml

@@ -3,6 +3,7 @@ witmotion_ros:
     port: ttyUSB3
     baud_rate: 115200 # baud
     polling_interval: 10 # ms
+    timeout_ms: 150 # ms
     restart_service_name: /restart_imu
     imu_publisher:
       topic_name: /imu

config/wt61c.yml

@@ -4,6 +4,7 @@ witmotion:
     #port: /dev/ttyUSB0
     baud_rate: 115200 # baud
     polling_interval: 25 # ms
+    timeout_ms: 150 # ms
     restart_service_name: /restart_imu
     imu_publisher:
       topic_name: /imu_data

config/wt901.yml

@@ -2,7 +2,8 @@ witmotion:
   ros__parameters:
     port: ttyUSB0
     baud_rate: 115200 # baud
-    polling_interval: 10 # ms
+    polling_interval: 50 # ms
+    timeout_ms: 150 # ms
     restart_service_name: /restart_imu
     imu_publisher:
       topic_name: /imu

include/witmotion/witmotion_ros.h

@@ -44,6 +44,7 @@ class ROSWitmotionSensorController: public QObject, public rclcpp::Node
     std::string port_name;
     QSerialPort::BaudRate port_rate;
     uint32_t interval;
+    uint32_t timeout_ms;
     QThread reader_thread;
     QBaseSerialWitmotionSensorReader* reader;
     static bool suspended;

include/witmotion_ros.h

@@ -47,6 +47,7 @@ class ROSWitmotionSensorController: public QObject
     std::string port_name;
     QSerialPort::BaudRate port_rate;
     uint32_t interval;
+    uint32_t timeout_ms;
     QThread reader_thread;
     QBaseSerialWitmotionSensorReader* reader;
     static bool suspended;

launch/witmotion.py launch/witmotion.launch.py

@@ -8,13 +8,13 @@ def generate_launch_description():
     ld = LaunchDescription()
 
     config = os.path.join(
-        get_package_share_directory('witmotion'),
+        get_package_share_directory('witmotion_ros'),
         'config',
         'witmotion.yml'
         )
 
     node=Node(
-        package = 'witmotion',
+        package = 'witmotion_ros',
         executable = 'witmotion_ros_node',
         parameters = [config]
     )

launch/wt31n.py launch/wt31n.launch.py

@@ -8,13 +8,13 @@ def generate_launch_description():
     ld = LaunchDescription()
 
     config = os.path.join(
-        get_package_share_directory('witmotion'),
+        get_package_share_directory('witmotion_ros'),
         'config',
         'wt31n.yml'
         )
 
     node=Node(
-        package = 'witmotion',
+        package = 'witmotion_ros',
         executable = 'witmotion_ros_node',
         parameters = [config]
     )

launch/wt61c.py launch/wt61c.launch.py

@@ -8,13 +8,13 @@ def generate_launch_description():
     ld = LaunchDescription()
 
     config = os.path.join(
-        get_package_share_directory('witmotion'),
+        get_package_share_directory('witmotion_ros'),
         'config',
         'wt61c.yml'
         )
 
     node=Node(
-        package = 'witmotion',
+        package = 'witmotion_ros',
         executable = 'witmotion_ros_node',
         parameters = [config]
     )

launch/wt901.py launch/wt901.launch.py

@@ -8,13 +8,13 @@ def generate_launch_description():
     ld = LaunchDescription()
 
     config = os.path.join(
-        get_package_share_directory('witmotion'),
+        get_package_share_directory('witmotion_ros'),
         'config',
         'wt901.yml'
         )
 
     node=Node(
-        package = 'witmotion',
+        package = 'witmotion_ros',
         executable = 'witmotion_ros_node',
         parameters = [config]
     )

src/witmotion_ros.cpp

@@ -440,14 +440,21 @@ ROSWitmotionSensorController::ROSWitmotionSensorController()
                                                 port_rate);
 
   int int_interval;
-  
   node->declare_parameter("polling_interval", 10);
   int_interval = node->get_parameter("polling_interval")
                        .get_parameter_value()
                        .get<int>();
   interval = static_cast<uint32_t>(int_interval);
   reader->SetSensorPollInterval(interval);
 
+  int int_timeout_ms;
+  node->declare_parameter("timeout_ms", 1000);
+  int_timeout_ms = node->get_parameter("timeout_ms")
+                       .get_parameter_value()
+                       .get<int>();
+  timeout_ms = static_cast<uint32_t>(int_timeout_ms);
+  reader->SetSensorTimeout(int_timeout_ms);
+
   reader->ValidatePackets(true);
   reader->moveToThread(&reader_thread);
   connect(&reader_thread, &QThread::finished, reader, &QObject::deleteLater);