From 47bc3b90daf629558abb3ec6c00ef547f8d854fc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 31 Jul 2019 20:20:12 +0300 Subject: [PATCH] simple_offboard: use velocity_body topic (for supporting mavros > 0.29) --- clever/src/simple_offboard.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/clever/src/simple_offboard.cpp b/clever/src/simple_offboard.cpp index 6cde374d..802fa0d6 100644 --- a/clever/src/simple_offboard.cpp +++ b/clever/src/simple_offboard.cpp @@ -220,8 +220,10 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) if (!TIMEOUT(velocity, velocity_timeout)) { try { // transform velocity + waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout); Vector3Stamped vec, vec_out; - vec.header = velocity.header; + vec.header.stamp = velocity.header.stamp; + vec.header.frame_id = velocity.header.frame_id; vec.vector = velocity.twist.linear; tf_buffer.transform(vec, vec_out, req.frame_id); @@ -761,7 +763,7 @@ int main(int argc, char **argv) // Telemetry subscribers auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage); - auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage); + auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage); auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage); auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage); auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage);