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

Jump to bottom

global_position: update pose and twist to odom msg #360

Merged
merged 3 commits into from
Aug 4, 2015
Merged

global_position: update pose and twist to odom msg #360

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8e0a72b
global_position: update pose and twist to odom msg
TSC21 Aug 4, 2015
b93d977
add nav_msgs to dependencies so to make Travis happy
TSC21 Aug 4, 2015
cc70e73
global_position: move relative_alt and compass_heading init back
TSC21 Aug 4, 2015
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 @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
sensor_msgs
nav_msgs
std_msgs
std_srvs
tf2_ros
Expand Down Expand Up @@ -147,7 +148,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES mavros
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions
DEPENDS Boost Eigen
)

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 @@ -29,6 +29,7 @@
<depend>rospy</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2_ros</depend>
Expand Down
72 changes: 37 additions & 35 deletions mavros/src/plugins/global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,10 @@
#include <eigen_conversions/eigen_msg.h>

#include <std_msgs/Float64.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>

namespace mavplugin {
Expand Down Expand Up @@ -68,8 +68,7 @@ class GlobalPositionPlugin : public MavRosPlugin {

// fused global position
gp_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
gp_pos_pub = gp_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("local", 10);
gp_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("gp_vel", 10);
gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 10);
Copy link
Member

Choose a reason for hiding this comment

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

Maybe name it "odom"? Users will confused on topic type change.

Copy link
Member Author

Choose a reason for hiding this comment

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

Well turns out to gives us local position. This is just to have congruence with global, which gives the fix topic. What do you think?

Copy link
Member

Choose a reason for hiding this comment

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

Ok, leave as is.

Copy link
Member Author

Choose a reason for hiding this comment

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

Anyhow, from what I know, no one is using data from this topic yet (fused gps data), so maybe we can keep it (also so we can then calculate the threshold from local_position/local).

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);
}
Expand All @@ -88,9 +87,8 @@ class GlobalPositionPlugin : public MavRosPlugin {

ros::Publisher raw_fix_pub;
ros::Publisher raw_vel_pub;
ros::Publisher gp_odom_pub;
ros::Publisher gp_fix_pub;
ros::Publisher gp_pos_pub;
ros::Publisher gp_vel_pub;
ros::Publisher gp_hdg_pub;
ros::Publisher gp_rel_alt_pub;

Expand Down Expand Up @@ -179,9 +177,8 @@ class GlobalPositionPlugin : public MavRosPlugin {
mavlink_global_position_int_t gpos;
mavlink_msg_global_position_int_decode(msg, &gpos);

auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
auto odom = boost::make_shared<nav_msgs::Odometry>();
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>();

Expand Down Expand Up @@ -209,22 +206,32 @@ class GlobalPositionPlugin : public MavRosPlugin {
fill_unknown_cov(fix);
}

/* Global position velocity
relative_alt->data = gpos.relative_alt / 1E3; // in meters
compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees

/* Global position odometry
*
* No transform needed:
* X: latitude m/s
* Y: longitude m/s
* Z: altitude m/s
* Assuming no transform is needed:
* X: latitude m
* Y: longitude m
* Z: altitude m
* VX: latitude vel m/s
* VY: longitude vel m/s
* VZ: altitude vel m/s
*/
vel->header = header;
odom->header = header;

// Velocity
tf::vectorEigenToMsg(
Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
vel->twist.linear);
odom->twist.twist.linear);

relative_alt->data = gpos.relative_alt / 1E3; // in meters
compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees
// Velocity covariance unknown
UAS::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
vel_cov_out.fill(0.0);
vel_cov_out(0) = -1.0;

// local position (UTM) calculation
// Local position (UTM) calculation
double northing, easting;
std::string zone;

Expand All @@ -233,18 +240,15 @@ class GlobalPositionPlugin : public MavRosPlugin {
*/
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 We really need attitude on UTM?
pose_cov->pose.pose.orientation = uas->get_attitude_orientation();
odom->pose.pose.position.x = easting;
odom->pose.pose.position.y = northing;
odom->pose.pose.position.z = relative_alt->data;
Copy link
Member

Choose a reason for hiding this comment

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

You moved assign of relative_alt->data below that line.

Copy link
Member Author

Choose a reason for hiding this comment

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

ups! fixing that now... forgot I was using this.

odom->pose.pose.orientation = uas->get_attitude_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 <<
UAS::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 ,
Expand All @@ -254,27 +258,25 @@ class GlobalPositionPlugin : public MavRosPlugin {

// publish
gp_fix_pub.publish(fix);
gp_pos_pub.publish(pose_cov);
gp_vel_pub.publish(vel);
gp_odom_pub.publish(odom);
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.stamp = odom->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;
transform.transform.rotation = odom->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;
// setOrigin()
transform.transform.translation.x = odom->pose.pose.position.x;
transform.transform.translation.y = odom->pose.pose.position.y;
transform.transform.translation.z = odom->pose.pose.position.z;

uas->tf2_broadcaster.sendTransform(transform);
}
Expand Down