diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index 69a0c881b..d71ff5cd9 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -80,7 +80,7 @@ class LocalPositionPlugin : public MavRosPlugin { * orientation in ENU, body-fixed */ tf::Transform transform; - transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z)); + transform.setOrigin(tf::Vector3(pos_ned.x, -pos_ned.y, -pos_ned.z)); transform.setRotation(uas->get_attitude_orientation()); auto pose = boost::make_shared(); diff --git a/mavros/src/plugins/setpoint_accel.cpp b/mavros/src/plugins/setpoint_accel.cpp index e54f14086..5652cfb77 100644 --- a/mavros/src/plugins/setpoint_accel.cpp +++ b/mavros/src/plugins/setpoint_accel.cpp @@ -81,7 +81,7 @@ class SetpointAccelerationPlugin : public MavRosPlugin, ignore_all_except_a_xyz, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - afy, afx, -afz, + afx, -afy, -afz, 0.0, 0.0); } diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index c218f7e09..f3b930f6e 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -105,7 +105,7 @@ class SetpointPositionPlugin : public MavRosPlugin, set_position_target_local_ned(stamp.toNSec() / 1000000, MAV_FRAME_LOCAL_NED, ignore_all_except_xyz_y, - origin.y(), origin.x(), -origin.z(), + origin.x(), -origin.y(), -origin.z(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, tf::getYaw(q), 0.0); diff --git a/mavros/src/plugins/setpoint_velocity.cpp b/mavros/src/plugins/setpoint_velocity.cpp index 4019bf611..b9bbe56b4 100644 --- a/mavros/src/plugins/setpoint_velocity.cpp +++ b/mavros/src/plugins/setpoint_velocity.cpp @@ -72,7 +72,7 @@ class SetpointVelocityPlugin : public MavRosPlugin, MAV_FRAME_LOCAL_NED, ignore_all_except_v_xyz_yr, 0.0, 0.0, 0.0, - vy, vx, -vz, + vx, -vy, -vz, 0.0, 0.0, 0.0, 0.0, yaw_rate); } diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp index 7eaf38bb7..951b8bb53 100644 --- a/mavros_extras/src/plugins/vision_pose_estimate.cpp +++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp @@ -123,8 +123,8 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, // TODO: check conversion. Issue #49. vision_position_estimate(stamp.toNSec() / 1000, - position.y(), position.x(), -position.z(), - roll, -pitch, -yaw); // ??? please check! + position.x(), -position.y(), -position.z(), + roll, -pitch, -yaw); // ??? please check! } /* -*- callbacks -*- */ diff --git a/mavros_extras/src/plugins/visualization.cpp b/mavros_extras/src/plugins/visualization.cpp index c39a2fd9a..e8e066db7 100644 --- a/mavros_extras/src/plugins/visualization.cpp +++ b/mavros_extras/src/plugins/visualization.cpp @@ -70,7 +70,7 @@ class VisualizationPlugin : public MavRosPlugin { mavlink_msg_local_position_ned_decode(msg, &pos_ned); tf::Transform transform; - transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z)); + transform.setOrigin(tf::Vector3(pos_ned.x, -pos_ned.y, -pos_ned.z)); transform.setRotation(uas->get_attitude_orientation()); auto pose = boost::make_shared();