vibration.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015 Nuno Marques.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
19 
20 #include <mavros_msgs/Vibration.h>
21 
22 namespace mavros {
23 namespace extra_plugins {
30 public:
32  vibe_nh("~vibration")
33  { }
34 
35  void initialize(UAS &uas_)
36  {
38 
39  vibe_nh.param<std::string>("frame_id", frame_id, "base_link");
40 
41  vibration_pub = vibe_nh.advertise<mavros_msgs::Vibration>("raw/vibration", 10);
42  }
43 
45  {
46  return {
48  };
49  }
50 
51 private:
53 
54  std::string frame_id;
55 
57 
58  void handle_vibration(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VIBRATION &vibration)
59  {
60  auto vibe_msg = boost::make_shared<mavros_msgs::Vibration>();
61 
62  vibe_msg->header = m_uas->synchronized_header(frame_id, vibration.time_usec);
63 
64  Eigen::Vector3d vib_enu = {vibration.vibration_x, vibration.vibration_y, vibration.vibration_z};
65  tf::vectorEigenToMsg(ftf::transform_frame_ned_enu(vib_enu), vibe_msg->vibration);
66 
67  vibe_msg->clipping[0] = vibration.clipping_0;
68  vibe_msg->clipping[1] = vibration.clipping_1;
69  vibe_msg->clipping[2] = vibration.clipping_2;
70 
71  vibration_pub.publish(vibe_msg);
72  }
73 };
74 } // namespace extra_plugins
75 } // namespace mavros
76 
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
void publish(const boost::shared_ptr< M > &message) const
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
T transform_frame_ned_enu(const T &in)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< HandlerInfo > Subscriptions
void initialize(UAS &uas_)
void handle_vibration(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VIBRATION &vibration)
Definition: vibration.cpp:58
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:18