diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-30 14:53:53 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-30 14:53:53 +0100 |
| commit | 16c66669c257767173d52febd0f526c0e718e004 (patch) | |
| tree | 497496aceab86ba08ac19341f58a1743ea6f9425 | |
| parent | 2623ec156f30bf6f5734fc965724d5d6f1d3f8ce (diff) | |
| download | px4-firmware-16c66669c257767173d52febd0f526c0e718e004.tar.gz px4-firmware-16c66669c257767173d52febd0f526c0e718e004.tar.bz2 px4-firmware-16c66669c257767173d52febd0f526c0e718e004.zip | |
dummy attitude estimator copies attitude from gazebo
| -rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 1 | ||||
| -rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 2 | ||||
| -rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 31 |
3 files changed, 29 insertions, 5 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1e76aae60..ddadf707c 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -52,6 +52,7 @@ #include <platforms/ros/eigen_math.h> #include <Eigen/Eigen> #endif +#include <platforms/px4_defines.h> namespace math { diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 20f099831..781c14d53 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -52,6 +52,8 @@ #include <platforms/ros/eigen_math.h> #endif +#include <platforms/px4_defines.h> + namespace math { diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index e64ba335d..99d1ae024 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -33,14 +33,16 @@ /** * @file att_estimator.cpp - * Dummy attitude estimator that forwards attitude from gazebo to px4 topic * * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Roman Bapst <romanbapst@yahoo.de> */ #include "attitude_estimator.h" #include <px4/vehicle_attitude.h> +#include <mathlib/mathlib.h> +#include <platforms/px4_defines.h> AttitudeEstimator::AttitudeEstimator() : _n(), @@ -51,13 +53,32 @@ AttitudeEstimator::AttitudeEstimator() : void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg) { - px4::vehicle_attitude msg_out; + px4::vehicle_attitude msg_v_att; /* Fill px4 attitude topic with contents from modelstates topic */ - ROS_INFO("Test x: %.4f", msg->pose[0].orientation.x); - //XXX - _vehicle_attitude_pub.publish(msg_out); + /* 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; + + 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(); + 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); + } + } + + msg_v_att.R_valid = true; + + _vehicle_attitude_pub.publish(msg_v_att); } int main(int argc, char **argv) |