Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix rosbag device loading by preventing set_option to HDR/Gain/Exposure #2409

Merged
merged 2 commits into from
Jul 17, 2022
Merged

fix rosbag device loading by preventing set_option to HDR/Gain/Exposure #2409

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f01d776
fix rosbag device loading by preventing set_option to HDR/Gain/Exposure
SamerKhshiboun Jul 13, 2022
7ffa714
use is<playback> instead
SamerKhshiboun Jul 13, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions realsense2_camera/include/ros_sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ namespace realsense2_camera
std::function<void()> hardware_reset_func,
std::shared_ptr<diagnostic_updater::Updater> diagnostics_updater,
rclcpp::Logger logger,
bool force_image_default_qos = false);
bool force_image_default_qos = false,
bool is_rosbag_file = false);
~RosSensor();
void registerSensorParameters();
bool getUpdatedProfiles(std::vector<rs2::stream_profile>& wanted_profiles);
Expand All @@ -88,7 +89,7 @@ namespace realsense2_camera

private:
void setupErrorCallback();
void setParameters();
void setParameters(bool is_rosbag_file = false);
void clearParameters();
void set_sensor_auto_exposure_roi();
void registerAutoExposureROIOptions();
Expand Down
14 changes: 10 additions & 4 deletions realsense2_camera/src/ros_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@ RosSensor::RosSensor(rs2::sensor sensor,
std::function<void()> hardware_reset_func,
std::shared_ptr<diagnostic_updater::Updater> diagnostics_updater,
rclcpp::Logger logger,
bool force_image_default_qos):
bool force_image_default_qos,
bool is_rosbag_file):
rs2::sensor(sensor),
_logger(logger),
_origin_frame_callback(frame_callback),
Expand All @@ -54,7 +55,7 @@ RosSensor::RosSensor(rs2::sensor sensor,

_origin_frame_callback(frame);
};
setParameters();
setParameters(is_rosbag_file);
}

RosSensor::~RosSensor()
Expand All @@ -63,11 +64,16 @@ RosSensor::~RosSensor()
stop();
}

void RosSensor::setParameters()
void RosSensor::setParameters(bool is_rosbag_file)
{
std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
_params.registerDynamicOptions(*this, module_name);
UpdateSequenceIdCallback();

// for rosbag files, don't set hdr(sequence_id) / gain / exposure options
// since these options can be changed only in real devices
if(!is_rosbag_file)
UpdateSequenceIdCallback();

registerSensorParameters();
}

Expand Down
8 changes: 4 additions & 4 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ void BaseRealSenseNode::setAvailableSensors()
ROS_INFO_STREAM("Device Product ID: 0x" << pid);

ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off"));

std::function<void(rs2::frame)> frame_callback_function = [this](rs2::frame frame){
bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr<NamedFilter> f){return (f->is_enabled()); }));
if (_sync_frames || is_filter)
Expand Down Expand Up @@ -135,17 +135,17 @@ void BaseRealSenseNode::setAvailableSensors()
sensor.is<rs2::fisheye_sensor>())
{
ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor.");
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process);
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is<playback>());
}
else if (sensor.is<rs2::motion_sensor>())
{
ROS_DEBUG_STREAM("Set " << module_name << " as ImuSensor.");
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger);
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is<playback>());
}
else if (sensor.is<rs2::pose_sensor>())
{
ROS_DEBUG_STREAM("Set " << module_name << " as PoseSensor.");
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger);
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is<playback>());
}
else
{
Expand Down