Skip to content

Commit

Permalink
first commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@TSC21
TSC21 committed Apr 15, 2017
1 parent 2d5eea2 commit d3782c9
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 18 deletions.
28 changes: 27 additions & 1 deletion mavros/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,25 @@ endif()

include(EnableCXX11)
include(MavrosMavlink)
include(ExternalProject)

# The configure step fails at catkin_package() if this directory
# doesn't exist yet. Create it at configure time!
file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)

ExternalProject_Add(
geographic_lib
GIT_REPOSITORY git://git.code.sourceforge.net/p/geographiclib/code
PATCH_COMMAND mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/data
CONFIGURE_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && cmake -D CMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} -D GEOGRAPHICLIB_LIB_TYPE=SHARED -D GEOGRAPHICLIB_DATA=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data .
BUILD_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && make -j1
INSTALL_COMMAND cd ${CMAKE_BINARY_DIR}/geographic_lib-prefix/src/geographic_lib && make install &&
./tools/geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96-5 &&
./tools/geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96 &&
./tools/geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data emm2015
)
set(GEOGRAPHICLIB_INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include)
set(GeographicLib_LIBRARIES ${CATKIN_DEVEL_PREFIX}/lib/libGeographic${CMAKE_SHARED_LIBRARY_SUFFIX})

# detect if sensor_msgs has BatteryState.msg
# http://answers.ros.org/question/223769/how-to-check-that-message-exists-with-catkin-for-conditional-compilation-sensor_msgsbatterystate/
Expand Down Expand Up @@ -79,6 +98,7 @@ include_directories(
${Boost_INCLUDE_DIRS}
${mavlink_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${GEOGRAPHICLIB_INCLUDE_DIRS}
)

add_library(mavros
Expand All @@ -97,7 +117,9 @@ target_link_libraries(mavros
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
add_dependencies(mavros mavros_msgs_generate_messages_cpp)
add_dependencies(mavros
mavros_msgs_generate_messages_cpp
)

add_library(mavros_plugins
src/plugins/dummy.cpp
Expand Down Expand Up @@ -127,11 +149,13 @@ add_library(mavros_plugins
)
add_dependencies(mavros_plugins
mavros
geographic_lib
)
target_link_libraries(mavros_plugins
mavros
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${GeographicLib_LIBRARIES}
)

## Declare a cpp executable
Expand All @@ -140,11 +164,13 @@ add_executable(mavros_node
)
add_dependencies(mavros_node
mavros
geographic_lib
)
target_link_libraries(mavros_node
mavros
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${GeographicLib_LIBRARIES}
)

add_executable(gcs_bridge
Expand Down
1 change: 1 addition & 0 deletions mavros/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
<depend>tf2_ros</depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>rospy</exec_depend>
<depend>geographic_lib</depend>

<!-- message packages -->
<depend>diagnostic_msgs</depend>
Expand Down
36 changes: 19 additions & 17 deletions mavros/src/plugins/global_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <mavros/mavros_plugin.h>
#include <mavros/gps_conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <GeographicLib/Geoid.hpp>

#include <std_msgs/Float64.h>
#include <nav_msgs/Odometry.h>
Expand All @@ -30,8 +31,6 @@

namespace mavros {
namespace std_plugins {


/**
* @brief Global position plugin.
*
Expand Down Expand Up @@ -75,9 +74,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
Subscriptions get_subscriptions()
{
return {
make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
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)
};
}

Expand All @@ -99,16 +98,19 @@ class GlobalPositionPlugin : public plugin::PluginBase {

template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
GeographicLib::Geoid egm96("egm96-5");
fix->latitude = msg.lat / 1E7; // deg
fix->longitude = msg.lon / 1E7; // deg
fix->altitude = msg.alt / 1E3; // m
fix->altitude = msg.alt / 1E3 + // conversion from height abov geoid (AMSL)
GeographicLib::Geoid::GEOIDTOELLIPSOID // to height above ellipsoid (WGS-84)
* egm96(fix->latitude, fix->longitude); // in meters
}

inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) {
fix->position_covariance.fill(0.0);
fix->position_covariance[0] = -1.0;
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}

/* -*- message handlers -*- */
Expand Down Expand Up @@ -137,10 +139,10 @@ class GlobalPositionPlugin : public plugin::PluginBase {

// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
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;
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
Expand All @@ -151,7 +153,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
raw_fix_pub.publish(fix);

if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != 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

Expand Down Expand Up @@ -218,8 +220,8 @@ class GlobalPositionPlugin : public plugin::PluginBase {

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

// Velocity covariance unknown
ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
Expand All @@ -244,12 +246,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;

// publish
gp_fix_pub.publish(fix);
Expand Down

0 comments on commit d3782c9

Please sign in to comment.