diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index b7bc536512..b81977711f 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -68,6 +68,7 @@ namespace realsense2_camera std::unique_ptr _realSenseNode; rs2::context _ctx; std::string _serial_no; + std::string _port_no; bool _initial_reset; std::thread _query_thread; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 8fab55e464..60c20fe2db 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -2,6 +2,7 @@ + @@ -91,6 +92,7 @@ + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index 835143c881..aaff237bfb 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -48,6 +48,7 @@ Processing enabled by this node: + @@ -117,6 +118,7 @@ Processing enabled by this node: + diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index 8f3b135a03..9d7ce67074 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -6,6 +6,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib --> + @@ -34,6 +35,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2897de936d..2f29cdcb74 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/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); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index fc05d65588..55254dc6b7 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -12,6 +12,8 @@ #include #include +#include + 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 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."<