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_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);
}
}
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_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);
}