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