Merge pull request #1248 from doronhi/support_L515

Add new L515 PID
IntelRealSense · Jun 18, 2020 · eb7e240 · eb7e240
2 parents 8cadbfc + 1cc944b
commit eb7e240
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 15 deletions.
diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h
@@ -34,7 +34,8 @@ namespace realsense2_camera
     const uint16_t RS435i_RGB_PID   = 0x0B3A; // AWGC_MM
     const uint16_t RS405_PID        = 0x0b0c; // DS5U
     const uint16_t RS_T265_PID      = 0x0b37; // 
-    const uint16_t RS_L515_PID      = 0x0B3D; // 
+    const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // 
+    const uint16_t RS_L515_PID      = 0x0B64; // 
 
 
     const bool ALIGN_DEPTH    = false;

diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h
@@ -25,6 +25,7 @@ namespace realsense2_camera
 {
     const stream_index_pair COLOR{RS2_STREAM_COLOR, 0};
     const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0};
+    const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0};
     const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1};
     const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2};
     const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0};
@@ -35,7 +36,7 @@ namespace realsense2_camera
     const stream_index_pair POSE{RS2_STREAM_POSE, 0};
 
 
-    const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA1, INFRA2,
+    const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2,
                                                           COLOR,
                                                           FISHEYE,
                                                           FISHEYE1, FISHEYE2};

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -705,37 +705,34 @@ void BaseRealSenseNode::setupDevice()
         ROS_INFO_STREAM("Device Sensors: ");
         for(auto&& sensor : _dev_sensors)
         {
+            for (auto& profile : sensor.get_stream_profiles())
+            {
+                auto video_profile = profile.as<rs2::video_stream_profile>();
+                stream_index_pair sip(video_profile.stream_type(), video_profile.stream_index());
+                if (_sensors.find( sip ) != _sensors.end())
+                    continue;
+                _sensors[sip] = sensor;
+            }
+
             std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME);
             if (sensor.is<rs2::depth_sensor>())
             {
-                _sensors[DEPTH] = sensor;
-                _sensors[INFRA1] = sensor;
-                _sensors[INFRA2] = sensor;
                 _sensors_callback[module_name] = frame_callback_function;
             }
             else if (sensor.is<rs2::color_sensor>())
             {
-                _sensors[COLOR] = sensor;
                 _sensors_callback[module_name] = frame_callback_function;
             }
             else if (sensor.is<rs2::fisheye_sensor>())
             {
-                _sensors[FISHEYE] = sensor;
                 _sensors_callback[module_name] = frame_callback_function;
             }
             else if (sensor.is<rs2::motion_sensor>())
             {
-                _sensors[GYRO] = sensor;
-                _sensors[ACCEL] = sensor;
                 _sensors_callback[module_name] = imu_callback_function;
             }
             else if (sensor.is<rs2::pose_sensor>())
             {
-                _sensors[GYRO] = sensor;
-                _sensors[ACCEL] = sensor;
-                _sensors[POSE] = sensor;
-                _sensors[FISHEYE1] = sensor;
-                _sensors[FISHEYE2] = sensor;
                 _sensors_callback[module_name] = multiple_message_callback_function;
             }
             else
@@ -1813,7 +1810,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& s
     {
         if (!strcmp(e.what(), "Requested extrinsics are not available!"))
         {
-            ROS_WARN_STREAM(e.what() << " : using unity as default.");
+            ROS_WARN_STREAM("(" << rs2_stream_to_string(stream.first) << ", " << stream.second << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default.");
             ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}});
         }
         else