Changes some frames from world to body conversion for NED to ENU.

mavlink · Jun 11, 2015 · 030c4c6 · 030c4c6
1 parent b6927c3
commit 030c4c6
Show file tree

Hide file tree

Showing 6 changed files with 7 additions and 7 deletions.
diff --git 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<geometry_msgs::PoseStamped>();

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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<geometry_msgs::PoseStamped>();