diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch
index c85dd1ce48..4acdcd17f8 100644
--- a/realsense2_camera/launch/rs_camera.launch
+++ b/realsense2_camera/launch/rs_camera.launch
@@ -9,36 +9,36 @@
-
-
+
+
-
-
+
+
-
-
+
+
-
+
-
-
+
+
-
-
+
+
-
-
-
-
-
-
+
+
+
+
+
+
diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch
index 3232b43d21..94e97670ad 100644
--- a/realsense2_camera/launch/rs_rgbd.launch
+++ b/realsense2_camera/launch/rs_rgbd.launch
@@ -52,29 +52,29 @@ Processing enabled by this node:
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
-
-
-
-
+
+
+
+
+
+
diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch
index 9ab3073e33..1c2e9a57ca 100644
--- a/realsense2_camera/launch/rs_t265.launch
+++ b/realsense2_camera/launch/rs_t265.launch
@@ -12,15 +12,15 @@ 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 54533f42db..6fe176d0cb 100644
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -1006,6 +1006,7 @@ void BaseRealSenseNode::enable_devices()
{
auto& sens = _sensors[elem];
auto profiles = sens.get_stream_profiles();
+ rs2::stream_profile default_profile, selected_profile;
for (auto& profile : profiles)
{
auto video_profile = profile.as();
@@ -1016,34 +1017,51 @@ void BaseRealSenseNode::enable_devices()
", Height: " << video_profile.height() <<
", FPS: " << video_profile.fps());
- if ((video_profile.stream_type() == elem.first) &&
- (_width[elem] == 0 || video_profile.width() == _width[elem]) &&
- (_height[elem] == 0 || video_profile.height() == _height[elem]) &&
- (_fps[elem] == 0 || video_profile.fps() == _fps[elem]) &&
- (_format.find(elem.first) == _format.end() || video_profile.format() == _format[elem.first] ) &&
- video_profile.stream_index() == elem.second)
+ if (profile.stream_type() == elem.first)
{
- _width[elem] = video_profile.width();
- _height[elem] = video_profile.height();
- _fps[elem] = video_profile.fps();
-
- _enabled_profiles[elem].push_back(profile);
-
- _image[elem] = cv::Mat(_height[elem], _width[elem], _image_format[elem.first], cv::Scalar(0, 0, 0));
-
- ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem] << ", " << "Format: " << video_profile.format());
- break;
+ if (profile.is_default())
+ {
+ default_profile = profile;
+ }
+ if ((_width[elem] == 0 || video_profile.width() == _width[elem]) &&
+ (_height[elem] == 0 || video_profile.height() == _height[elem]) &&
+ (_fps[elem] == 0 || video_profile.fps() == _fps[elem]) &&
+ (_format.find(elem.first) == _format.end() || video_profile.format() == _format[elem.first] ) &&
+ video_profile.stream_index() == elem.second)
+ {
+ selected_profile = profile;
+ break;
+ }
}
+
}
- if (_enabled_profiles.find(elem) == _enabled_profiles.end())
+ if (!selected_profile)
{
- ROS_WARN_STREAM("Given stream configuration is not supported by the device! " <<
+ ROS_WARN_STREAM_COND((_width[elem]!=-1 && _height[elem]!=-1 && _fps[elem]!=-1), "Given stream configuration is not supported by the device! " <<
" Stream: " << rs2_stream_to_string(elem.first) <<
", Stream Index: " << elem.second <<
", Width: " << _width[elem] <<
", Height: " << _height[elem] <<
", FPS: " << _fps[elem] <<
", Format: " << ((_format.find(elem.first) == _format.end())? "None":rs2_format_to_string(rs2_format(_format[elem.first]))));
+ if (default_profile)
+ {
+ ROS_WARN_STREAM_COND((_width[elem]!=-1 && _height[elem]!=-1 && _fps[elem]!=-1), "Using default profile instead.");
+ selected_profile = default_profile;
+ }
+ }
+ if (selected_profile)
+ {
+ auto video_profile = selected_profile.as();
+ _width[elem] = video_profile.width();
+ _height[elem] = video_profile.height();
+ _fps[elem] = video_profile.fps();
+ _enabled_profiles[elem].push_back(selected_profile);
+ _image[elem] = cv::Mat(_height[elem], _width[elem], _image_format[elem.first], cv::Scalar(0, 0, 0));
+ ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem] << ", " << "Format: " << video_profile.format());
+ }
+ else
+ {
_enable[elem] = false;
}
}
@@ -1064,6 +1082,7 @@ void BaseRealSenseNode::enable_devices()
{
auto& sens = _sensors[elem];
auto profiles = sens.get_stream_profiles();
+ rs2::stream_profile default_profile, selected_profile;
ROS_DEBUG_STREAM("Available profiles:");
for (rs2::stream_profile& profile : profiles)
{
@@ -1072,24 +1091,35 @@ void BaseRealSenseNode::enable_devices()
}
for (rs2::stream_profile& profile : profiles)
{
- if (profile.stream_type() == elem.first &&
- (_fps[elem] == 0 || profile.fps() == _fps[elem]))
+ if (profile.stream_type() == elem.first)
{
- _fps[elem] = profile.fps();
- _enabled_profiles[elem].push_back(profile);
- break;
+ if (profile.is_default())
+ default_profile = profile;
+ if (_fps[elem] == 0 || profile.fps() == _fps[elem])
+ {
+ selected_profile = profile;
+ break;
+ }
}
}
- if (_enabled_profiles.find(elem) == _enabled_profiles.end())
+ if (!selected_profile)
{
std::string stream_name(STREAM_NAME(elem));
- ROS_WARN_STREAM("No mathcing profile found for " << stream_name << " with fps=" << _fps[elem]);
- ROS_WARN_STREAM("profiles found for " <