Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] Set framework to define offset between global origin and current local position #691

Merged
merged 15 commits into from
Jun 7, 2017
Merged

[WIP] Set framework to define offset between global origin and current local position #691

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
92b275c
add handlers for GPS_GLOBAL_ORIGIN and SET_GPS_GLOBAL_ORIGIN
TSC21 Apr 6, 2017
c4941a9
fix cast of encoding types
TSC21 Apr 7, 2017
5828670
refactor gps coord conversions
TSC21 Apr 7, 2017
5ec3c56
uncrustify
TSC21 Apr 7, 2017
55315c4
Merge branch 'master' of https://github.com/mavlink/mavros into globa…
TSC21 May 16, 2017
b732739
global_position: add LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET handler
TSC21 May 16, 2017
64111d4
global_position: add trasform sender for offset
TSC21 May 16, 2017
8215c7c
global_origin: refactor covariance matrix
TSC21 May 16, 2017
cbf87be
global_position: update copyright
TSC21 May 27, 2017
9420462
global_position: add initial support to REP 105
TSC21 May 27, 2017
35b0817
px4_config: global_position: update frame description
TSC21 May 27, 2017
555c3f1
global_position: correct identation
TSC21 May 27, 2017
855835e
global position: be consistent with frame and methods names (ecef!=wg…
TSC21 Jun 6, 2017
fd1fe5e
global_position: updates to code structure
TSC21 Jun 6, 2017
4f4127a
global_position: fix identation
TSC21 Jun 6, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions mavros/launch/px4_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ global_position:
tf:
send: false # send TF?
frame_id: "local_origin" # TF frame_id
global_frame_id: "global_origin" # TF frame_id
child_frame_id: "fcu_utm" # TF child_frame_id

# imu_pub
Expand Down
61 changes: 51 additions & 10 deletions mavros/src/plugins/global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <geographic_msgs/GeoPointStamped.h>
Expand Down Expand Up @@ -56,6 +57,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
// tf subsection
gp_nh.param("tf/send", tf_send, true);
gp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
gp_nh.param<std::string>("tf/global_frame_id", tf_global_frame_id, "global_map");
gp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");

UAS_DIAG(m_uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);
Expand All @@ -70,6 +72,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
gp_global_origin_pub = gp_nh.advertise<geographic_msgs::GeoPointStamped>("gp_origin", 10);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not latching type?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what do you mean with latching type?

gp_global_offset_pub = gp_nh.advertise<geometry_msgs::PoseStamped>("gp_lp_offset", 10);

gp_set_global_origin_sub = gp_nh.subscribe("set_gp_origin", 10, &GlobalPositionPlugin::set_gp_origin_cb, this);
}
Expand All @@ -80,7 +83,8 @@ class GlobalPositionPlugin : public plugin::PluginBase {
make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
make_handler(&GlobalPositionPlugin::handle_global_position_int),
make_handler(&GlobalPositionPlugin::handle_gps_global_origin)
make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
};
}

Expand All @@ -94,11 +98,13 @@ class GlobalPositionPlugin : public plugin::PluginBase {
ros::Publisher gp_hdg_pub;
ros::Publisher gp_rel_alt_pub;
ros::Publisher gp_global_origin_pub;
ros::Publisher gp_global_offset_pub;

ros::Subscriber gp_set_global_origin_sub;

std::string frame_id; //!< frame for topic headers
std::string tf_frame_id; //!< origin for TF
std::string tf_global_frame_id; //!< global origin for TF
std::string tf_child_frame_id; //!< frame for TF and Pose
bool tf_send;
double rot_cov;
Expand Down Expand Up @@ -191,11 +197,11 @@ class GlobalPositionPlugin : public plugin::PluginBase {
void handle_gps_global_origin(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_GLOBAL_ORIGIN &glob_orig)
{
auto g_origin = boost::make_shared<geographic_msgs::GeoPointStamped>();
// auto header = m_uas->synchronized_header(frame_id, gpos.time_boot_ms); #TODO: requires Mavlink msg update
// auto header = m_uas->synchronized_header(frame_id, glob_orig.time_boot_ms); #TODO: requires Mavlink msg update

g_origin->header.frame_id = frame_id;
g_origin->header.stamp = ros::Time::now();
fill_lla_wgs84(glob_orig, g_origin); // @warning TODO: #529
fill_lla_wgs84(glob_orig, g_origin); // @warning TODO: #693

gp_global_origin_pub.publish(g_origin);
}
Expand Down Expand Up @@ -276,12 +282,12 @@ class GlobalPositionPlugin : public plugin::PluginBase {
ftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());
pos_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;
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 ;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm, we probably may do that:

pos_cov_out.setZero();
pos_cov_out.block<3, 3>(0, 0) = gps_cov;
pos_cov_out.block<3, 3>(3, 3).diagonal() = rot_cov;

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah. more reasonable


// publish
gp_fix_pub.publish(fix);
Expand Down Expand Up @@ -309,6 +315,41 @@ class GlobalPositionPlugin : public plugin::PluginBase {
}
}

void handle_lpned_system_global_offset(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET &offset)
{
auto global_offset = boost::make_shared<geometry_msgs::PoseStamped>();
global_offset->header = m_uas->synchronized_header(tf_global_frame_id, offset.time_boot_ms);

auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z));
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(
ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw)));

tf::pointEigenToMsg(enu_position, global_offset->pose.position);
tf::quaternionEigenToMsg(enu_baselink_orientation, global_offset->pose.orientation);

gp_global_offset_pub.publish(global_offset);

// TF
if (tf_send) {
geometry_msgs::TransformStamped transform;

transform.header.stamp = global_offset->header.stamp;
transform.header.frame_id = tf_global_frame_id;
transform.child_frame_id = tf_frame_id;

// setRotation()
transform.transform.rotation = global_offset->pose.orientation;

// setOrigin()
transform.transform.translation.x = global_offset->pose.position.x;
transform.transform.translation.y = global_offset->pose.position.y;
transform.transform.translation.z = global_offset->pose.position.z;

m_uas->tf2_broadcaster.sendTransform(transform);
}
}

/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
Expand Down Expand Up @@ -348,7 +389,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {

gpo.target_system = m_uas->get_tgt_system();
// gpo.time_boot_ms = stamp.toNSec() / 1000; #TODO: requires Mavlink msg update
fill_lla_amsl(gpo, req);// @warning TODO: #529
fill_lla_amsl(gpo, req);// @warning TODO: #693

UAS_FCU(m_uas)->send_message_ignore_drop(gpo);
}
Expand Down
1 change: 0 additions & 1 deletion mavros/src/plugins/hil_actuator_controls.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,4 +70,3 @@ class HilActuatorControlsPlugin : public plugin::PluginBase {

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HilActuatorControlsPlugin, mavros::plugin::PluginBase)