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 1 commit
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
4 changes: 3 additions & 1 deletion realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,8 @@ namespace realsense2_camera
BaseRealSenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process = false);
bool use_intra_process = false,
bool is_rosbag_file = false);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since you are getting the rs2::device as well, try to drop the new parameter (all over the code) and use this API, see if it's possible:

if (dev.is<playback>())

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks.
It make sense and make the code easier.
I saw the PR you talked about, but still I wanted to do this without asking the primary node about rosbag_filename parameter value, since I don't thinks this is the best way to do this.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Great, Lets also check if this API works too:

sensor.is<playback_sensor>()

If this works we can drop more changes.
Worth checking

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure it's possible, I don't see playback_sensor in the API

~BaseRealSenseNode();
void publishTopics();

Expand Down Expand Up @@ -293,6 +294,7 @@ namespace realsense2_camera
mutable std::condition_variable _cv_temp, _cv_mpc, _cv_tf;
bool _is_profile_changed;
bool _is_align_depth_changed;
bool _is_rosbag_file;

std::shared_ptr<diagnostic_updater::Updater> _diagnostics_updater;
rs2::stream_profile _base_profile;
Expand Down
2 changes: 1 addition & 1 deletion realsense2_camera/include/realsense_node_factory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ namespace realsense2_camera
private:
void init();
void closeDevice();
void startDevice();
void startDevice(bool is_rosbag_file = false);
void changeDeviceCallback(rs2::event_information& info);
void getDevice(rs2::device_list list);
void tryGetLogSeverity(rs2_log_severity& severity) const;
Expand Down
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
3 changes: 2 additions & 1 deletion realsense2_camera/include/t265_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,8 @@ namespace realsense2_camera
T265RealsenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process = false);
bool use_intra_process = false,
bool is_rosbag_file = false);
void publishTopics();

protected:
Expand Down
6 changes: 4 additions & 2 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ size_t SyncedImuPublisher::getNumSubscribers()
BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process) :
bool use_intra_process,
bool is_rosbag_file) :
_is_running(true),
_node(node),
_logger(node.get_logger()),
Expand All @@ -85,7 +86,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
_is_initialized_time_base(false),
_sync_frames(SYNC_FRAMES),
_is_profile_changed(false),
_is_align_depth_changed(false)
_is_align_depth_changed(false),
_is_rosbag_file(is_rosbag_file)
{
if ( use_intra_process )
{
Expand Down
8 changes: 4 additions & 4 deletions realsense2_camera/src/realsense_node_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,7 @@ void RealSenseNodeFactory::init()
}
if (_device)
{
startDevice();
startDevice(true);
}
}
else
Expand Down Expand Up @@ -347,7 +347,7 @@ void RealSenseNodeFactory::init()
}
}

void RealSenseNodeFactory::startDevice()
void RealSenseNodeFactory::startDevice(bool is_rosbag_file)
{
if (_realSenseNode) _realSenseNode.reset();
std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
Expand Down Expand Up @@ -376,10 +376,10 @@ void RealSenseNodeFactory::startDevice()
case RS_L515_PID_PRE_PRQ:
case RS_L515_PID:
case RS_L535_PID:
_realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
_realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file));
break;
case RS_T265_PID:
_realSenseNode = std::unique_ptr<T265RealsenseNode>(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
_realSenseNode = std::unique_ptr<T265RealsenseNode>(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file));
break;
default:
ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str);
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
6 changes: 3 additions & 3 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 @@ -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, _is_rosbag_file);
}
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, _is_rosbag_file);
}
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, _is_rosbag_file);
}
else
{
Expand Down
5 changes: 3 additions & 2 deletions realsense2_camera/src/t265_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,9 @@ using namespace realsense2_camera;
T265RealsenseNode::T265RealsenseNode(rclcpp::Node& node,
rs2::device dev,
std::shared_ptr<Parameters> parameters,
bool use_intra_process) :
BaseRealSenseNode(node, dev, parameters, use_intra_process),
bool use_intra_process,
bool is_rosbag_file) :
BaseRealSenseNode(node, dev, parameters, use_intra_process, is_rosbag_file),
_wo_snr(dev.first<rs2::wheel_odometer>()),
_use_odom_in(false)
{
Expand Down