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 2 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
3 changes: 2 additions & 1 deletion mavros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
sensor_msgs
nav_msgs
geographic_msgs
std_msgs
std_srvs
tf2_ros
Expand Down Expand Up @@ -65,7 +66,7 @@ catkin_python_setup()
catkin_package(
INCLUDE_DIRS include
LIBRARIES mavros
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions mavros_msgs
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs geographic_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions mavros_msgs
DEPENDS Boost EIGEN3
)

Expand Down
1 change: 1 addition & 0 deletions mavros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
<depend>mavros_msgs</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geographic_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>

Expand Down
44 changes: 43 additions & 1 deletion mavros/src/plugins/global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <sensor_msgs/NavSatStatus.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <geographic_msgs/GeoPointStamped.h>

namespace mavros {
namespace std_plugins {
Expand Down Expand Up @@ -70,14 +71,18 @@ class GlobalPositionPlugin : public plugin::PluginBase {
gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 10);
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_set_global_origin_sub = gp_nh.subscribe("set_gp_origin", 10, &GlobalPositionPlugin::set_gp_origin_cb, this);
}

Subscriptions get_subscriptions()
{
return {
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_global_position_int),
make_handler(&GlobalPositionPlugin::handle_gps_global_origin)
};
}

Expand All @@ -90,6 +95,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
ros::Publisher gp_fix_pub;
ros::Publisher gp_hdg_pub;
ros::Publisher gp_rel_alt_pub;
ros::Publisher gp_global_origin_pub;

ros::Subscriber gp_set_global_origin_sub;

std::string frame_id; //!< frame for topic headers
std::string tf_frame_id; //!< origin for TF
Expand Down Expand Up @@ -168,6 +176,20 @@ 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

g_origin->header.frame_id = frame_id;
g_origin->header.stamp = ros::Time::now();
g_origin->position.latitude = (double)glob_orig.latitude; // @warning TODO: #529
g_origin->position.longitude = (double)glob_orig.longitude;
g_origin->position.altitude = (double)glob_orig.altitude;
Copy link
Member

Choose a reason for hiding this comment

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

Read message documentation! That is not how you should convert int32 encoded coord's to floating point.


gp_global_origin_pub.publish(g_origin);
}

/** @todo Handler for GLOBAL_POSITION_INT_COV */

void handle_global_position_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GLOBAL_POSITION_INT &gpos)
Expand Down Expand Up @@ -307,6 +329,26 @@ class GlobalPositionPlugin : public plugin::PluginBase {
else
stat.add("EPV (m)", "Unknown");
}

/* -*- callbacks -*- */

void set_gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &req)
{
mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo;

gpo.target_system = m_uas->get_tgt_system();
// gpo.time_boot_ms = stamp.toNSec() / 1000; #TODO: requires Mavlink msg update
// [[[cog:
// for f in ('latitude', 'longitude', 'altitude'):
// cog.outl("gpo.%s = req->%s;" % (f, f))
// ]]]
gpo.latitude = (int32_t)req->position.latitude;
gpo.longitude = (int32_t)req->position.longitude;
gpo.altitude = (int32_t)req->position.altitude;
// [[[end]]] (checksum: 283da7efac0ea8cccd0d5e144ca29a03)

UAS_FCU(m_uas)->send_message_ignore_drop(gpo);
}
};
} // namespace std_plugins
} // namespace mavros
Expand Down