diff options
Diffstat (limited to 'src/platforms')
-rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 18 |
1 files changed, 11 insertions, 7 deletions
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index bde436aca..163b111be 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -46,8 +46,8 @@ AttitudeEstimator::AttitudeEstimator() : _n(), - _sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)), - _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10)) + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1)) { } @@ -62,23 +62,27 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP //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; + 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); msg_v_att.q[2] = quat(2); msg_v_att.q[3] = quat(3); - math::Matrix<3, 3> Rot = quat.to_dcm(); + math::Matrix<3, 3> rot = quat.to_dcm(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - PX4_R(msg_v_att.R, i, j) = Rot(i, j); + PX4_R(msg_v_att.R, i, j) = rot(i, j); } } - msg_v_att.R_valid = true; + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + _vehicle_attitude_pub.publish(msg_v_att); } |