setpoint_attitude.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/mavros_plugin.h>
19 #include <mavros/setpoint_mixin.h>
21 
25 
26 #include <geometry_msgs/PoseStamped.h>
27 #include <geometry_msgs/TwistStamped.h>
28 #include <mavros_msgs/Thrust.h>
29 
30 namespace mavros {
31 namespace std_plugins {
32 
37 
44  private plugin::SetAttitudeTargetMixin<SetpointAttitudePlugin>,
45  private plugin::TF2ListenerMixin<SetpointAttitudePlugin> {
46 public:
48  sp_nh("~setpoint_attitude"),
49  tf_rate(50.0),
50  use_quaternion(false),
51  reverse_thrust(false),
52  tf_listen(false)
53  { }
54 
55  void initialize(UAS &uas_) override
56  {
57  PluginBase::initialize(uas_);
58 
59  // main params
60  sp_nh.param("use_quaternion", use_quaternion, false);
61  sp_nh.param("reverse_thrust", reverse_thrust, false);
62  // tf params
63  sp_nh.param("tf/listen", tf_listen, false);
64  sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
65  sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "target_attitude");
66  sp_nh.param("tf/rate_limit", tf_rate, 50.0);
67 
68  // thrust msg subscriber to sync
69  th_sub.subscribe(sp_nh, "thrust", 1);
70 
71  if (tf_listen) {
72  ROS_INFO_STREAM_NAMED("attitude",
73  "Listen to desired attitude transform "
74  << tf_frame_id << " -> " << tf_child_frame_id);
75 
76  tf2_start<mavros_msgs::Thrust>("AttitudeSpTFSync", th_sub, &SetpointAttitudePlugin::transform_cb);
77  }
78  else if (use_quaternion) {
82  pose_sub.subscribe(sp_nh, "attitude", 1);
83 
89  sync_pose->registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_pose_cb, this, _1, _2));
90  }
91  else {
92  twist_sub.subscribe(sp_nh, "cmd_vel", 1);
94  sync_twist->registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_twist_cb, this, _1, _2));
95  }
96  }
97 
99  {
100  return { /* Rx disabled */ };
101  }
102 
103 private:
105  friend class TF2ListenerMixin;
107 
111 
112  std::unique_ptr<SyncPoseThrust> sync_pose;
113  std::unique_ptr<SyncTwistThrust> sync_twist;
114 
115  std::string tf_frame_id;
116  std::string tf_child_frame_id;
117 
118  bool tf_listen;
119  double tf_rate;
120 
122 
125 
130  inline bool is_normalized(float thrust){
131  if (reverse_thrust) {
132  if (thrust < -1.0) {
133  ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0);
134  return false;
135  }
136  }
137  else {
138  if (thrust < 0.0) {
139  ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0);
140  return false;
141  }
142  }
143 
144  if (thrust > 1.0) {
145  ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0);
146  return false;
147  }
148  return true;
149  }
150 
151  /* -*- mid-level helpers -*- */
152 
156  void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust)
157  {
161  const uint8_t ignore_all_except_q_and_thrust = (7 << 0);
162 
164  ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))
165  );
166 
167  set_attitude_target(stamp.toNSec() / 1000000,
168  ignore_all_except_q_and_thrust,
169  q,
170  Eigen::Vector3d::Zero(),
171  thrust);
172  }
173 
177  void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust)
178  {
182  const uint8_t ignore_all_except_rpy = (1 << 7);
183 
184  auto av = ftf::transform_frame_ned_enu(ang_vel);
185 
186  set_attitude_target(stamp.toNSec() / 1000000,
187  ignore_all_except_rpy,
188  Eigen::Quaterniond::Identity(),
189  av,
190  thrust);
191  }
192 
193  /* -*- callbacks -*- */
194 
195  void transform_cb(const geometry_msgs::TransformStamped &transform, const mavros_msgs::Thrust::ConstPtr &thrust_msg) {
196  Eigen::Affine3d tr;
197  tf::transformMsgToEigen(transform.transform, tr);
198 
199  send_attitude_quaternion(transform.header.stamp, tr, thrust_msg->thrust);
200  }
201 
203  Eigen::Affine3d tr;
204  tf::poseMsgToEigen(pose_msg->pose, tr);
205 
206  if (is_normalized(thrust_msg->thrust))
207  send_attitude_quaternion(pose_msg->header.stamp, tr, thrust_msg->thrust);
208  }
209 
211  Eigen::Vector3d ang_vel;
212  tf::vectorMsgToEigen(req->twist.angular, ang_vel);
213 
214  if (is_normalized(thrust_msg->thrust))
215  send_attitude_ang_velocity(req->header.stamp, ang_vel, thrust_msg->thrust);
216  }
217 
218 };
219 } // namespace std_plugins
220 } // namespace mavros
221 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
message_filters::Subscriber< geometry_msgs::PoseStamped > pose_sub
#define ROS_WARN_NAMED(name,...)
MAVROS Plugin base.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
message_filters::sync_policies::ApproximateTime< geometry_msgs::PoseStamped, mavros_msgs::Thrust > SyncPoseThrustPolicy
This mixin adds TF2 listener thread to plugin.
message_filters::sync_policies::ApproximateTime< geometry_msgs::TwistStamped, mavros_msgs::Thrust > SyncTwistThrustPolicy
#define ROS_INFO_STREAM_NAMED(name, args)
message_filters::Subscriber< mavros_msgs::Thrust > th_sub
T transform_orientation_enu_ned(const T &in)
Transform from attitude represented WRT ENU frame to attitude represented WRT NED frame...
Definition: frame_tf.h:169
message_filters::Synchronizer< SyncTwistThrustPolicy > SyncTwistThrust
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
message_filters::Synchronizer< SyncPoseThrustPolicy > SyncPoseThrust
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Mixin for setpoint plugins.
void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust)
Send angular velocity setpoint and thrust to FCU attitude controller.
uint64_t toNSec() const
T transform_frame_ned_enu(const T &in)
Transform data expressed in NED to ENU frame.
Definition: frame_tf.h:215
std::unique_ptr< SyncTwistThrust > sync_twist
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< geometry_msgs::TwistStamped > twist_sub
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
This mixin adds set_attitude_target()
void initialize(UAS &uas_) override
Plugin initializer.
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.
bool is_normalized(float thrust)
Function to verify if the thrust values are normalized; considers also the reversed trust values...
void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust)
Send attitude setpoint and thrust to FCU attitude controller.
T transform_orientation_baselink_aircraft(const T &in)
Transform from attitude represented WRT baselink frame to attitude represented WRT body frame...
Definition: frame_tf.h:187
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void attitude_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose_msg, const mavros_msgs::Thrust::ConstPtr &thrust_msg)
void transform_cb(const geometry_msgs::TransformStamped &transform, const mavros_msgs::Thrust::ConstPtr &thrust_msg)
void attitude_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req, const mavros_msgs::Thrust::ConstPtr &thrust_msg)
std::unique_ptr< SyncPoseThrust > sync_pose


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