Merge remote-tracking branch 'origin/development' into development

IntelRealSense · Jun 18, 2020 · c769595 · c769595
2 parents f260f31 + 4b33439
commit c769595
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 18 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
@@ -242,14 +242,18 @@ bool is_checkbox(rs2::options sensor, rs2_option option)
 
 bool is_enum_option(rs2::options sensor, rs2_option option)
 {
+    static const int MAX_ENUM_OPTION_VALUES(100);
+    static const float EPSILON(0.05);
+
     rs2::option_range op_range = sensor.get_option_range(option);
-    if (op_range.step < 0.001f) return false;
+    if (abs((op_range.step - 1)) > EPSILON || (op_range.max > MAX_ENUM_OPTION_VALUES)) return false;
     for (auto i = op_range.min; i <= op_range.max; i += op_range.step)
     {
         if (sensor.get_option_value_description(option, i) == nullptr)
-            return false;
+            continue;
+        return true;
     }
-    return true;
+    return false;
 }
 
 bool is_int_option(rs2::options sensor, rs2_option option)
@@ -269,6 +273,8 @@ std::map<std::string, int> get_enum_method(rs2::options sensor, rs2_option optio
         const auto op_range_step = int(op_range.step);
         for (auto val = op_range_min; val <= op_range_max; val += op_range_step)
         {
+            if (sensor.get_option_value_description(option, val) == nullptr)
+                continue;
             dict[sensor.get_option_value_description(option, val)] = val;
         }
     }
@@ -705,37 +711,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 +1816,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