Allow to use usb port number to connect to devices

realsense2_camera/include/realsense_node_factory.h

@@ -68,6 +68,7 @@ namespace realsense2_camera
         std::unique_ptr<InterfaceRealSenseNode> _realSenseNode;
         rs2::context _ctx;
         std::string _serial_no;
+        std::string _port_no;
         bool _initial_reset;
         std::thread _query_thread;
 

realsense2_camera/launch/includes/nodelet.launch.xml

@@ -2,6 +2,7 @@
   <arg name="external_manager"    default="false"/>
   <arg name="manager"             default="realsense2_camera_manager"/>
   <arg name="serial_no"           default=""/>
+  <arg name="port_no"           default=""/>
   <arg name="tf_prefix"           default=""/>
   <arg name="json_file_path"      default=""/>
   <arg name="rosbag_filename"     default=""/>
@@ -91,6 +92,7 @@
   <node unless="$(arg external_manager)" pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen" required="$(arg required)"/>
   <node pkg="nodelet" type="nodelet" name="realsense2_camera" args="load realsense2_camera/RealSenseNodeFactory $(arg manager)" required="$(arg required)">
     <param name="serial_no"                type="str"  value="$(arg serial_no)"/>
+    <param name="port_no"                  type="str"  value="$(arg port_no)"/>
     <param name="json_file_path"           type="str"  value="$(arg json_file_path)"/>
     <param name="rosbag_filename"          type="str"  value="$(arg rosbag_filename)"/>
 

realsense2_camera/launch/rs_rgbd.launch

@@ -48,6 +48,7 @@ Processing enabled by this node:
   <!-- Camera device specific arguments -->
 
   <arg name="serial_no"           default=""/>
+  <arg name="port_no"             default=""/>
   <arg name="json_file_path"      default=""/>
 
   <arg name="fisheye_width"       default="640"/>
@@ -117,6 +118,7 @@ Processing enabled by this node:
       <arg name="manager"                  value="$(arg manager)"/>
       <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
       <arg name="serial_no"                value="$(arg serial_no)"/>
+      <arg name="port_no"                  value="$(arg port_no)"/>
       <arg name="json_file_path"           value="$(arg json_file_path)"/>
 
       <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>

realsense2_camera/launch/rs_t265.launch

@@ -6,6 +6,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib
 -->
 <launch>
   <arg name="serial_no"           default=""/>
+  <arg name="port_no"             default=""/>
   <arg name="json_file_path"      default=""/>
   <arg name="camera"              default="camera"/>
   <arg name="tf_prefix"           default="$(arg camera)"/>
@@ -34,6 +35,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
       <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
       <arg name="serial_no"                value="$(arg serial_no)"/>
+      <arg name="port_no"                  value="$(arg port_no)"/>
       <arg name="json_file_path"           value="$(arg json_file_path)"/>
 
       <arg name="enable_sync"              value="$(arg enable_sync)"/>

realsense2_camera/src/base_realsense_node.cpp

@@ -649,6 +649,10 @@ void BaseRealSenseNode::setupDevice()
 
         ROS_INFO_STREAM("Device Serial No: " << _serial_no);
 
+        auto camera_id = _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
+
+        ROS_INFO_STREAM("Device physical port: " << camera_id);
+
         auto fw_ver = _dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
         ROS_INFO_STREAM("Device FW version: " << fw_ver);
 

realsense2_camera/src/realsense_node_factory.cpp

@@ -12,6 +12,8 @@
 #include <thread>
 #include <sys/time.h>
 
+#include <boost/algorithm/string.hpp>
+
 using namespace realsense2_camera;
 
 #define REALSENSE_ROS_EMBEDDED_VERSION_STR (VAR_ARG_STRING(VERSION: REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION))
@@ -57,22 +59,55 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
 		else
 		{
 			bool found = false;
+      ROS_INFO_STREAM(" ");
 			for (auto&& dev : list)
 			{
 				auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-				ROS_DEBUG_STREAM("Device with serial number " << sn << " was found.");
-				if (_serial_no.empty() || sn == _serial_no)
+        auto pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
+        auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
+        ROS_INFO_STREAM("Device with physical ID " << pn << " was found.");
+        std::string port_id;
+        std::vector<std::string> results;
+        ROS_INFO_STREAM("Device with name " << name << " was found.");
+        if(strcmp(name,"Intel RealSense T265") == 0)
+        {
+          ROS_DEBUG_STREAM("T265 was found!");
+          boost::split(results, pn, [](char c){return c == ' ';});
+          std::string bus_line = results[2];
+          std::string port_line = results[3];
+          boost::split(results, bus_line, [](char c){return c == '_';});
+          std::string bus_no = results[1];
+          boost::split(results, port_line, [](char c){return c == '_';});
+          std::string port_no = results[1];
+          port_id = bus_no +"-"+ port_no;
+        }
+        else// if(strcmp(name, "Intel RealSense D435") == 0)
+        {
+          //ROS_DEBUG_STREAM("D345 was found!");
+          boost::split(results, pn, [](char c){return c == ':';});
+          std::string bus_line = results[3];
+          boost::split(results, bus_line, [](char c){return c == '/';});
+          std::string port_line = results[results.size()-1];
+          boost::replace_all(port_line,".","-");
+          port_id = port_line;
+        }
+        ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
+
+
+        ROS_INFO_STREAM("Device with serial number " << sn << " was found."<<std::endl);
+        if ((_serial_no.empty() || sn == _serial_no) && (_port_no.empty() || port_id == _port_no))
 				{
 					_device = dev;
 					_serial_no = sn;
 					found = true;
 					break;
-				}
+        }
 			}
 			if (!found)
 			{
 				// T265 could be caught by another node.
 				ROS_ERROR_STREAM("The requested device with serial number " << _serial_no << " is NOT found. Will Try again.");
+        ROS_ERROR_STREAM("The requested device with port number " << _port_no << " is NOT found. Will Try again.");
 			}
 		}
 	}
@@ -135,6 +170,7 @@ void RealSenseNodeFactory::onInit()
 		ros::NodeHandle nh = getNodeHandle();
 		auto privateNh = getPrivateNodeHandle();
 		privateNh.param("serial_no", _serial_no, std::string(""));
+    privateNh.param("port_no", _port_no, std::string(""));
 
 		std::string rosbag_filename("");
 		privateNh.param("rosbag_filename", rosbag_filename, std::string(""));