i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

format updateStreamInfo function

Browse Source
This commit is contained in:
datean
2024-11-27 21:27:13 +08:00
parent b2b3be6075
commit 49854e5e9f
2 changed files with 73 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/gemini_330_series_interleave_laser_g335L.launch.py
+2 -2
View File
@@ -189,8 +189,8 @@ def generate_launch_description():
DeclareLaunchArgument('use_intra_process_comms', default_value='false'), DeclareLaunchArgument('use_intra_process_comms', default_value='false'),
DeclareLaunchArgument('attach_to_shared_component_container', default_value='false'), DeclareLaunchArgument('attach_to_shared_component_container', default_value='false'),
DeclareLaunchArgument('component_container_name', default_value='shared_orbbec_container'), DeclareLaunchArgument('component_container_name', default_value='shared_orbbec_container'),
DeclareLaunchArgument('delta_duration', default_value='5000.0'), DeclareLaunchArgument('delta_duration', default_value='1000.0'),
DeclareLaunchArgument('delta_fps', default_value='2'), DeclareLaunchArgument('delta_fps', default_value='1'),
] ]
def get_params(context, args): def get_params(context, args):
orbbec_camera/src/ob_camera_node.cpp
+71 -67
View File
@@ -2031,72 +2031,76 @@ std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
return frame; return frame;
} }
void OBCameraNode::updateStreamInfo(const std::shared_ptr<ob::Frame> &frame, VideoStreamInfo& stream_info) { void OBCameraNode::updateStreamInfo(const std::shared_ptr<ob::Frame> &frame,
uint64_t current_timestamp = getFrameTimestampUs(frame); VideoStreamInfo &stream_info) {
uint64_t duration = current_timestamp - stream_info.last_frame_time; uint64_t current_timestamp = getFrameTimestampUs(frame);
double dst_duration = duration; uint64_t duration = current_timestamp - stream_info.last_frame_time;
int dst_fps = 0; double dst_duration = 0.0;
stream_index_pair dst_frame_type; int dst_fps = 0;
double delta_duration = delta_duration_; stream_index_pair dst_frame_type;
int delta_fps = delta_fps_; double delta_duration = delta_duration_;
if (current_timestamp <= stream_info.last_frame_time) { int delta_fps = delta_fps_;
RCLCPP_INFO_STREAM(logger_, "[WARNING] current_timestamp " if (current_timestamp <= stream_info.last_frame_time) {
<< current_timestamp << " stream_info.last_frame_time " << stream_info.last_frame_time << "\n"); RCLCPP_INFO_STREAM(logger_, "[WARNING] current_timestamp "
} << current_timestamp << " stream_info.last_frame_time "
<< stream_info.last_frame_time << "\n");
switch (stream_info.frame_type) {
case OB_FRAME_COLOR:
dst_frame_type = stream_index_pair{OB_STREAM_COLOR, 0};
break;
case OB_FRAME_DEPTH:
dst_frame_type = stream_index_pair{OB_STREAM_DEPTH, 0};
break;
case OB_FRAME_IR_LEFT:
dst_frame_type = stream_index_pair{OB_STREAM_IR_LEFT, 0};
break;
case OB_FRAME_IR_RIGHT:
dst_frame_type = stream_index_pair{OB_STREAM_IR_RIGHT, 0};
break;
default:
RCLCPP_INFO_STREAM(logger_, "[WARNING] Unknown frame_type "
<< stream_info.frame_type << "\n");
return;
}
if (interleave_skip_enable_) {
dst_duration = (1000000.0 / fps_[dst_frame_type]) * 2 + delta_duration;
dst_fps = fps_[dst_frame_type] / 2;
} else {
dst_duration = 1000000.0 / fps_[dst_frame_type];
dst_fps = fps_[dst_frame_type];
}
if (duration > dst_duration) {
RCLCPP_INFO_STREAM(logger_, "[WARNING] Frame interval for frame_type "
<< stream_info.frame_type << " exceeded " << dst_duration / 1000.0 << " ms. Interval: "
<< duration / 1000.0 << " ms\n");
}
stream_info.frame_count++;
if (stream_info.frame_count % dst_fps == 0) {
double elapsed_seconds = (current_timestamp - stream_info.last_fps_calculation_time) / 1e6;
if (elapsed_seconds > 0) {
int frames_in_interval = stream_info.frame_count - stream_info.last_fps_frame_count;
stream_info.frame_rate = frames_in_interval / elapsed_seconds;
if (stream_info.frame_rate < (dst_fps - delta_fps)) {
RCLCPP_INFO_STREAM(logger_, "[INFO] Frame rate for frame_type " << stream_info.frame_type
<< ": " << stream_info.frame_rate << " FPS\n");
}
stream_info.last_fps_calculation_time = current_timestamp;
stream_info.last_fps_frame_count = stream_info.frame_count;
}
} }
stream_info.last_frame_time = current_timestamp; switch (stream_info.frame_type) {
case OB_FRAME_COLOR:
dst_frame_type = stream_index_pair{OB_STREAM_COLOR, 0};
break;
case OB_FRAME_DEPTH:
dst_frame_type = stream_index_pair{OB_STREAM_DEPTH, 0};
break;
case OB_FRAME_IR_LEFT:
dst_frame_type = stream_index_pair{OB_STREAM_IR_LEFT, 0};
break;
case OB_FRAME_IR_RIGHT:
dst_frame_type = stream_index_pair{OB_STREAM_IR_RIGHT, 0};
break;
default:
RCLCPP_INFO_STREAM(logger_,
"[WARNING] Unknown frame_type " << stream_info.frame_type << "\n");
return;
}
if (interleave_skip_enable_) {
dst_duration = (1000000.0 / fps_[dst_frame_type]) * 2 + delta_duration;
dst_fps = fps_[dst_frame_type] / 2;
} else {
dst_duration = 1000000.0 / fps_[dst_frame_type] + delta_duration;
dst_fps = fps_[dst_frame_type];
}
if (duration > dst_duration) {
RCLCPP_INFO_STREAM(logger_, "[WARNING] Frame interval for frame_type "
<< stream_info.frame_type << " exceeded "
<< dst_duration / 1000.0
<< " ms. Interval: " << duration / 1000.0 << " ms\n");
}
stream_info.frame_count++;
if (stream_info.frame_count % dst_fps == 0) {
double elapsed_seconds = (current_timestamp - stream_info.last_fps_calculation_time) / 1e6;
if (elapsed_seconds > 0) {
int frames_in_interval = stream_info.frame_count - stream_info.last_fps_frame_count;
stream_info.frame_rate = frames_in_interval / elapsed_seconds;
if (stream_info.frame_rate < (dst_fps - delta_fps)) {
RCLCPP_INFO_STREAM(logger_, "[INFO] Frame rate for frame_type "
<< stream_info.frame_type << ": " << stream_info.frame_rate
<< " FPS\n");
}
stream_info.last_fps_calculation_time = current_timestamp;
stream_info.last_fps_frame_count = stream_info.frame_count;
}
}
stream_info.last_frame_time = current_timestamp;
} }
void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame, void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
@@ -2124,7 +2128,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
interleave_skip_depth_index_++; interleave_skip_depth_index_++;
RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d", RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
interleave_skip_depth_index_); interleave_skip_depth_index_);
if (interleave_skip_depth_index_ % 2 == 0 ) { if (interleave_skip_depth_index_ % 2 == 0) {
RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType()); RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
return; return;
} }
@@ -2144,10 +2148,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType()); RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
return; return;
} }
if (frame->getType() == OB_FRAME_IR_LEFT){ if (frame->getType() == OB_FRAME_IR_LEFT) {
updateStreamInfo(frame, left_ir_stream_info_); updateStreamInfo(frame, left_ir_stream_info_);
} }
if (frame->getType() == OB_FRAME_IR_RIGHT){ if (frame->getType() == OB_FRAME_IR_RIGHT) {
updateStreamInfo(frame, right_ir_stream_info_); updateStreamInfo(frame, right_ir_stream_info_);
} }
} }