actuator_control.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Marcel Stüttgen <stuettgen@fh-aachen.de>
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/ActuatorControl.h>
00021 
00022 namespace mavplugin {
00028 class ActuatorControlPlugin : public MavRosPlugin {
00029 public:
00030         ActuatorControlPlugin() :
00031                 nh("~"),
00032                 uas(nullptr)
00033         { };
00034 
00035         void initialize(UAS &uas_)
00036         {
00037                 uas = &uas_;
00038 
00039                 actuator_control_sub = nh.subscribe("actuator_control", 10, &ActuatorControlPlugin::actuator_control_cb, this);
00040         }
00041 
00042         const message_map get_rx_handlers() {
00043                 return { /* Rx disabled */ };
00044         }
00045 
00046 private:
00047         ros::NodeHandle nh;
00048         UAS *uas;
00049         ros::Subscriber actuator_control_sub;
00050 
00051         /* -*- low-level send -*- */
00052 
00054         void set_actuator_control_target(const uint64_t time_usec,
00055                         const uint8_t group_mix,
00056                         const float controls[8])
00057         {
00058                 mavlink_message_t msg;
00059 
00060                 mavlink_msg_set_actuator_control_target_pack_chan(UAS_PACK_CHAN(uas), &msg,
00061                                 time_usec,
00062                                 group_mix,
00063                                 UAS_PACK_TGT(uas),
00064                                 controls);
00065                 UAS_FCU(uas)->send_message(&msg);
00066         }
00067 
00068         /* -*- callbacks -*- */
00069 
00070         void actuator_control_cb(const mavros_msgs::ActuatorControl::ConstPtr &req) {
00072                 set_actuator_control_target(ros::Time::now().toNSec() / 1000,
00073                                 req->group_mix,
00074                                 req->controls.data());
00075         }
00076 };
00077 };      // namespace mavplugin
00078 
00079 PLUGINLIB_EXPORT_CLASS(mavplugin::ActuatorControlPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17