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