diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-21 11:54:31 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:25:36 +0100 |
commit | e5d54a487fbca638a52d8c5a12ef4374680cdf96 (patch) | |
tree | 2ab9376d3f791c07bb4e1bdbf44174f23de7bb89 /src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp | |
parent | 4bf3107faf50f821cda8715252357754b0ee844a (diff) | |
download | px4-firmware-e5d54a487fbca638a52d8c5a12ef4374680cdf96.tar.gz px4-firmware-e5d54a487fbca638a52d8c5a12ef4374680cdf96.tar.bz2 px4-firmware-e5d54a487fbca638a52d8c5a12ef4374680cdf96.zip |
ros offboard attitude sp demo: move attitude
Diffstat (limited to 'src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp')
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp | 9 |
1 files changed, 5 insertions, 4 deletions
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 027b29a87..fb0b09de1 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; |