vibration.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2015 Nuno Marques.
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/Vibration.h>
00021 
00022 namespace mavplugin {
00028 class VibrationPlugin : public MavRosPlugin {
00029 public:
00030         VibrationPlugin() :
00031                 vibe_nh("~vibration"),
00032                 uas(nullptr)
00033         { };
00034 
00035         void initialize(UAS &uas_)
00036         {
00037                 uas = &uas_;
00038 
00039                 vibe_nh.param<std::string>("frame_id", frame_id, "vibration");
00040 
00041                 vibration_pub = vibe_nh.advertise<mavros_msgs::Vibration>("raw/vibration", 10);
00042         }
00043 
00044         const message_map get_rx_handlers() {
00045                 return {
00046                                MESSAGE_HANDLER(MAVLINK_MSG_ID_VIBRATION, &VibrationPlugin::handle_vibration)
00047                 };
00048         }
00049 
00050 private:
00051         ros::NodeHandle vibe_nh;
00052         UAS *uas;
00053 
00054         std::string frame_id;
00055 
00056         ros::Publisher vibration_pub;
00057 
00058         void handle_vibration(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00059                 mavlink_vibration_t vibration;
00060                 mavlink_msg_vibration_decode(msg, &vibration);
00061 
00062                 auto vibe_msg = boost::make_shared<mavros_msgs::Vibration>();
00063 
00064                 vibe_msg->header = uas->synchronized_header(frame_id, vibration.time_usec);
00065 
00066                 // TODO no transform_frame?
00067                 vibe_msg->vibration.x = vibration.vibration_x;
00068                 vibe_msg->vibration.y = vibration.vibration_y;
00069                 vibe_msg->vibration.z = vibration.vibration_z;
00070                 vibe_msg->clipping[0] = vibration.clipping_0;
00071                 vibe_msg->clipping[1] = vibration.clipping_1;
00072                 vibe_msg->clipping[2] = vibration.clipping_2;
00073 
00074                 vibration_pub.publish(vibe_msg);
00075         }
00076 };
00077 };      // namespace mavplugin
00078 
00079 PLUGINLIB_EXPORT_CLASS(mavplugin::VibrationPlugin, mavplugin::MavRosPlugin)


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