void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
auto orientation = uas->get_attitude_orientation();
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
tf::pointEigenToMsg(position, pose->pose.position);
// XXX FIX ME #319
tf::quaternionTFToMsg(orientation, pose->pose.orientation);
local_position.publish(pose);
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
transform.transform.rotation = pose->pose.orientation;
tf::vectorEigenToMsg(position, transform.transform.translation);
tf2_broadcaster.sendTransform(transform);
}
}
void handle_timesync(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync);
uint64_t now_ns = ros::Time::now().toNSec();
if (tsync.tc1 == 0) {
send_timesync_msg(now_ns, tsync.ts1);
return;
}
else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2;
int64_t dt = time_offset_ns - offset_ns;
if (std::abs(dt) > 10000000) { // 10 millisecond skew
time_offset_ns = offset_ns; // hard-set it.
uas->set_time_offset(time_offset_ns);
dt_diag.clear();
dt_diag.set_timestamp(tsync.tc1);
ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). "
"Hard syncing clocks.", dt / 1e9);
}
else {
average_offset(offset_ns);
dt_diag.tick(dt, tsync.tc1, time_offset_ns);
uas->set_time_offset(time_offset_ns);
}
}
}
void command_int(bool broadcast,
uint8_t frame, uint16_t command,
uint8_t current, uint8_t autocontinue,
float param1, float param2,
float param3, float param4,
int32_t x, int32_t y,
float z)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
frame,
command,
current,
autocontinue,
param1, param2,
param3, param4,
x, y, z);
UAS_FCU(uas)->send_message(&msg);
}
void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (!uas->is_my_target(sysid)) {
ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
return;
}
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
// update context && setup connection timeout
uas->update_heartbeat(hb.type, hb.autopilot);
uas->update_connection_status(true);
timeout_timer.stop();
timeout_timer.start();
// build state message after updating uas
auto state_msg = boost::make_shared<mavros::State>();
state_msg->header.stamp = ros::Time::now();
state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);
state_pub.publish(state_msg);
hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
}
void command_long(bool broadcast,
uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
command,
confirmation_fixed,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg);
}
void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type > 2)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else {
ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}
fill_lla(raw_gps, fix);
float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;
if (!isnan(eph)) {
const double hdop = eph;
// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
}
// store & publish
uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
raw_fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
raw_vel_pub.publish(vel);
}
}
/**
* Common function for command service callbacks.
*
* NOTE: success is bool in messages, but has unsigned char type in C++
*/
bool send_command_long_and_wait(uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7,
unsigned char &success, uint8_t &result) {
unique_lock lock(mutex);
/* check transactions */
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == command) {
ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command);
return false;
}
//! @note APM always send COMMAND_ACK, while PX4 never.
bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
if (is_ack_required)
ack_waiting_list.push_back(new CommandTransaction(command));
command_long(command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required) {
auto it = ack_waiting_list.begin();
for (; it != ack_waiting_list.end(); it++)
if ((*it)->expected_command == command)
break;
if (it == ack_waiting_list.end()) {
ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command);
return false;
}
lock.unlock();
bool is_not_timeout = wait_ack_for(*it);
lock.lock();
success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
result = (*it)->result;
delete *it;
ack_waiting_list.erase(it);
}
else {
success = true;
result = MAV_RESULT_ACCEPTED;
}
return true;
}
void uas_store_attitude(tf::Quaternion &orientation,
geometry_msgs::Vector3 &gyro_vec,
geometry_msgs::Vector3 &acc_vec)
{
tf::Vector3 angular_velocity;
tf::Vector3 linear_acceleration;
tf::vector3MsgToTF(gyro_vec, angular_velocity);
tf::vector3MsgToTF(acc_vec, linear_acceleration);
uas->set_attitude_orientation(orientation);
uas->set_attitude_angular_velocity(angular_velocity);
uas->set_attitude_linear_acceleration(linear_acceleration);
}
void connection_cb(bool connected) {
// if connection changes, start delayed version request
version_retries = RETRIES_COUNT;
if (connected)
autopilot_version_timer.start();
else
autopilot_version_timer.stop();
// add/remove APM diag tasks
if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
"Extra diagnostic disabled.");
#else
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
}
else {
UAS_DIAG(uas).removeByName(mem_diag.getName());
UAS_DIAG(uas).removeByName(hwst_diag.getName());
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
}
}
/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
int fix_type, satellites_visible;
float eph, epv;
uas->get_gps_epts(eph, epv, fix_type, satellites_visible);
if (satellites_visible <= 0)
stat.summary(2, "No satellites");
else if (fix_type < 2)
stat.summary(1, "No fix");
else if (fix_type == 2)
stat.summary(0, "2D fix");
else if (fix_type >= 3)
stat.summary(0, "3D fix");
stat.addf("Satellites visible", "%zd", satellites_visible);
stat.addf("Fix type", "%d", fix_type);
if (!isnan(eph))
stat.addf("EPH (m)", "%.2f", eph);
else
stat.add("EPH (m)", "Unknown");
if (!isnan(epv))
stat.addf("EPV (m)", "%.2f", epv);
else
stat.add("EPV (m)", "Unknown");
}
/**
* Send transform to FCU position controller
*
* Note: send only XYZ, Yaw
*/
void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
// ENU frame
tf::Vector3 origin = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
/* Documentation start from bit 1 instead 0,
* Ignore velocity and accel vectors, yaw rate
*/
uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
if (uas->is_px4()) {
/**
* Current PX4 has bug: it cuts throttle if there no velocity sp
* Issue #273.
*
* @todo Revesit this quirk later. Should be fixed in firmware.
*/
ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
}
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyz_y,
origin.y(), origin.x(), -origin.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
tf::getYaw(q), 0.0);
}
void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
/*
* Arming/disarming the UAV
*/
void arming(const mms::Arm::ConstPtr msg){
mavlink_message_t msg_mav;
if (msg->arm_disarm){
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 1; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav);
ROS_INFO("Arming UAV");
} else {
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 0; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav); //TODO decide if send or not disarm by software
ROS_INFO("Disarming UAV");
}
}
void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_statustext_t textm;
mavlink_msg_statustext_decode(msg, &textm);
std::string text(textm.text,
strnlen(textm.text, sizeof(textm.text)));
if (uas->is_ardupilotmega())
process_statustext_apm_quirk(textm.severity, text);
else
process_statustext_normal(textm.severity, text);
}
void handle_cam_trig(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_camera_trigger_t ctrig;
mavlink_msg_camera_trigger_decode(msg, &ctrig);
auto sync_msg = boost::make_shared<mavros_extras::CamIMUStamp>();
sync_msg->frame_stamp = uas->synchronise_stamp(ctrig.time_usec);
sync_msg->frame_seq_id = ctrig.seq;
cam_imu_pub.publish(sync_msg);
ROS_INFO("Sent timestamp %u.%u:",sync_msg->frame_stamp.sec,sync_msg->frame_stamp.nsec);
}
bool set_mode_cb(mavros::SetMode::Request &req,
mavros::SetMode::Response &res) {
mavlink_message_t msg;
uint8_t base_mode = req.base_mode;
uint32_t custom_mode = 0;
if (req.custom_mode != "") {
if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
res.success = false;
return true;
}
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
}
mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
uas->get_tgt_system(),
base_mode,
custom_mode);
UAS_FCU(uas)->send_message(&msg);
res.success = true;
return true;
}
void handle_vibration(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_vibration_t vibration;
mavlink_msg_vibration_decode(msg, &vibration);
auto vibe_msg = boost::make_shared<mavros_msgs::Vibration>();
vibe_msg->header = uas->synchronized_header(frame_id, vibration.time_usec);
// TODO no transform_frame?
vibe_msg->vibration.x = vibration.vibration_x;
vibe_msg->vibration.y = vibration.vibration_y;
vibe_msg->vibration.z = vibration.vibration_z;
vibe_msg->clipping[0] = vibration.clipping_0;
vibe_msg->clipping[1] = vibration.clipping_1;
vibe_msg->clipping[2] = vibration.clipping_2;
vibration_pub.publish(vibe_msg);
}
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
tf::Transform transform;
transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));
transform.setRotation(uas->get_attitude_orientation());
geometry_msgs::PoseStampedPtr pose = boost::make_shared<geometry_msgs::PoseStamped>();
tf::poseTFToMsg(transform, pose->pose);
pose->header.frame_id = fixed_frame_id;
pose->header.stamp = ros::Time();
publish_vehicle_marker();
publish_vis_marker(pose);
}
void handle_raw_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (has_hr_imu || has_scaled_imu)
return;
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
mavlink_raw_imu_t imu_raw;
mavlink_msg_raw_imu_decode(msg, &imu_raw);
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
/* NOTE: APM send SCALED_IMU data as RAW_IMU */
fill_imu_msg_raw(imu_msg,
imu_raw.xgyro * MILLIRS_TO_RADSEC,
-imu_raw.ygyro * MILLIRS_TO_RADSEC,
-imu_raw.zgyro * MILLIRS_TO_RADSEC,
imu_raw.xacc * MILLIG_TO_MS2,
-imu_raw.yacc * MILLIG_TO_MS2,
-imu_raw.zacc * MILLIG_TO_MS2);
if (!uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "RAW_IMU: linear acceleration known on APM only");
linear_accel_vec.x = 0.0;
linear_accel_vec.y = 0.0;
linear_accel_vec.z = 0.0;
}
imu_msg->header = header;
imu_raw_pub.publish(imu_msg);
/* -*- magnetic vector -*- */
sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
// Convert from local NED plane to ENU
magn_msg->magnetic_field.x = imu_raw.ymag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = imu_raw.xmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.z = -imu_raw.zmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field_covariance = magnetic_cov;
magn_msg->header = header;
magn_pub.publish(magn_msg);
}
void handle_autopilot_version(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_autopilot_version_t apv;
mavlink_msg_autopilot_version_decode(msg, &apv);
autopilot_version_timer.stop();
uas->update_capabilities(true, apv.capabilities);
// Note based on current APM's impl.
// APM uses custom version array[8] as a string
ROS_INFO_NAMED("sys", "VER: Capabilities 0x%016llx", (long long int)apv.capabilities);
ROS_INFO_NAMED("sys", "VER: Flight software: %08x (%*s)",
apv.flight_sw_version,
8, apv.flight_custom_version);
ROS_INFO_NAMED("sys", "VER: Middleware software: %08x (%*s)",
apv.middleware_sw_version,
8, apv.middleware_custom_version);
ROS_INFO_NAMED("sys", "VER: OS software: %08x (%*s)",
apv.os_sw_version,
8, apv.os_custom_version);
ROS_INFO_NAMED("sys", "VER: Board hardware: %08x", apv.board_version);
ROS_INFO_NAMED("sys", "VER: VID/PID: %04x:%04x", apv.vendor_id, apv.product_id);
ROS_INFO_NAMED("sys", "VER: UID: %016llx", (long long int)apv.uid);
}
void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_global_position_int_t gpos;
mavlink_msg_global_position_int_decode(msg, &gpos);
auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
auto relative_alt = boost::make_shared<std_msgs::Float64>();
auto compass_heading = boost::make_shared<std_msgs::Float64>();
auto header = uas->synchronized_header(frame_id, gpos.time_boot_ms);
// Global position fix
fix->header = header;
fill_lla(gpos, fix);
// fill GPS status fields using GPS_RAW data
auto raw_fix = uas->get_gps_fix();
if (raw_fix) {
fix->status.service = raw_fix->status.service;
fix->status.status = raw_fix->status.status;
fix->position_covariance = raw_fix->position_covariance;
fix->position_covariance_type = raw_fix->position_covariance_type;
}
else {
// no GPS_RAW_INT -> fix status unknown
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
// we don't know covariance
fill_unknown_cov(fix);
}
/* Global position velocity
*
* No transform needed:
* X: latitude m/s
* Y: longitude m/s
* Z: altitude m/s
*/
vel->header = header;
tf::vectorEigenToMsg(
Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
vel->twist.linear);
relative_alt->data = gpos.relative_alt / 1E3; // in meters
compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees
// local position (UTM) calculation
double northing, easting;
std::string zone;
/** @note Adapted from gps_umd ROS package @http://wiki.ros.org/gps_umd
* Author: Ken Tossell <ken AT tossell DOT net>
*/
UTM::LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);
pose_cov->header = header;
pose_cov->pose.pose.position.x = easting;
pose_cov->pose.pose.position.y = northing;
pose_cov->pose.pose.position.z = relative_alt->data;
// XXX FIX ME #319, We really need attitude on UTM?
tf::quaternionTFToMsg(uas->get_attitude_orientation(), pose_cov->pose.pose.orientation);
// Use ENU covariance to build XYZRPY covariance
UAS::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
UAS::EigenMapCovariance6d cov_out(pose_cov->pose.covariance.data());
cov_out <<
gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0 , 0.0 , 0.0 ,
gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0 , 0.0 , 0.0 ,
gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , rot_cov , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , rot_cov , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , rot_cov ;
// publish
gp_fix_pub.publish(fix);
gp_pos_pub.publish(pose_cov);
gp_vel_pub.publish(vel);
gp_rel_alt_pub.publish(relative_alt);
gp_hdg_pub.publish(compass_heading);
// TF
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose_cov->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
// XXX do we really need rotation in UTM coordinates?
// setRotation()
transform.transform.rotation = pose_cov->pose.pose.orientation;
// setOrigin(), why to do transform_frame?
transform.transform.translation.x = pose_cov->pose.pose.position.x;
transform.transform.translation.y = pose_cov->pose.pose.position.y;
transform.transform.translation.z = pose_cov->pose.pose.position.z;
uas->tf2_broadcaster.sendTransform(transform);
}
}
/**
* Receive distance sensor data from FCU.
*/
void handle_distance_sensor(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_distance_sensor_t dist_sen;
mavlink_msg_distance_sensor_decode(msg, &dist_sen);
auto it = sensor_map.find(dist_sen.id);
if (it == sensor_map.end()) {
ROS_ERROR_NAMED("distance_sensor",
"DS: no mapping for sensor id: %d, type: %d, orientation: %d",
dist_sen.id, dist_sen.type, dist_sen.orientation);
return;
}
auto sensor = it->second;
if (sensor->is_subscriber) {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s (id %d) is subscriber, but i got sensor data for that id from FCU",
sensor->topic_name.c_str(), sensor->sensor_id);
return;
}
if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s: received sensor data has different orientation (%s) than in config (%s)!",
sensor->topic_name.c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation)).c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(sensor->orientation)).c_str());
}
auto range = boost::make_shared<sensor_msgs::Range>();
range->header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms);
range->min_range = dist_sen.min_distance * 1E-2; // in meters
range->max_range = dist_sen.max_distance * 1E-2;
range->field_of_view = sensor->field_of_view;
if (dist_sen.type == MAV_DISTANCE_SENSOR_LASER) {
range->radiation_type = sensor_msgs::Range::INFRARED;
}
else if (dist_sen.type == MAV_DISTANCE_SENSOR_ULTRASOUND) {
range->radiation_type = sensor_msgs::Range::ULTRASOUND;
}
else {
ROS_ERROR_NAMED("distance_sensor",
"DS: %s: Wrong/undefined type of sensor (type: %d). Droping!...",
sensor->topic_name.c_str(), dist_sen.type);
return;
}
range->range = dist_sen.current_distance * 1E-2; // in meters
if (sensor->send_tf) {
/* variables init */
auto q = UAS::sensor_orientation_matching(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation));
geometry_msgs::TransformStamped transform;
transform.header = uas->synchronized_header("fcu", dist_sen.time_boot_ms);
transform.child_frame_id = sensor->frame_id;
/* rotation and position set */
tf::quaternionEigenToMsg(q, transform.transform.rotation);
tf::vectorEigenToMsg(sensor->position, transform.transform.translation);
/* transform broadcast */
uas->tf2_broadcaster.sendTransform(transform);
}
sensor->pub.publish(range);
}
void timeout_cb(const ros::TimerEvent &event) {
uas->update_connection_status(false);
}