Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Thomas Gubler
2015-02-15 11:40:07 +01:00
parent 8d36305f8b
commit ca250d21eb
2 changed files with 31 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/mavlink/mavlink.cpp
+20 -1
View File
@@ -49,6 +49,7 @@ using namespace px4;
Mavlink::Mavlink() : Mavlink::Mavlink() :
_n(), _n(),
_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), _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)) _offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1))
{ {
_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _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_system_id(),
_link->get_component_id(), _link->get_component_id(),
_link->get_channel(), _link->get_channel(),
&msg_m, //XXX hardcoded &msg_m,
get_time_micros() / 1000, get_time_micros() / 1000,
msg->q[0], msg->q[0],
msg->q[1], msg->q[1],
@@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
_link->send_message(&msg_m); _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 Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
(void)sysid; (void)sysid;
(void)compid; (void)compid;
src/platforms/ros/nodes/mavlink/mavlink.h
+11 -1
View File
@@ -43,6 +43,7 @@
#include "ros/ros.h" #include "ros/ros.h"
#include <mavconn/interface.h> #include <mavconn/interface.h>
#include <px4/vehicle_attitude.h> #include <px4/vehicle_attitude.h>
#include <px4/vehicle_local_position.h>
#include <px4/vehicle_attitude_setpoint.h> #include <px4/vehicle_attitude_setpoint.h>
#include <px4/offboard_control_mode.h> #include <px4/offboard_control_mode.h>
@@ -61,17 +62,26 @@ protected:
ros::NodeHandle _n; ros::NodeHandle _n;
mavconn::MAVConnInterface::Ptr _link; mavconn::MAVConnInterface::Ptr _link;
ros::Subscriber _v_att_sub; ros::Subscriber _v_att_sub;
ros::Subscriber _v_local_pos_sub;
ros::Publisher _v_att_sp_pub; ros::Publisher _v_att_sp_pub;
ros::Publisher _offboard_control_mode_pub; ros::Publisher _offboard_control_mode_pub;
/** /**
* *
* Simulates output of attitude data from the FCU * 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); 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);
/** /**
* *