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

Simplify publish code

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-08 11:53:59 +03:00
parent 3ebb5faa2a
commit 6899093c43
1 changed files with 10 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
clover/src/simple_offboard.cpp
View File

@@ -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 +