diff options
Diffstat (limited to 'src/platforms/ros/nodes')
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.h | 4 |
2 files changed, 26 insertions, 2 deletions
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index b6413c3b4..131a4930f 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -47,10 +47,11 @@ using namespace px4; Mavlink::Mavlink() : - _n() + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); } int main(int argc, char **argv) @@ -60,3 +61,22 @@ int main(int argc, char **argv) ros::spin(); return 0; } + +void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_attitude_quaternion_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, //XXX hardcoded + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); + _link->send_message(&msg_m); +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 5b3946887..e683597a9 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -42,6 +42,7 @@ #include "ros/ros.h" #include <mavconn/interface.h> +#include <px4/vehicle_attitude.h> namespace px4 { @@ -57,6 +58,9 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; + ros::Subscriber _v_att_sub; + + void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } |