From 6899093c43972165c4301898ddfe73c03ea105f1 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 8 Dec 2022 11:53:59 +0300 Subject: [PATCH] Simplify publish code --- clover/src/simple_offboard.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 6983875c..8731ea1d 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -424,17 +424,6 @@ void publish(const ros::Time stamp) } } - // transform velocity - if (setpoint_type == VELOCITY) { - setpoint_velocity.header.stamp = stamp; - try { - tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05)); - } catch (tf2::TransformException& ex) { - // can't transform velocity, use last known - ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); - } - } - if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position); @@ -486,6 +475,16 @@ void publish(const ros::Time stamp) } if (setpoint_type == VELOCITY) { + // transform velocity to local frame + setpoint_velocity.header.stamp = stamp; + try { + tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05)); + } catch (tf2::TransformException& ex) { + // can't transform velocity, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); + } + + // publish velocity position_raw_msg.type_mask = PositionTarget::IGNORE_PX + PositionTarget::IGNORE_PY + PositionTarget::IGNORE_PZ +