altitude.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Andreas Antener <andreas@uaventure.com>.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <mavros/mavros_plugin.h>
00018 #include <pluginlib/class_list_macros.h>
00019 
00020 #include <mavros_msgs/Altitude.h>
00021 
00022 namespace mavplugin {
00026 class AltitudePlugin : public MavRosPlugin {
00027 public:
00028     AltitudePlugin() :
00029         nh("~"),
00030         uas(nullptr)
00031     { }
00032 
00036     void initialize(UAS &uas_)
00037     {
00038         uas = &uas_;
00039         nh.param<std::string>("frame_id", frame_id, "map");
00040         altitude_pub = nh.advertise<mavros_msgs::Altitude>("altitude", 10);
00041     }
00042 
00043     const message_map get_rx_handlers() {
00044         return {
00045                    MESSAGE_HANDLER(MAVLINK_MSG_ID_ALTITUDE, &AltitudePlugin::handle_altitude),
00046         };
00047     }
00048 
00049 private:
00050     ros::NodeHandle nh;
00051     UAS *uas;
00052     std::string frame_id;
00053 
00054     ros::Publisher altitude_pub;
00055 
00056     void handle_altitude(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00057         mavlink_altitude_t altitude;
00058         mavlink_msg_altitude_decode(msg, &altitude);
00059 
00060         auto ros_msg = boost::make_shared<mavros_msgs::Altitude>();
00061         ros_msg->header = uas->synchronized_header(frame_id, altitude.time_usec);
00062         
00063         ros_msg->monotonic = altitude.altitude_monotonic;
00064         ros_msg->amsl = altitude.altitude_amsl;
00065         ros_msg->local = altitude.altitude_local;
00066         ros_msg->relative = altitude.altitude_relative;
00067         ros_msg->terrain = altitude.altitude_terrain;
00068         ros_msg->bottom_clearance = altitude.bottom_clearance;
00069 
00070         altitude_pub.publish(ros_msg);
00071     }
00072 
00073 };
00074 };  // namespace mavplugin
00075 
00076 PLUGINLIB_EXPORT_CLASS(mavplugin::AltitudePlugin, mavplugin::MavRosPlugin)
00077 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19