Fix serial parameter not taken into account.

sam-xl · May 20, 2022 · ab4ae6e · ab4ae6e
1 parent 1cc0f54
commit ab4ae6e
Showing 1 changed file with 10 additions and 4 deletions.
diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp
@@ -14,7 +14,13 @@ namespace scancontrol_driver
         private_nh_.param("resolution", config_.resolution, -1);
 
         // Multiple device parameters
-        private_nh_.param("serial", config_.serial, std::string(""));
+        if (!private_nh_.getParam("serial", config_.serial)) {
+            // When using rosrun the serial number is interpreted as an int and
+            // can't be forced into a string. See https://github.com/ros/ros_comm/issues/1339
+            int serial_int;
+            if (private_nh_.getParam("serial", serial_int))
+                config_.serial = std::to_string(serial_int);
+        }
         private_nh_.param("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID));
         private_nh_.param("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME));
 
@@ -64,7 +70,7 @@ namespace scancontrol_driver
 
             // Check if the available device is the same as the prefered device (if a serial is provided):
             std::string interface(available_interfaces[0]);
-            if ((config_.serial == "") || (interface.find(config_.serial) > -1)){
+            if ((config_.serial == "") || (interface.find(config_.serial) != std::string::npos)){
                 ROS_INFO_STREAM("Interface found: " << interface);
             }
             else{
@@ -78,7 +84,7 @@ namespace scancontrol_driver
             if (config_.serial != ""){
                 for (int i = 0; i < interface_count; i++){
                     std::string interface(available_interfaces[i]);
-                    if (interface.find(config_.serial) > -1){
+                    if (interface.find(config_.serial) != std::string::npos){
                         ROS_INFO_STREAM("Interface found: " << interface);
                         selected_interface = i;
                         break;
@@ -163,7 +169,7 @@ namespace scancontrol_driver
             gint8 selected_resolution = -1;
             for (int i = 0; i < return_code; i++){
                 std::string resolution = std::to_string(available_resolutions[i]);
-                if (resolution.find(config_.serial) > -1){
+                if (resolution.find(config_.serial) != std::string::npos){
                     selected_resolution = i;
                     break;
                 }