From e5d54a487fbca638a52d8c5a12ef4374680cdf96 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 21 Feb 2015 11:54:31 +0100
Subject: [PATCH] ros offboard attitude sp demo: move attitude

---
 CMakeLists.txt                                           | 1 +
 package.xml                                              | 2 ++
 .../demo_offboard_attitude_setpoints.cpp                 | 9 +++++----
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 01b52a436d..f015e56184 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   mav_msgs
   libmavconn
+  tf
 )
 find_package(Eigen REQUIRED)
 
diff --git a/package.xml b/package.xml
index 96d622a682..28b682c008 100644
--- a/package.xml
+++ b/package.xml
@@ -45,11 +45,13 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>eigen</build_depend>
   <build_depend>libmavconn</build_depend>
+  <build_depend>tf</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>libmavconn</run_depend>
+  <run_depend>tf</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
index 027b29a875..fb0b09de1e 100644
--- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
+++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
@@ -45,6 +45,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <std_msgs/Float64.h>
 #include <math.h>
+#include <tf/transform_datatypes.h>
 
 DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
 	_n(),
@@ -64,13 +65,13 @@ int DemoOffboardAttitudeSetpoints::main()
 
 		/* Publish example offboard attitude setpoint */
 		geometry_msgs::PoseStamped pose;
-		pose.pose.position.x = 0;
-		pose.pose.position.y = 0;
-		pose.pose.position.z = 1;
+		tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0);
+		quaternionTFToMsg(q, pose.pose.orientation);
+
 		_attitude_sp_pub.publish(pose);
 
 		std_msgs::Float64 thrust;
-		thrust.data = 0.4f + 0.25 * (sinf(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
+		thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump'
 		_thrust_sp_pub.publish(thrust);
 	}
 	return 0;