From 0ea60f56e9f52ea2ee5eac41183fea09720d9e7c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Dec 2014 15:55:39 +0100 Subject: attitude estimator: fix readin of pose --- .../ros/nodes/attitude_estimator/attitude_estimator.cpp | 11 ++++++----- 1 file 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("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); -- cgit v1.2.3