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

Publish real frame rate of realsense camera node topics/publishers #2408

Merged
merged 2 commits into from
Jul 14, 2022
Merged

Publish real frame rate of realsense camera node topics/publishers #2408

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
e699bc5
fix ticking frequency
SamerKhshiboun Jul 12, 2022
1d7b2f5
move throw to outside callback
SamerKhshiboun Jul 12, 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
200 changes: 97 additions & 103 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 @@ -518,127 +518,121 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)
void BaseRealSenseNode::frame_callback(rs2::frame frame)
{
_synced_imu_publisher->Pause();
try{
double frame_time = frame.get_timestamp();

// We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
// and the incremental timestamp from the camera.
// In sync mode the timestamp is based on ROS time
bool placeholder_false(false);
if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
double frame_time = frame.get_timestamp();

// We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
// and the incremental timestamp from the camera.
// In sync mode the timestamp is based on ROS time
bool placeholder_false(false);
if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
{
_is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
}

rclcpp::Time t(frameSystemTimeSec(frame));
if (frame.is<rs2::frameset>())
{
ROS_DEBUG("Frameset arrived.");
auto frameset = frame.as<rs2::frameset>();
ROS_DEBUG("List of frameset before applying filters: size: %d", static_cast<int>(frameset.size()));
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
_is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
auto f = (*it);
auto stream_type = f.get_profile().stream_type();
auto stream_index = f.get_profile().stream_index();
auto stream_format = f.get_profile().format();
auto stream_unique_id = f.get_profile().unique_id();

ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame_time, t.nanoseconds());
}
// Clip depth_frame for max range:
rs2::depth_frame original_depth_frame = frameset.get_depth_frame();
if (original_depth_frame && _clipping_distance > 0)
{
clip_depth(original_depth_frame, _clipping_distance);
}

rclcpp::Time t(frameSystemTimeSec(frame));
if (frame.is<rs2::frameset>())
ROS_DEBUG("num_filters: %d", static_cast<int>(_filters.size()));
for (auto filter_it : _filters)
{
ROS_DEBUG("Frameset arrived.");
auto frameset = frame.as<rs2::frameset>();
ROS_DEBUG("List of frameset before applying filters: size: %d", static_cast<int>(frameset.size()));
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
auto f = (*it);
auto stream_type = f.get_profile().stream_type();
auto stream_index = f.get_profile().stream_index();
auto stream_format = f.get_profile().format();
auto stream_unique_id = f.get_profile().unique_id();

ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame_time, t.nanoseconds());
}
// Clip depth_frame for max range:
rs2::depth_frame original_depth_frame = frameset.get_depth_frame();
if (original_depth_frame && _clipping_distance > 0)
{
clip_depth(original_depth_frame, _clipping_distance);
}
frameset = filter_it->Process(frameset);
}

ROS_DEBUG("List of frameset after applying filters: size: %d", static_cast<int>(frameset.size()));
bool sent_depth_frame(false);
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
auto f = (*it);
auto stream_type = f.get_profile().stream_type();
auto stream_index = f.get_profile().stream_index();
auto stream_format = f.get_profile().format();
stream_index_pair sip{stream_type,stream_index};

ROS_DEBUG("num_filters: %d", static_cast<int>(_filters.size()));
for (auto filter_it : _filters)
ROS_DEBUG("Frameset contain (%s, %d, %s) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), f.get_frame_number(), frame_time, t.nanoseconds());
if (f.is<rs2::video_frame>())
ROS_DEBUG_STREAM("frame: " << f.as<rs2::video_frame>().get_width() << " x " << f.as<rs2::video_frame>().get_height());

if (f.is<rs2::points>())
{
frameset = filter_it->Process(frameset);
publishPointCloud(f.as<rs2::points>(), t, frameset);
continue;
}

ROS_DEBUG("List of frameset after applying filters: size: %d", static_cast<int>(frameset.size()));
bool sent_depth_frame(false);
for (auto it = frameset.begin(); it != frameset.end(); ++it)
if (stream_type == RS2_STREAM_DEPTH)
{
auto f = (*it);
auto stream_type = f.get_profile().stream_type();
auto stream_index = f.get_profile().stream_index();
auto stream_format = f.get_profile().format();
stream_index_pair sip{stream_type,stream_index};

ROS_DEBUG("Frameset contain (%s, %d, %s) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), f.get_frame_number(), frame_time, t.nanoseconds());
if (f.is<rs2::video_frame>())
ROS_DEBUG_STREAM("frame: " << f.as<rs2::video_frame>().get_width() << " x " << f.as<rs2::video_frame>().get_height());

if (f.is<rs2::points>())
if (sent_depth_frame) continue;
sent_depth_frame = true;
if (_align_depth_filter->is_enabled())
{
publishPointCloud(f.as<rs2::points>(), t, frameset);
publishFrame(f, t, COLOR,
_depth_aligned_image,
_depth_aligned_info_publisher,
_depth_aligned_image_publishers,
false);
continue;
}
if (stream_type == RS2_STREAM_DEPTH)
{
if (sent_depth_frame) continue;
sent_depth_frame = true;
if (_align_depth_filter->is_enabled())
{
publishFrame(f, t, COLOR,
_depth_aligned_image,
_depth_aligned_info_publisher,
_depth_aligned_image_publishers,
false);
continue;
}
}
publishFrame(f, t, sip,
_image,
_info_publisher,
_image_publishers);
}
if (original_depth_frame && _align_depth_filter->is_enabled())
{
rs2::frame frame_to_send;
if (_colorizer_filter->is_enabled())
frame_to_send = _colorizer_filter->Process(original_depth_frame);
else
frame_to_send = original_depth_frame;

publishFrame(frame_to_send, t,
DEPTH,
_image,
_info_publisher,
_image_publishers);
}
publishFrame(f, t, sip,
_image,
_info_publisher,
_image_publishers);
}
else if (frame.is<rs2::video_frame>())
if (original_depth_frame && _align_depth_filter->is_enabled())
{
auto stream_type = frame.get_profile().stream_type();
auto stream_index = frame.get_profile().stream_index();
ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.nanoseconds());
rs2::frame frame_to_send;
if (_colorizer_filter->is_enabled())
frame_to_send = _colorizer_filter->Process(original_depth_frame);
else
frame_to_send = original_depth_frame;

publishFrame(frame_to_send, t,
DEPTH,
_image,
_info_publisher,
_image_publishers);
}
}
else if (frame.is<rs2::video_frame>())
{
auto stream_type = frame.get_profile().stream_type();
auto stream_index = frame.get_profile().stream_index();
ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.nanoseconds());

stream_index_pair sip{stream_type,stream_index};
if (frame.is<rs2::depth_frame>())
stream_index_pair sip{stream_type,stream_index};
if (frame.is<rs2::depth_frame>())
{
if (_clipping_distance > 0)
{
if (_clipping_distance > 0)
{
clip_depth(frame, _clipping_distance);
}
clip_depth(frame, _clipping_distance);
}
publishFrame(frame, t,
sip,
_image,
_info_publisher,
_image_publishers);
}
}
catch(const std::exception& ex)
{
ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what());
publishFrame(frame, t,
sip,
_image,
_info_publisher,
_image_publishers);
}
_synced_imu_publisher->Resume();
} // frame_callback
Expand Down
15 changes: 11 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 @@ -49,10 +49,17 @@ RosSensor::RosSensor(rs2::sensor sensor,
auto stream_type = frame.get_profile().stream_type();
auto stream_index = frame.get_profile().stream_index();
stream_index_pair sip{stream_type, stream_index};
if (_frequency_diagnostics.find(sip) != _frequency_diagnostics.end())
_frequency_diagnostics.at(sip).Tick();

_origin_frame_callback(frame);
try
{
_origin_frame_callback(frame);
if (_frequency_diagnostics.find(sip) != _frequency_diagnostics.end())
_frequency_diagnostics.at(sip).Tick();
}
catch(const std::exception& ex)
{
// don't tick the frequency diagnostics for this publisher
ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what());
}
};
setParameters();
}
Expand Down