setpoint_mixin.h
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014 Vladimir Ermakov.
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 #pragma once
18 
19 #include <functional>
20 #include <mavros/utils.h>
21 #include <mavros/mavros_plugin.h>
22 
23 #include <geometry_msgs/TransformStamped.h>
24 
25 #include "tf2_ros/message_filter.h"
26 #include <message_filters/subscriber.h>
27 
28 
29 namespace mavros {
30 namespace plugin {
34 template <class D>
35 class SetPositionTargetLocalNEDMixin {
36 public:
38  void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame,
39  uint16_t type_mask,
40  Eigen::Vector3d p,
41  Eigen::Vector3d v,
42  Eigen::Vector3d af,
43  float yaw, float yaw_rate)
44  {
45  mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
46  mavlink::common::msg::SET_POSITION_TARGET_LOCAL_NED sp = {};
47 
48  m_uas_->msg_set_target(sp);
49 
50  // [[[cog:
51  // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'yaw', 'yaw_rate'):
52  // cog.outl("sp.%s = %s;" % (f, f))
53  // for fp, vp in (('', 'p'), ('v', 'v'), ('af', 'af')):
54  // for a in ('x', 'y', 'z'):
55  // cog.outl("sp.%s%s = %s.%s();" % (fp, a, vp, a))
56  // ]]]
57  sp.time_boot_ms = time_boot_ms;
58  sp.coordinate_frame = coordinate_frame;
59  sp.type_mask = type_mask;
60  sp.yaw = yaw;
61  sp.yaw_rate = yaw_rate;
62  sp.x = p.x();
63  sp.y = p.y();
64  sp.z = p.z();
65  sp.vx = v.x();
66  sp.vy = v.y();
67  sp.vz = v.z();
68  sp.afx = af.x();
69  sp.afy = af.y();
70  sp.afz = af.z();
71  // [[[end]]] (checksum: 6a9b9dacbcf85c5d428d754c20afe110)
72 
73  UAS_FCU(m_uas_)->send_message_ignore_drop(sp);
74  }
75 };
76 
80 template <class D>
81 class SetPositionTargetGlobalIntMixin {
82 public:
84  void set_position_target_global_int(uint32_t time_boot_ms, uint8_t coordinate_frame,
85  uint16_t type_mask,
86  int32_t lat_int, int32_t lon_int, float alt,
87  Eigen::Vector3d v,
88  Eigen::Vector3d af,
89  float yaw, float yaw_rate)
90  {
91  mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
92  mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT sp = {};
93 
94  m_uas_->msg_set_target(sp);
95 
96  // [[[cog:
97  // for f in ('time_boot_ms', 'coordinate_frame', 'type_mask', 'lat_int', 'lon_int', 'alt', 'yaw', 'yaw_rate'):
98  // cog.outl("sp.%s = %s;" % (f, f))
99  // for fp, vp in (('v', 'v'), ('af', 'af')):
100  // for a in ('x', 'y', 'z'):
101  // cog.outl("sp.%s%s = %s.%s();" % (fp, a, vp, a))
102  // ]]]
103  sp.time_boot_ms = time_boot_ms;
104  sp.coordinate_frame = coordinate_frame;
105  sp.type_mask = type_mask;
106  sp.lat_int = lat_int;
107  sp.lon_int = lon_int;
108  sp.alt = alt;
109  sp.yaw = yaw;
110  sp.yaw_rate = yaw_rate;
111  sp.vx = v.x();
112  sp.vy = v.y();
113  sp.vz = v.z();
114  sp.afx = af.x();
115  sp.afy = af.y();
116  sp.afz = af.z();
117  // [[[end]]] (checksum: 30c9629ad309d488df1f63b683dac6a4)
118 
119  UAS_FCU(m_uas_)->send_message_ignore_drop(sp);
120  }
121 };
122 
126 template <class D>
127 class SetAttitudeTargetMixin {
128 public:
130  void set_attitude_target(uint32_t time_boot_ms,
131  uint8_t type_mask,
132  Eigen::Quaterniond orientation,
133  Eigen::Vector3d body_rate,
134  float thrust)
135  {
136  mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
137  mavlink::common::msg::SET_ATTITUDE_TARGET sp = {};
138 
139  m_uas_->msg_set_target(sp);
140  mavros::ftf::quaternion_to_mavlink(orientation, sp.q);
141 
142  // [[[cog:
143  // for f in ('time_boot_ms', 'type_mask', 'thrust'):
144  // cog.outl("sp.%s = %s;" % (f, f))
145  // for f, v in (('roll', 'x'), ('pitch', 'y'), ('yaw', 'z')):
146  // cog.outl("sp.body_%s_rate = body_rate.%s();" % (f, v))
147  // ]]]
148  sp.time_boot_ms = time_boot_ms;
149  sp.type_mask = type_mask;
150  sp.thrust = thrust;
151  sp.body_roll_rate = body_rate.x();
152  sp.body_pitch_rate = body_rate.y();
153  sp.body_yaw_rate = body_rate.z();
154  // [[[end]]] (checksum: aa941484927bb7a7d39a2c31d08fcfc1)
155 
156  UAS_FCU(m_uas_)->send_message_ignore_drop(sp);
157  }
158 };
159 
166 template <class D>
167 class TF2ListenerMixin {
168 public:
169  std::thread tf_thread;
170  std::string tf_thd_name;
171 
178  void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) )
179  {
180  tf_thd_name = _thd_name;
181  auto tf_transform_cb = std::bind(cbp, static_cast<D *>(this), std::placeholders::_1);
182 
183  tf_thread = std::thread([this, tf_transform_cb]() {
184  mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
185 
186  mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
187  std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
188  std::string &_child_frame_id = static_cast<D *>(this)->tf_child_frame_id;
189 
190  ros::Rate rate(static_cast<D *>(this)->tf_rate);
191  while (ros::ok()) {
192  // Wait up to 3s for transform
193  if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) {
194  try {
195  auto transform = m_uas_->tf2_buffer.lookupTransform(
196  _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
197  tf_transform_cb(transform);
198  }
199  catch (tf2::LookupException &ex) {
200  ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
201  }
202  }
203  rate.sleep();
204  }
205  });
206  }
207 
214  template <class T>
215  void tf2_start(const char *_thd_name, message_filters::Subscriber<T> &topic_sub, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &))
216  {
217  tf_thd_name = _thd_name;
218 
219  tf_thread = std::thread([this, cbp, &topic_sub]() {
220  mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
221 
222  mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
223  ros::NodeHandle &_sp_nh = static_cast<D *>(this)->sp_nh;
224  std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
225  std::string &_child_frame_id = static_cast<D *>(this)->tf_child_frame_id;
226 
227  tf2_ros::MessageFilter<T> tf2_filter(topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh);
228 
229  ros::Rate rate(static_cast<D *>(this)->tf_rate);
230  while (ros::ok()) {
231  // Wait up to 3s for transform
232  if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) {
233  try {
234  auto transform = m_uas_->tf2_buffer.lookupTransform(
235  _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
236 
237  tf2_filter.registerCallback(boost::bind(cbp, static_cast<D *>(this), transform, _1));
238  }
239  catch (tf2::LookupException &ex) {
240  ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
241  }
242  }
243  rate.sleep();
244  }
245  });
246  }
247 };
248 } // namespace plugin
249 } // namespace mavros
ConstPtr
std::shared_ptr< MAVConnInterface const > ConstPtr
ros::NodeHandle
transform
transform
mavros::plugin::TF2ListenerMixin::tf_thd_name
std::string tf_thd_name
Definition: setpoint_mixin.h:170
tf2_ros::MessageFilter
mavros_plugin.h
MAVROS Plugin base.
mavros::plugin::TF2ListenerMixin
This mixin adds TF2 listener thread to plugin.
Definition: setpoint_mixin.h:167
mavconn::utils::set_this_thread_name
bool set_this_thread_name(const std::string &name, Args &&...args)
mavros::plugin::TF2ListenerMixin::tf2_start
void tf2_start(const char *_thd_name, void(D::*cbp)(const geometry_msgs::TransformStamped &))
start tf listener
Definition: setpoint_mixin.h:178
mavros::ftf::quaternion_to_mavlink
void quaternion_to_mavlink(const Eigen::Quaternion< _Scalar > &q, std::array< float, 4 > &qmsg)
Store Quaternion to MAVLink float[4] format.
Definition: frame_tf.h:373
mavros::UAS::msg_set_target
void msg_set_target(_T &msg)
Definition: mavros_uas.h:388
mavros::plugin::SetPositionTargetLocalNEDMixin::set_position_target_local_ned
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.
Definition: setpoint_mixin.h:38
mavros::UAS
UAS for plugins.
Definition: mavros_uas.h:67
D
#define D
UAS_FCU
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
mavros::plugin::TF2ListenerMixin::tf_thread
std::thread tf_thread
Definition: setpoint_mixin.h:169
ros::ok
ROSCPP_DECL bool ok()
ros::Rate::sleep
bool sleep()
message_filter.h
mavros::plugin::SetPositionTargetLocalNEDMixin
This mixin adds set_position_target_local_ned()
Definition: setpoint_mixin.h:35
mavros::plugin::SetPositionTargetGlobalIntMixin::set_position_target_global_int
void set_position_target_global_int(uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, Eigen::Vector3d v, Eigen::Vector3d af, float yaw, float yaw_rate)
Message specification: https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT.
Definition: setpoint_mixin.h:84
mavros::plugin::SetAttitudeTargetMixin
This mixin adds set_attitude_target()
Definition: setpoint_mixin.h:127
mavros::plugin::SetAttitudeTargetMixin::set_attitude_target
void set_attitude_target(uint32_t time_boot_ms, uint8_t type_mask, Eigen::Quaterniond orientation, Eigen::Vector3d body_rate, float thrust)
Message sepecification: https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET.
Definition: setpoint_mixin.h:130
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
message_filters::Subscriber
mavros::plugin::SetPositionTargetGlobalIntMixin
This mixin adds set_position_target_global_int()
Definition: setpoint_mixin.h:81
mavros::plugin::TF2ListenerMixin::tf2_start
void tf2_start(const char *_thd_name, message_filters::Subscriber< T > &topic_sub, void(D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &))
start tf listener syncronized with another topic
Definition: setpoint_mixin.h:215
message_filters::SimpleFilter::registerCallback
Connection registerCallback(const C &callback)

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