ros: mavlink dummy node: listen to vehicle local position and publish to mavlink (LOCAL_POSITION_NED)

2 files changed, 31 insertions, 2 deletions

diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
index 8d658caa5..3485b1f4e 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.cpp
+++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp
@@ -49,6 +49,7 @@ using namespace px4;
 Mavlink::Mavlink() :
 	_n(),
 	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
+	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
 	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1))
 {
 	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
@@ -71,7 +72,7 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
 			_link->get_system_id(),
 			_link->get_component_id(),
 			_link->get_channel(),
-			&msg_m, //XXX hardcoded
+			&msg_m,
 			get_time_micros() / 1000,
 			msg->q[0],
 			msg->q[1],
@@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
 	_link->send_message(&msg_m);
 }
 
+void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg)
+{
+	mavlink_message_t msg_m;
+	mavlink_msg_local_position_ned_pack_chan(
+			_link->get_system_id(),
+			_link->get_component_id(),
+			_link->get_channel(),
+			&msg_m,
+			get_time_micros() / 1000,
+			msg->x,
+			msg->y,
+			msg->z,
+			msg->vx,
+			msg->vy,
+			msg->vz);
+	_link->send_message(&msg_m);
+}
+
 void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
 	(void)sysid;
 	(void)compid;
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h
index 2ee383b4f..a246af4a4 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.h
+++ b/src/platforms/ros/nodes/mavlink/mavlink.h
@@ -43,6 +43,7 @@
 #include "ros/ros.h"
 #include <mavconn/interface.h>
 #include <px4/vehicle_attitude.h>
+#include <px4/vehicle_local_position.h>
 #include <px4/vehicle_attitude_setpoint.h>
 #include <px4/offboard_control_mode.h>
 
@@ -61,17 +62,26 @@ protected:
 	ros::NodeHandle _n;
 	mavconn::MAVConnInterface::Ptr _link;
 	ros::Subscriber _v_att_sub;
+	ros::Subscriber _v_local_pos_sub;
 	ros::Publisher _v_att_sp_pub;
 	ros::Publisher _offboard_control_mode_pub;
 
 	/**
 	 *
 	 * Simulates output of attitude data from the FCU
-	 * Equivalent to the mavlink stream ATTITUDE
+	 * Equivalent to the mavlink stream ATTITUDE_QUATERNION
 	 *
 	 * */
 	void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg);
 
+	/**
+	 *
+	 * Simulates output of local position data from the FCU
+	 * Equivalent to the mavlink stream LOCAL_POSITION_NED
+	 *
+	 * */
+	void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg);
+
 
 	/**
 	 *