CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-06 17:49:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix yaw value in rewritten fcu_horiz

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-19 20:13:28 +03:00
parent 094f96b096
commit b89574c6c5
2 changed files with 26 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
clever/src/fcu_horiz.cpp
View File

@@ -4,6 +4,8 @@
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "util.h"
class FcuHoriz : public nodelet::Nodelet
{
geometry_msgs::TransformStamped t_;
@@ -11,11 +13,18 @@ class FcuHoriz : public nodelet::Nodelet
void handlePose(const geometry_msgs::PoseStampedConstPtr& msg)
{
static tf2_ros::TransformBroadcaster br;
double roll, pitch, yaw;
t_.header.stamp = msg->header.stamp;
t_.header.frame_id = msg->header.frame_id;
t_.transform.translation.x = msg->pose.position.x;
t_.transform.translation.y = msg->pose.position.y;
t_.transform.translation.z = msg->pose.position.z;
// Warning: this is not thead-safe
quaternionToEuler(msg->pose.orientation, roll, pitch, yaw);
eulerToQuaternion(t_.transform.rotation, 0, 0, yaw);
br.sendTransform(t_);
}

17
clever/src/util.h Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
inline void quaternionToEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw)
{
tf::Quaternion tfq(q.x, q.y, q.z, q.w);
tf::Matrix3x3 m(tfq);
m.getRPY(roll, pitch, yaw);
}
inline void eulerToQuaternion(geometry_msgs::Quaternion& q, double roll, double pitch, double yaw)
{
tf::Quaternion tfq(roll, pitch, yaw);
quaternionTFToMsg(tfq, q);
}