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

fixed imu frame id.

Browse Source
This commit is contained in:
lixiaobin
2024-01-15 11:53:14 +08:00
parent 573cf4e6e0
commit 75b6b6f8e6
2 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+2
View File
@@ -311,6 +311,8 @@ class OBCameraNode {
std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
std::atomic_bool pipeline_started_{false};
std::string camera_name_ = "camera";
const std::string imu_optical_frame_id_ = "camera_gyro_accel_optical_frame";
const std::string imu_frame_id_ = "camera_gyro_accel_frame";
std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
orbbec_camera/src/ob_camera_node.cpp
+11 -10
View File
@@ -1221,8 +1221,7 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
auto imu_msg = sensor_msgs::msg::Imu();
setDefaultIMUMessage(imu_msg);
std::string imu_optical_frame_id ="camera_gyro_accel_optical_frame";
imu_msg.header.frame_id = imu_optical_frame_id;
imu_msg.header.frame_id = imu_optical_frame_id_;
auto timestamp = frameTimeStampToROSTime(accelframe->systemTimeStamp());
imu_msg.header.stamp = timestamp;
auto gyro_frame = gryoframe->as<ob::GyroFrame>();
@@ -1429,14 +1428,16 @@ void OBCameraNode::calcAndPublishStaticTransform() {
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
optical_frame_id_[stream_index]);
}
for (const auto &stream_index : HID_STREAMS) {
if (enable_stream_[stream_index]) {
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_,
frame_id_[stream_index]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
optical_frame_id_[stream_index]);
}
}
// for (const auto &stream_index : HID_STREAMS) {
// if (enable_stream_[stream_index]) {
// publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_,
// frame_id_[stream_index]);
// publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
// optical_frame_id_[stream_index]);
// }
// }
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, imu_frame_id_);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, imu_frame_id_, imu_optical_frame_id_);
}
void OBCameraNode::publishStaticTransforms() {