i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-11 23:09:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed timestamp

Browse Source
This commit is contained in:
Joe Dong
2024-03-15 10:32:53 +08:00
parent f3f5cb8df9
commit 382c83b53a
3 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/utils.h
+3 -1
View File
@@ -47,7 +47,9 @@ std::ostream& operator<<(std::ostream& os, const OBCameraParam& rhs);
orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform& extrinsics, orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform& extrinsics,
const std::string& frame_id); const std::string& frame_id);
rclcpp::Time frameTimeStampToROSTime(uint64_t ms); rclcpp::Time fromMsToROSTime(uint64_t ms);
rclcpp::Time fromUsToROSTime(uint64_t us);
std::string getObSDKVersion(); std::string getObSDKVersion();
orbbec_camera/src/ob_camera_node.cpp
+8 -6
View File
@@ -782,7 +782,8 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
} }
} }
} }
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp()); auto timestamp = use_hardware_time_ ? fromUsToROSTime(video_frame->timeStampUs())
: fromMsToROSTime(video_frame->systemTimeStamp());
if (!ordered_pc_) { if (!ordered_pc_) {
point_cloud_msg_.is_dense = true; point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count; point_cloud_msg_.width = valid_count;
@@ -902,7 +903,8 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
} }
} }
} }
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp()); auto timestamp = use_hardware_time_ ? fromUsToROSTime(depth_frame->timeStampUs())
: fromMsToROSTime(depth_frame->systemTimeStamp());
if (!ordered_pc_) { if (!ordered_pc_) {
point_cloud_msg_.is_dense = true; point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count; point_cloud_msg_.width = valid_count;
@@ -1133,8 +1135,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
} }
int width = static_cast<int>(video_frame->width()); int width = static_cast<int>(video_frame->width());
int height = static_cast<int>(video_frame->height()); int height = static_cast<int>(video_frame->height());
auto frame_time_stamp = use_hardware_time_?video_frame->timeStamp():video_frame->systemTimeStamp(); auto timestamp = use_hardware_time_ ? fromUsToROSTime(video_frame->timeStampUs())
auto timestamp = frameTimeStampToROSTime(frame_time_stamp); : fromMsToROSTime(video_frame->systemTimeStamp());
if (!camera_param_) { if (!camera_param_) {
camera_param_ = pipeline_->getCameraParam(); camera_param_ = pipeline_->getCameraParam();
} }
@@ -1247,7 +1249,7 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
setDefaultIMUMessage(imu_msg); setDefaultIMUMessage(imu_msg);
imu_msg.header.frame_id = imu_optical_frame_id_; imu_msg.header.frame_id = imu_optical_frame_id_;
auto timestamp = frameTimeStampToROSTime(accelframe->systemTimeStamp()); auto timestamp = fromUsToROSTime(accelframe->timeStampUs());
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
auto gyro_frame = gryoframe->as<ob::GyroFrame>(); auto gyro_frame = gryoframe->as<ob::GyroFrame>();
auto gyroData = gyro_frame->value(); auto gyroData = gyro_frame->value();
@@ -1276,7 +1278,7 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
auto imu_msg = sensor_msgs::msg::Imu(); auto imu_msg = sensor_msgs::msg::Imu();
setDefaultIMUMessage(imu_msg); setDefaultIMUMessage(imu_msg);
imu_msg.header.frame_id = optical_frame_id_[stream_index]; imu_msg.header.frame_id = optical_frame_id_[stream_index];
auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp()); auto timestamp = fromUsToROSTime(frame->timeStampUs());
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
if (frame->type() == OB_FRAME_GYRO) { if (frame->type() == OB_FRAME_GYRO) {
auto gyro_frame = frame->as<ob::GyroFrame>(); auto gyro_frame = frame->as<ob::GyroFrame>();
orbbec_camera/src/utils.cpp
+9 -2
View File
@@ -35,7 +35,6 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
info.d[6] = distortion.k5; info.d[6] = distortion.k5;
info.d[7] = distortion.k6; info.d[7] = distortion.k6;
info.k.fill(0.0); info.k.fill(0.0);
info.k[0] = intrinsic.fx; info.k[0] = intrinsic.fx;
info.k[2] = intrinsic.cx; info.k[2] = intrinsic.cx;
@@ -235,7 +234,7 @@ orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extr
return msg; return msg;
} }
rclcpp::Time frameTimeStampToROSTime(uint64_t ms) { rclcpp::Time fromMsToROSTime(uint64_t ms) {
auto total = static_cast<uint64_t>(ms * 1e6); auto total = static_cast<uint64_t>(ms * 1e6);
uint64_t sec = total / 1000000000; uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000; uint64_t nano_sec = total % 1000000000;
@@ -243,6 +242,14 @@ rclcpp::Time frameTimeStampToROSTime(uint64_t ms) {
return stamp; return stamp;
} }
rclcpp::Time fromUsToROSTime(uint64_t us) {
auto total = static_cast<uint64_t>(us * 1e3);
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
return stamp;
}
std::string getObSDKVersion() { std::string getObSDKVersion() {
std::string major = std::to_string(ob::Version::getMajor()); std::string major = std::to_string(ob::Version::getMajor());
std::string minor = std::to_string(ob::Version::getMinor()); std::string minor = std::to_string(ob::Version::getMinor());