manual_control.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Matias Nitsche.
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/ManualControl.h>
00021 
00022 namespace mavplugin {
00026 class ManualControlPlugin : public MavRosPlugin {
00027 public:
00028         ManualControlPlugin() :
00029                 manual_control_nh("~manual_control"),
00030                 uas(nullptr)
00031         { };
00032 
00033         void initialize(UAS &uas_)
00034         {
00035                 uas = &uas_;
00036 
00037                 control_pub = manual_control_nh.advertise<mavros_msgs::ManualControl>("control", 10);
00038                 //uas->sig_connection_changed.connect(boost::bind(&RCIOPlugin::connection_cb, this, _1));
00039         };
00040 
00041         const message_map get_rx_handlers() {
00042                 return {
00043                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MANUAL_CONTROL, &ManualControlPlugin::handle_manual_control),
00044                 };
00045         }
00046 
00047 private:
00048         ros::NodeHandle manual_control_nh;
00049         UAS *uas;
00050 
00051         ros::Publisher control_pub;
00052 
00053         /* -*- rx handlers -*- */
00054 
00055         void handle_manual_control(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00056                 mavlink_manual_control_t manual_control;
00057                 mavlink_msg_manual_control_decode(msg, &manual_control);
00058 
00059                 auto manual_control_msg = boost::make_shared<mavros_msgs::ManualControl>();
00060 
00061                 manual_control_msg->header.stamp = ros::Time::now();
00062                 manual_control_msg->x = (manual_control.x / 1000.0);
00063                 manual_control_msg->y = (manual_control.y / 1000.0);
00064                 manual_control_msg->z = (manual_control.z / 1000.0);
00065                 manual_control_msg->r = (manual_control.r / 1000.0);
00066                 manual_control_msg->buttons = manual_control.buttons;
00067 
00068                 control_pub.publish(manual_control_msg);
00069         }
00070 };
00071 };      // namespace mavplugin
00072 
00073 PLUGINLIB_EXPORT_CLASS(mavplugin::ManualControlPlugin, mavplugin::MavRosPlugin)
00074 


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