diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index c76deae626..23172b6e26 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -29,31 +29,31 @@ {'name': 'unite_imu_method', 'default': '', 'description': '[copy|linear_interpolation]'}, {'name': 'json_file_path', 'default': '', 'description': 'allows advanced configuration'}, {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, - {'name': 'depth_width', 'default': '640', 'description': 'depth image width'}, - {'name': 'depth_height', 'default': '480', 'description': 'depth image height'}, + {'name': 'depth_width', 'default': '-1', 'description': 'depth image width'}, + {'name': 'depth_height', 'default': '-1', 'description': 'depth image height'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, - {'name': 'color_width', 'default': '640', 'description': 'color image width'}, - {'name': 'color_height', 'default': '480', 'description': 'color image height'}, + {'name': 'color_width', 'default': '-1', 'description': 'color image width'}, + {'name': 'color_height', 'default': '-1', 'description': 'color image height'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, - {'name': 'infra_width', 'default': '640', 'description': 'infra width'}, - {'name': 'infra_height', 'default': '480', 'description': 'infra width'}, + {'name': 'infra_width', 'default': '-1', 'description': 'infra width'}, + {'name': 'infra_height', 'default': '-1', 'description': 'infra width'}, {'name': 'enable_infra1', 'default': 'true', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'true', 'description': 'enable infra2 stream'}, {'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'fisheye_width', 'default': '848', 'description': 'fisheye width'}, - {'name': 'fisheye_height', 'default': '800', 'description': 'fisheye width'}, + {'name': 'fisheye_width', 'default': '-1', 'description': 'fisheye width'}, + {'name': 'fisheye_height', 'default': '-1', 'description': 'fisheye width'}, {'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'}, {'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'}, - {'name': 'confidence_width', 'default': '640', 'description': 'depth image width'}, - {'name': 'confidence_height', 'default': '480', 'description': 'depth image height'}, + {'name': 'confidence_width', 'default': '-1', 'description': 'depth image width'}, + {'name': 'confidence_height', 'default': '-1', 'description': 'depth image height'}, {'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'}, - {'name': 'fisheye_fps', 'default': '30.', 'description': ''}, - {'name': 'depth_fps', 'default': '30.', 'description': ''}, - {'name': 'confidence_fps', 'default': '30.', 'description': ''}, - {'name': 'infra_fps', 'default': '30.', 'description': ''}, - {'name': 'color_fps', 'default': '30.', 'description': ''}, - {'name': 'gyro_fps', 'default': '400.', 'description': ''}, - {'name': 'accel_fps', 'default': '250.', 'description': ''}, + {'name': 'fisheye_fps', 'default': '-1.', 'description': ''}, + {'name': 'depth_fps', 'default': '-1.', 'description': ''}, + {'name': 'confidence_fps', 'default': '-1.', 'description': ''}, + {'name': 'infra_fps', 'default': '-1.', 'description': ''}, + {'name': 'color_fps', 'default': '-1.', 'description': ''}, + {'name': 'gyro_fps', 'default': '-1.', 'description': ''}, + {'name': 'accel_fps', 'default': '-1.', 'description': ''}, {'name': 'color_qos', 'default': 'SENSOR_DATA', 'description': 'QoS profile name'}, {'name': 'confidence_qos', 'default': 'SENSOR_DATA', 'description': 'QoS profile name'}, {'name': 'depth_qos', 'default': 'SENSOR_DATA', 'description': 'QoS profile name'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 7663dfb8f5..19eebd7668 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1092,6 +1092,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(); @@ -1102,34 +1103,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; } } @@ -1150,6 +1168,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) { @@ -1158,24 +1177,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 " <