setpoint_raw.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015,2016 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 #include <mavros/mavros_plugin.h>
18 #include <mavros/setpoint_mixin.h>
20 
21 #include <mavros_msgs/AttitudeTarget.h>
22 #include <mavros_msgs/PositionTarget.h>
23 #include <mavros_msgs/GlobalPositionTarget.h>
24 
25 namespace mavros {
26 namespace std_plugins {
34  private plugin::SetPositionTargetLocalNEDMixin<SetpointRawPlugin>,
35  private plugin::SetPositionTargetGlobalIntMixin<SetpointRawPlugin>,
36  private plugin::SetAttitudeTargetMixin<SetpointRawPlugin> {
37 public:
39  sp_nh("~setpoint_raw")
40  { }
41 
42  void initialize(UAS &uas_) override
43  {
44  PluginBase::initialize(uas_);
45 
46  bool tf_listen;
47 
51  target_local_pub = sp_nh.advertise<mavros_msgs::PositionTarget>("target_local", 10);
52  target_global_pub = sp_nh.advertise<mavros_msgs::GlobalPositionTarget>("target_global", 10);
53  target_attitude_pub = sp_nh.advertise<mavros_msgs::AttitudeTarget>("target_attitude", 10);
54  }
55 
57  {
58  return {
62  };
63  }
64 
65 private:
68  friend class SetAttitudeTargetMixin;
70 
73 
74  /* -*- message handlers -*- */
75  void handle_position_target_local_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_LOCAL_NED &tgt)
76  {
77  // Transform desired position,velocities,and accels from ENU to NED frame
78  auto position = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.x, tgt.y, tgt.z));
79  auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz));
80  auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz));
81  float yaw = ftf::quaternion_get_yaw(
84  ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw))));
85  Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate);
86  auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned);
87  float yaw_rate = ang_vel_enu.z();
88 
89  auto target = boost::make_shared<mavros_msgs::PositionTarget>();
90 
91  target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms);
92  target->coordinate_frame = tgt.coordinate_frame;
93  target->type_mask = tgt.type_mask;
94  tf::pointEigenToMsg(position, target->position);
95  tf::vectorEigenToMsg(velocity, target->velocity);
96  tf::vectorEigenToMsg(af, target->acceleration_or_force);
97  target->yaw = yaw;
98  target->yaw_rate = yaw_rate;
99 
100  target_local_pub.publish(target);
101  }
102 
103  void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_GLOBAL_INT &tgt)
104  {
105  // Transform desired velocities from ENU to NED frame
106  auto velocity = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.vx, tgt.vy, tgt.vz));
107  auto af = ftf::transform_frame_ned_enu(Eigen::Vector3d(tgt.afx, tgt.afy, tgt.afz));
108  float yaw = ftf::quaternion_get_yaw(
111  ftf::quaternion_from_rpy(0.0, 0.0, tgt.yaw))));
112  Eigen::Vector3d ang_vel_ned(0.0, 0.0, tgt.yaw_rate);
113  auto ang_vel_enu = ftf::transform_frame_ned_enu(ang_vel_ned);
114  float yaw_rate = ang_vel_enu.z();
115 
116  auto target = boost::make_shared<mavros_msgs::GlobalPositionTarget>();
117 
118  target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms);
119  target->coordinate_frame = tgt.coordinate_frame;
120  target->type_mask = tgt.type_mask;
121  target->latitude = tgt.lat_int / 1e7;
122  target->longitude = tgt.lon_int / 1e7;
123  target->altitude = tgt.alt;
124  tf::vectorEigenToMsg(velocity, target->velocity);
125  tf::vectorEigenToMsg(af, target->acceleration_or_force);
126  target->yaw = yaw;
127  target->yaw_rate = yaw_rate;
128 
129  target_global_pub.publish(target);
130  }
131 
132  void handle_attitude_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_TARGET &tgt)
133  {
134  // Transform orientation from baselink -> ENU
135  // to aircraft -> NED
136  auto orientation = ftf::transform_orientation_ned_enu(
138  Eigen::Quaterniond(tgt.q[0], tgt.q[1], tgt.q[2], tgt.q[3])));
139 
140  auto body_rate = ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tgt.body_roll_rate, tgt.body_pitch_rate, tgt.body_yaw_rate));
141 
142  auto target = boost::make_shared<mavros_msgs::AttitudeTarget>();
143 
144  target->header.stamp = m_uas->synchronise_stamp(tgt.time_boot_ms);
145  target->type_mask = tgt.type_mask;
146  tf::quaternionEigenToMsg(orientation, target->orientation);
147  tf::vectorEigenToMsg(body_rate, target->body_rate);
148  target->thrust = tgt.thrust;
149 
150  target_attitude_pub.publish(target);
151  }
152 
153  /* -*- callbacks -*- */
154 
156  {
157  Eigen::Vector3d position, velocity, af;
158  float yaw, yaw_rate;
159 
160  tf::pointMsgToEigen(req->position, position);
161  tf::vectorMsgToEigen(req->velocity, velocity);
162  tf::vectorMsgToEigen(req->acceleration_or_force, af);
163 
164  // Transform frame ENU->NED
165  if (req->coordinate_frame == mavros_msgs::PositionTarget::FRAME_BODY_NED || req->coordinate_frame == mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED) {
166  position = ftf::transform_frame_baselink_aircraft(position);
167  velocity = ftf::transform_frame_baselink_aircraft(velocity);
171  ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)));
172  } else {
173  position = ftf::transform_frame_enu_ned(position);
174  velocity = ftf::transform_frame_enu_ned(velocity);
179  ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
180  }
181 
182  Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
183  auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
184  yaw_rate = ang_vel_ned.z();
185 
187  req->header.stamp.toNSec() / 1000000,
188  req->coordinate_frame,
189  req->type_mask,
190  position,
191  velocity,
192  af,
193  yaw, yaw_rate);
194  }
195 
197  {
198  Eigen::Vector3d velocity, af;
199  float yaw, yaw_rate;
200 
201  tf::vectorMsgToEigen(req->velocity, velocity);
202  tf::vectorMsgToEigen(req->acceleration_or_force, af);
203 
204  // Transform frame ENU->NED
205  velocity = ftf::transform_frame_enu_ned(velocity);
210  ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
211  Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
212  auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
213  yaw_rate = ang_vel_ned.z();
214 
216  req->header.stamp.toNSec() / 1000000,
217  req->coordinate_frame,
218  req->type_mask,
219  req->latitude * 1e7,
220  req->longitude * 1e7,
221  req->altitude,
222  velocity,
223  af,
224  yaw, yaw_rate);
225  }
226 
228  {
229  double thrust_scaling;
230  Eigen::Quaterniond desired_orientation;
231  Eigen::Vector3d baselink_angular_rate;
232  Eigen::Vector3d body_rate;
233  double thrust;
234 
235  // Set Thrust scaling in px4_config.yaml, setpoint_raw block.
236  // ignore thrust is false by default, unless no thrust scalling is set or thrust is zero
237  auto ignore_thrust = req->thrust != 0.0 && !sp_nh.getParam("thrust_scaling", thrust_scaling);
238 
239  if (ignore_thrust) {
240  // I believe it's safer without sending zero thrust, but actually ignoring the actuation.
241  ROS_FATAL_THROTTLE_NAMED(5, "setpoint_raw", "Recieved thrust, but ignore_thrust is true: "
242  "the most likely cause of this is a failure to specify the thrust_scaling parameters "
243  "on px4/apm_config.yaml. Actuation will be ignored.");
244  return;
245  } else {
246  if (thrust_scaling == 0.0) {
247  ROS_WARN_THROTTLE_NAMED(5, "setpoint_raw", "thrust_scaling parameter is set to zero.");
248  }
249  thrust = std::min(1.0, std::max(0.0, req->thrust * thrust_scaling));
250  }
251 
252  // Take care of attitude setpoint
253  desired_orientation = ftf::to_eigen(req->orientation);
254 
255  // Transform desired orientation to represent aircraft->NED,
256  // MAVROS operates on orientation of base_link->ENU
257  auto ned_desired_orientation = ftf::transform_orientation_enu_ned(
258  ftf::transform_orientation_baselink_aircraft(desired_orientation));
259 
261  ftf::to_eigen(req->body_rate));
262 
264  req->header.stamp.toNSec() / 1000000,
265  req->type_mask,
266  ned_desired_orientation,
267  body_rate,
268  thrust);
269 
270  }
271 };
272 } // namespace std_plugins
273 } // namespace mavros
274 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void publish(const boost::shared_ptr< M > &message) const
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
MAVROS Plugin base.
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
void attitude_cb(const mavros_msgs::AttitudeTarget::ConstPtr &req)
void global_cb(const mavros_msgs::GlobalPositionTarget::ConstPtr &req)
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.
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
void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_GLOBAL_INT &tgt)
void local_cb(const mavros_msgs::PositionTarget::ConstPtr &req)
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
T transform_orientation_absolute_frame_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:197
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
UAS for plugins.
Definition: mavros_uas.h:67
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
void handle_position_target_local_ned(const mavlink::mavlink_message_t *msg, mavlink::common::msg::POSITION_TARGET_LOCAL_NED &tgt)
Mixin for setpoint plugins.
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
T transform_frame_ned_enu(const T &in)
Transform data expressed in NED to ENU frame.
Definition: frame_tf.h:215
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d.
Definition: frame_tf.h:452
#define ROS_FATAL_THROTTLE_NAMED(rate, name,...)
T transform_frame_baselink_aircraft(const T &in)
Transform data expressed in Baselink frame to Aircraft frame.
Definition: frame_tf.h:242
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
This mixin adds set_position_target_local_ned()
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
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.
double quaternion_get_yaw(const Eigen::Quaterniond &q)
Get Yaw angle from quaternion.
This mixin adds set_attitude_target()
bool getParam(const std::string &key, std::string &s) const
T transform_orientation_ned_enu(const T &in)
Transform from attitude represented WRT NED frame to attitude represented WRT ENU frame...
Definition: frame_tf.h:160
void handle_attitude_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_TARGET &tgt)
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.
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
T transform_orientation_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:178
This mixin adds set_position_target_global_int()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void initialize(UAS &uas_) override
Plugin initializer.


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