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 | |
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
-rw-r--r-- | CMakeLists.txt | 1 | ||||
-rw-r--r-- | package.xml | 2 | ||||
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp | 9 |
3 files changed, 8 insertions, 4 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt index 01b52a436..f015e5618 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 96d622a68..28b682c00 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 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; |