CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

simple_offboard: obtain vehicle pose using lookupTransform

Browse Source
This commit is contained in:
Oleg Kalachev
2019-07-30 00:26:43 +03:00
parent 6ea693eb85
commit 4cf825e004
1 changed files with 13 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
clever/src/simple_offboard.cpp
View File

@@ -201,24 +201,20 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
res.mode = state.mode;
}
waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
try {
waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
res.x = transform.transform.translation.x;
res.y = transform.transform.translation.y;
res.z = transform.transform.translation.z;
if (!TIMEOUT(local_position, local_position_timeout)) {
try {
// transform pose
PoseStamped pose;
tf_buffer.transform(local_position, pose, req.frame_id);
res.x = pose.pose.position.x;
res.y = pose.pose.position.y;
res.z = pose.pose.position.z;
// Tait-Bryan angles, order z-y-x
double yaw, pitch, roll;
tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
res.yaw = yaw;
res.pitch = pitch;
res.roll = roll;
} catch (const tf2::TransformException& e) {}
double yaw, pitch, roll;
tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
res.yaw = yaw;
res.pitch = pitch;
res.roll = roll;
} catch (const tf2::TransformException& e) {
ROS_DEBUG(e.what());
}
if (!TIMEOUT(velocity, velocity_timeout)) {