Skip to content

Commit

Permalink
Merge pull request #1614 from doronhi/fix-unknown-profile
Browse files Browse the repository at this point in the history 
Choose default profile
  • Loading branch information
@doronhi
doronhi authored Feb 16, 2021
2 parents e425451 + fddd00a commit f23fd1d
Show file tree
Hide file tree
Showing 4 changed files with 95 additions and 65 deletions.
34 changes: 17 additions & 17 deletions realsense2_camera/launch/rs_camera.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,36 +9,36 @@
<arg name="manager" default="realsense2_camera_manager"/>
<arg name="output" default="screen"/>

<arg name="fisheye_width" default="640"/>
<arg name="fisheye_height" default="480"/>
<arg name="fisheye_width" default="-1"/>
<arg name="fisheye_height" default="-1"/>
<arg name="enable_fisheye" default="false"/>

<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="depth_width" default="-1"/>
<arg name="depth_height" default="-1"/>
<arg name="enable_depth" default="true"/>

<arg name="confidence_width" default="640"/>
<arg name="confidence_height" default="480"/>
<arg name="confidence_width" default="-1"/>
<arg name="confidence_height" default="-1"/>
<arg name="enable_confidence" default="true"/>
<arg name="confidence_fps" default="30"/>
<arg name="confidence_fps" default="-1"/>

<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="infra_width" default="-1"/>
<arg name="infra_height" default="-1"/>
<arg name="enable_infra" default="false"/>
<arg name="enable_infra1" default="false"/>
<arg name="enable_infra2" default="false"/>
<arg name="infra_rgb" default="false"/>

<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="color_width" default="-1"/>
<arg name="color_height" default="-1"/>
<arg name="enable_color" default="true"/>

<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="400"/>
<arg name="accel_fps" default="250"/>
<arg name="fisheye_fps" default="-1"/>
<arg name="depth_fps" default="-1"/>
<arg name="infra_fps" default="-1"/>
<arg name="color_fps" default="-1"/>
<arg name="gyro_fps" default="-1"/>
<arg name="accel_fps" default="-1"/>
<arg name="enable_gyro" default="false"/>
<arg name="enable_accel" default="false"/>

Expand Down
28 changes: 14 additions & 14 deletions realsense2_camera/launch/rs_rgbd.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,29 +52,29 @@ Processing enabled by this node:
<arg name="device_type" default=""/>
<arg name="json_file_path" default=""/>

<arg name="fisheye_width" default="640"/>
<arg name="fisheye_height" default="480"/>
<arg name="fisheye_width" default="-1"/>
<arg name="fisheye_height" default="-1"/>
<arg name="enable_fisheye" default="false"/>

<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="depth_width" default="-1"/>
<arg name="depth_height" default="-1"/>
<arg name="enable_depth" default="true"/>

<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="infra_width" default="-1"/>
<arg name="infra_height" default="-1"/>
<arg name="enable_infra1" default="false"/>
<arg name="enable_infra2" default="false"/>

<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="color_width" default="-1"/>
<arg name="color_height" default="-1"/>
<arg name="enable_color" default="true"/>

<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="400"/>
<arg name="accel_fps" default="250"/>
<arg name="fisheye_fps" default="-1"/>
<arg name="depth_fps" default="-1"/>
<arg name="infra_fps" default="-1"/>
<arg name="color_fps" default="-1"/>
<arg name="gyro_fps" default="-1"/>
<arg name="accel_fps" default="-1"/>
<arg name="enable_gyro" default="false"/>
<arg name="enable_accel" default="false"/>

Expand Down
10 changes: 5 additions & 5 deletions realsense2_camera/launch/rs_t265.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>

<arg name="fisheye_width" default="848"/>
<arg name="fisheye_height" default="800"/>
<arg name="fisheye_width" default="-1"/>
<arg name="fisheye_height" default="-1"/>
<arg name="enable_fisheye1" default="false"/>
<arg name="enable_fisheye2" default="false"/>

<arg name="fisheye_fps" default="30"/>
<arg name="fisheye_fps" default="-1"/>

<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="62"/>
<arg name="gyro_fps" default="-1"/>
<arg name="accel_fps" default="-1"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_pose" default="true"/>
Expand Down
88 changes: 59 additions & 29 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1007,6 +1007,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<rs2::video_stream_profile>();
Expand All @@ -1017,34 +1018,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<rs2::video_stream_profile>();
_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;
}
}
Expand All @@ -1065,6 +1083,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)
{
Expand All @@ -1073,24 +1092,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 " <<stream_name << ":");
for (rs2::stream_profile& profile : profiles)
ROS_WARN_STREAM_COND((_fps[elem]!=-1), "No mathcing profile found for " << stream_name << " with fps=" << _fps[elem]);
if (default_profile)
{
if (profile.stream_type() != elem.first) continue;
ROS_WARN_STREAM("fps: " << profile.fps() << ". format: " << profile.format());
ROS_WARN_STREAM_COND((_fps[elem]!=-1), "Using default profile instead.");
selected_profile = default_profile;
}
}
if(selected_profile)
{
_fps[elem] = selected_profile.fps();
_enabled_profiles[elem].push_back(selected_profile);
ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - fps: " << _fps[elem]);
}
else
{
_enable[elem] = false;
}
}
Expand Down

0 comments on commit f23fd1d

Please sign in to comment.