hil_controls.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2016 Pavel Vechersky.
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/HilControls.h>
00021 
00022 namespace mavplugin {
00026 class HilControlsPlugin : public MavRosPlugin {
00027 public:
00028         HilControlsPlugin() :
00029                 hil_controls_nh("~hil_controls"),
00030                 uas(nullptr)
00031         { };
00032 
00033         void initialize(UAS &uas_)
00034         {
00035                 uas = &uas_;
00036 
00037                 hil_controls_pub = hil_controls_nh.advertise<mavros_msgs::HilControls>("hil_controls", 10);
00038         };
00039 
00040         const message_map get_rx_handlers() {
00041                 return {
00042                                MESSAGE_HANDLER(MAVLINK_MSG_ID_HIL_CONTROLS, &HilControlsPlugin::handle_hil_controls),
00043                 };
00044         }
00045 
00046 private:
00047         ros::NodeHandle hil_controls_nh;
00048         UAS *uas;
00049 
00050         ros::Publisher hil_controls_pub;
00051 
00052         /* -*- rx handlers -*- */
00053 
00054         void handle_hil_controls(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00055                 mavlink_hil_controls_t hil_controls;
00056                 mavlink_msg_hil_controls_decode(msg, &hil_controls);
00057 
00058                 auto hil_controls_msg = boost::make_shared<mavros_msgs::HilControls>();
00059 
00060                 hil_controls_msg->header.stamp = uas->synchronise_stamp(hil_controls.time_usec);
00061                 hil_controls_msg->roll_ailerons = hil_controls.roll_ailerons;
00062                 hil_controls_msg->pitch_elevator = hil_controls.pitch_elevator;
00063                 hil_controls_msg->yaw_rudder = hil_controls.yaw_rudder;
00064                 hil_controls_msg->throttle = hil_controls.throttle;
00065                 hil_controls_msg->aux1 = hil_controls.aux1;
00066                 hil_controls_msg->aux2 = hil_controls.aux2;
00067                 hil_controls_msg->aux3 = hil_controls.aux3;
00068                 hil_controls_msg->aux4 = hil_controls.aux4;
00069                 hil_controls_msg->mode = hil_controls.mode;
00070                 hil_controls_msg->nav_mode = hil_controls.nav_mode;
00071 
00072                 hil_controls_pub.publish(hil_controls_msg);
00073         }
00074 };
00075 };      // namespace mavplugin
00076 
00077 PLUGINLIB_EXPORT_CLASS(mavplugin::HilControlsPlugin, mavplugin::MavRosPlugin)
00078 


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