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

Update accel_gyro_frame_id

Browse Source
This commit is contained in:
datean
2024-12-24 20:31:52 +08:00
parent 056185e901
commit 975219297c
2 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+1 -1
View File
@@ -353,7 +353,7 @@ class OBCameraNode {
std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr; std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
std::atomic_bool pipeline_started_{false}; std::atomic_bool pipeline_started_{false};
std::string camera_name_ = "camera"; std::string camera_name_ = "camera";
const std::string imu_optical_frame_id_ = "camera_gyro_optical_frame"; std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame";
const std::string imu_frame_id_ = "camera_gyro_frame"; const std::string imu_frame_id_ = "camera_gyro_frame";
std::shared_ptr<ob::Config> pipeline_config_ = nullptr; std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_; std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
orbbec_camera/src/ob_camera_node.cpp
+14 -12
View File
@@ -1039,6 +1039,8 @@ void OBCameraNode::getParameters() {
camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame"; camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame";
} }
accel_gyro_frame_id_ = camera_name_ + "_accel_gyro_optical_frame";
setAndGetNodeParameter(publish_tf_, "publish_tf", true); setAndGetNodeParameter(publish_tf_, "publish_tf", true);
setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0); setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0);
setAndGetNodeParameter(depth_registration_, "depth_registration", false); setAndGetNodeParameter(depth_registration_, "depth_registration", false);
@@ -2208,32 +2210,32 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
imu_msg.header.frame_id = optical_frame_id_[GYRO]; imu_msg.header.frame_id = optical_frame_id_[GYRO];
auto frame_timestamp = getFrameTimestampUs(accelframe); auto frame_timestamp = getFrameTimestampUs(accelframe);
auto timestamp = fromUsToROSTime(frame_timestamp); auto timestamp = fromUsToROSTime(frame_timestamp);
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
auto gyro_info = createIMUInfo(GYRO); auto gyro_info = createIMUInfo(GYRO);
gyro_info.header = imu_msg.header; gyro_info.header = imu_msg.header;
imu_info_publishers_[GYRO]->publish(gyro_info); imu_info_publishers_[GYRO]->publish(gyro_info);
auto accel_info = createIMUInfo(ACCEL); auto accel_info = createIMUInfo(ACCEL);
imu_msg.header.frame_id = optical_frame_id_[ACCEL]; imu_msg.header.frame_id = optical_frame_id_[ACCEL];
accel_info.header = imu_msg.header; accel_info.header = imu_msg.header;
imu_info_publishers_[ACCEL]->publish(accel_info); imu_info_publishers_[ACCEL]->publish(accel_info);
imu_msg.header.frame_id = imu_optical_frame_id_; imu_msg.header.frame_id = accel_gyro_frame_id_;
auto gyro_frame = gryoframe->as<ob::GyroFrame>(); auto gyro_frame = gryoframe->as<ob::GyroFrame>();
auto gyroData = gyro_frame->value(); auto gyroData = gyro_frame->value();
imu_msg.angular_velocity.x = gyroData.x - gyro_info.bias[0]; imu_msg.angular_velocity.x = gyroData.x - gyro_info.bias[0];
imu_msg.angular_velocity.y = gyroData.y - gyro_info.bias[1]; imu_msg.angular_velocity.y = gyroData.y - gyro_info.bias[1];
imu_msg.angular_velocity.z = gyroData.z - gyro_info.bias[2]; imu_msg.angular_velocity.z = gyroData.z - gyro_info.bias[2];
auto accel_frame = accelframe->as<ob::AccelFrame>(); auto accel_frame = accelframe->as<ob::AccelFrame>();
auto accelData = accel_frame->value(); auto accelData = accel_frame->value();
imu_msg.linear_acceleration.x = accelData.x - accel_info.bias[0]; imu_msg.linear_acceleration.x = accelData.x - accel_info.bias[0];
imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1]; imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2]; imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
imu_gyro_accel_publisher_->publish(imu_msg); imu_gyro_accel_publisher_->publish(imu_msg);
} }
@@ -2255,15 +2257,15 @@ 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 = fromUsToROSTime(frame->timeStampUs()); auto timestamp = fromUsToROSTime(frame->timeStampUs());
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
auto imu_info = createIMUInfo(stream_index); auto imu_info = createIMUInfo(stream_index);
imu_info.header = imu_msg.header; imu_info.header = imu_msg.header;
imu_info_publishers_[stream_index]->publish(imu_info); imu_info_publishers_[stream_index]->publish(imu_info);
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>();
auto data = gyro_frame->value(); auto data = gyro_frame->value();