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 tf error

Browse Source
This commit is contained in:
Joe Dong
2023-07-14 11:14:17 +08:00
parent 1b887340c3
commit 4cee459714
2 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+3 -1
View File
@@ -30,6 +30,8 @@
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>
#include <std_srvs/srv/set_bool.hpp>
#include <std_srvs/srv/empty.hpp>
@@ -154,7 +156,7 @@ class OBCameraNode {
void setupPublishers();
void publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans,
const tf2::Quaternion& q, const std::string& from, const std::string& to);
void calcAndPublishStaticTransform();
orbbec_camera/src/ob_camera_node.cpp
+13 -13
View File
@@ -906,17 +906,16 @@ std::optional<OBCameraParam> OBCameraNode::getColorCameraParam() {
return {};
}
void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans,
const tf2::Quaternion& q, const std::string& from,
const std::string& to) {
CHECK_EQ(trans.size(), 3u);
geometry_msgs::msg::TransformStamped msg;
msg.header.stamp = t;
msg.header.frame_id = from;
msg.child_frame_id = to;
msg.transform.translation.x = trans.at(2) / 1000.0;
msg.transform.translation.y = -trans.at(0) / 1000.0;
msg.transform.translation.z = -trans.at(1) / 1000.0;
msg.transform.translation.x = trans[2] / 1000.0;
msg.transform.translation.y = -trans[0] / 1000.0;
msg.transform.translation.z = -trans[1] / 1000.0;
msg.transform.rotation.x = q.getX();
msg.transform.rotation.y = q.getY();
msg.transform.rotation.z = q.getZ();
@@ -926,22 +925,23 @@ void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<floa
void OBCameraNode::calcAndPublishStaticTransform() {
tf2::Quaternion quaternion_optical, zero_rot, Q;
std::vector<float> trans(3, 0);
zero_rot.setRPY(0.0, 0.0, 0.0);
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
std::vector<float> zero_trans = {0, 0, 0};
tf2::Vector3 zero_trans(0, 0, 0);
tf2::Vector3 trans(0, 0, 0);
auto camera_param = pipeline_->getCameraParam();
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();
Q.normalize();
Q = Q.inverse();
trans[0] = -ex.trans[0];
trans[1] = -ex.trans[1];
trans[2] = -ex.trans[2];
trans[0] = ex.trans[0];
trans[1] = ex.trans[1];
trans[2] = ex.trans[2];
tf2::Transform transform(Q, trans);
transform = transform.inverse();
Q = transform.getRotation();
trans = transform.getOrigin();
rclcpp::Time tf_timestamp = node_->now();
publishStaticTF(tf_timestamp, trans, Q, frame_id_[DEPTH], frame_id_[COLOR]);