diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index b3d61228fe..fa89873fa3 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -198,7 +198,8 @@ namespace realsense2_camera void enable_devices(); void setupFilters(); void setupStreams(); - void setBaseTime(double frame_time, rs2_timestamp_domain time_domain); + bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain); + double frameSystemTimeSec(rs2::frame frame); cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image); void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 311d5369de..ae1d5c7e4e 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1354,17 +1354,16 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync bool placeholder_false(false); if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) { - setBaseTime(frame_time, frame.get_frame_timestamp_domain()); + _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } seq += 1; - double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; if (0 != _synced_imu_publisher->getNumSubscribers()) { auto crnt_reading = *(reinterpret_cast(frame.get_data())); Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); - CimuData imu_data(stream_index, v, elapsed_camera_ms); + CimuData imu_data(stream_index, v, frameSystemTimeSec(frame)); std::deque imu_msgs; switch (sync_method) { @@ -1379,9 +1378,7 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync while (imu_msgs.size()) { sensor_msgs::Imu imu_msg = imu_msgs.front(); - ros::Time t(_ros_time_base.toSec() + imu_msg.header.stamp.toSec()); imu_msg.header.seq = seq; - imu_msg.header.stamp = t; ImuMessage_AddDefaultValues(imu_msg); _synced_imu_publisher->Publish(imu_msg); ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); @@ -1398,7 +1395,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) bool placeholder_false(false); if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) { - setBaseTime(frame_time, frame.get_frame_timestamp_domain()); + _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", @@ -1409,8 +1406,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; if (0 != _imu_publishers[stream_index].getNumSubscribers()) { - double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; - ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); + ros::Time t(frameSystemTimeSec(frame)); auto imu_msg = sensor_msgs::Imu(); ImuMessage_AddDefaultValues(imu_msg); @@ -1443,7 +1439,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) bool placeholder_false(false); if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) { - setBaseTime(frame_time, frame.get_frame_timestamp_domain()); + _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", @@ -1452,8 +1448,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); const auto& stream_index(POSE); rs2_pose pose = frame.as().get_pose_data(); - double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; - ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); + ros::Time t(frameSystemTimeSec(frame)); geometry_msgs::PoseStamped pose_msg; pose_msg.pose.position.x = -pose.translation.z; @@ -1543,19 +1538,10 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) bool placeholder_false(false); if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) { - setBaseTime(frame_time, frame.get_frame_timestamp_domain()); - } - - ros::Time t; - if (_sync_frames) - { - t = ros::Time::now(); - } - else - { - t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame_time - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000); + _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); } + ros::Time t(frameSystemTimeSec(frame)); if (frame.is()) { ROS_DEBUG("Frameset arrived."); @@ -1724,14 +1710,30 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met } } -void BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain) +bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain) { - - ROS_WARN_COND(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME, "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)"); - ROS_WARN_COND(time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, "frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); + ROS_WARN_ONCE(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME ? "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)" : ""); + if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) + { + ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); + _ros_time_base = ros::Time::now(); + _camera_time_base = frame_time; + return true; + } + return false; +} - _ros_time_base = ros::Time::now(); - _camera_time_base = frame_time; +double BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) +{ + if (frame.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) + { + double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / 1000.0; + return (_ros_time_base.toSec() + elapsed_camera_ms); + } + else + { + return (frame.get_timestamp() / 1000.0); + } } void BaseRealSenseNode::setupStreams()