attitude estimator: fix readin of pose

1 files changed, 6 insertions, 5 deletions

diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
index 99d1ae024..bde436aca 100644
--- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -46,7 +46,7 @@
 
 AttitudeEstimator::AttitudeEstimator() :
 	_n(),
-	_sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
+	_sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
 	_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
 {
 }
@@ -59,10 +59,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
 
 	/* Convert quaternion to rotation matrix */
 	math::Quaternion quat;
-	quat(0) = (float)msg->pose[0].orientation.w;
-	quat(1) = (float)msg->pose[0].orientation.x;
-	quat(2) = (float)msg->pose[0].orientation.y;
-	quat(3) = (float)msg->pose[0].orientation.z;
+	//XXX: search for vtol or other (other than 'plane') vehicle here
+	quat(0) = (float)msg->pose[1].orientation.w;
+	quat(1) = (float)msg->pose[1].orientation.x;
+	quat(2) = (float)msg->pose[1].orientation.y;
+	quat(3) = (float)msg->pose[1].orientation.z;
 
 	msg_v_att.q[0] = quat(0);
 	msg_v_att.q[1] = quat(1);