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

fixed TF

Browse Source
This commit is contained in:
Joe Dong
2023-06-12 17:04:33 +08:00
parent 4b87d9807a
commit 78aa992165
1 changed files with 29 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+29 -12
View File
@@ -470,15 +470,15 @@ void OBCameraNode::setupPublishers() {
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
}
// for (const auto& stream_index : HID_STREAMS) {
// if (!enable_stream_[stream_index]) {
// continue;
// }
// std::string data_topic_name = stream_name_[stream_index] + "/sample";
// auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
// imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
// data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
// }
// for (const auto& stream_index : HID_STREAMS) {
// if (!enable_stream_[stream_index]) {
// continue;
// }
// std::string data_topic_name = stream_name_[stream_index] + "/sample";
// auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
// imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
// data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
// }
if (enable_publish_extrinsic_) {
extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local());
@@ -865,6 +865,8 @@ std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
int depth_h = param.depthIntrinsic.height;
int color_w = param.rgbIntrinsic.width;
int color_h = param.rgbIntrinsic.height;
RCLCPP_INFO_STREAM(logger_, "depth_w: " << depth_w << " depth_h: " << depth_h
<< " color_w: " << color_w << " color_h: " << color_h);
if ((depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) &&
(color_w * height_[COLOR] == color_h * width_[COLOR])) {
return param;
@@ -924,23 +926,38 @@ void OBCameraNode::calcAndPublishStaticTransform() {
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
std::vector<float> zero_trans = {0, 0, 0};
auto camera_param = findDefaultCameraParam();
if (enable_publish_extrinsic_ && extrinsics_publisher_ && camera_param.has_value()) {
if (camera_param.has_value()) {
auto ex = camera_param->transform;
RCLCPP_INFO_STREAM(logger_,
"transform x " << ex.trans[0] << " y " << ex.trans[1] << " z " << trans[2]);
Q = rotationMatrixToQuaternion(ex.rot);
Q = quaternion_optical * Q * quaternion_optical.inverse();
extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"));
trans[0] = ex.trans[0];
trans[1] = ex.trans[1];
trans[2] = ex.trans[2];
} else {
Q.setRPY(0, 0, 0);
}
if (enable_publish_extrinsic_ && extrinsics_publisher_ && camera_param.has_value()) {
auto ex = camera_param->transform;
extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"));
}
rclcpp::Time tf_timestamp = node_->now();
publishStaticTF(tf_timestamp, trans, Q, frame_id_[DEPTH], frame_id_[COLOR]);
publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
optical_frame_id_[COLOR]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[DEPTH],
optical_frame_id_[DEPTH]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[ACCEL],
optical_frame_id_[ACCEL]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[GYRO],
optical_frame_id_[GYRO]);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[INFRA0],
optical_frame_id_[INFRA0]);
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[DEPTH]);
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[INFRA0]);
publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[ACCEL]);
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[GYRO]);
}