setpoint_accel.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014 Nuno Marques.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #include <mavros/utils.h>
19 #include <mavros/mavros_plugin.h>
20 #include <mavros/setpoint_mixin.h>
22 
23 #include <geometry_msgs/Vector3Stamped.h>
24 
25 namespace mavros {
26 namespace std_plugins {
33  private plugin::SetPositionTargetLocalNEDMixin<SetpointAccelerationPlugin> {
34 public:
36  sp_nh("~setpoint_accel"),
37  send_force(false)
38  { }
39 
40  void initialize(UAS &uas_) override
41  {
42  PluginBase::initialize(uas_);
43 
44  sp_nh.param("send_force", send_force, false);
45 
47  }
48 
50  {
51  return { /* Rx disabled */ };
52  }
53 
54 private:
57 
59 
60  bool send_force;
61 
62  /* -*- mid-level helpers -*- */
63 
69  void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu)
70  {
71  using mavlink::common::MAV_FRAME;
72 
73  /* Documentation start from bit 1 instead 0.
74  * Ignore position and velocity vectors, yaw and yaw rate
75  */
76  uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0);
77 
78  if (send_force)
79  ignore_all_except_a_xyz |= (1 << 9);
80 
81  auto accel = ftf::transform_frame_enu_ned(accel_enu);
82 
83  set_position_target_local_ned(stamp.toNSec() / 1000000,
84  utils::enum_value(MAV_FRAME::LOCAL_NED),
85  ignore_all_except_a_xyz,
86  Eigen::Vector3d::Zero(),
87  Eigen::Vector3d::Zero(),
88  accel,
89  0.0, 0.0);
90  }
91 
92  /* -*- callbacks -*- */
93 
95  Eigen::Vector3d accel_enu;
96 
97  tf::vectorMsgToEigen(req->vector, accel_enu);
98  send_setpoint_acceleration(req->header.stamp, accel_enu);
99  }
100 };
101 } // namespace std_plugins
102 } // namespace mavros
103 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
MAVROS Plugin base.
Setpoint acceleration/force plugin.
void initialize(UAS &uas_) override
Plugin initializer.
void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d af, float yaw, float yaw_rate)
Message specification: https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED.
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
UAS for plugins.
Definition: mavros_uas.h:67
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Mixin for setpoint plugins.
uint64_t toNSec() const
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
This mixin adds set_position_target_local_ned()
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu)
Send acceleration/force to FCU acceleration controller.
void accel_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req)
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26