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_)
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  position = ftf::transform_frame_enu_ned(position);
166  velocity = ftf::transform_frame_enu_ned(velocity);
171  ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
172  Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
173  auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
174  yaw_rate = ang_vel_ned.z();
175 
177  req->header.stamp.toNSec() / 1000000,
178  req->coordinate_frame,
179  req->type_mask,
180  position,
181  velocity,
182  af,
183  yaw, yaw_rate);
184  }
185 
187  {
188  Eigen::Vector3d velocity, af;
189  float yaw, yaw_rate;
190 
191  tf::vectorMsgToEigen(req->velocity, velocity);
192  tf::vectorMsgToEigen(req->acceleration_or_force, af);
193 
194  // Transform frame ENU->NED
195  velocity = ftf::transform_frame_enu_ned(velocity);
200  ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
201  Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
202  auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
203  yaw_rate = ang_vel_ned.z();
204 
206  req->header.stamp.toNSec() / 1000000,
207  req->coordinate_frame,
208  req->type_mask,
209  req->latitude * 1e7,
210  req->longitude * 1e7,
211  req->altitude,
212  velocity,
213  af,
214  yaw, yaw_rate);
215  }
216 
218  {
219  double thrust_scaling;
220  Eigen::Quaterniond desired_orientation;
221  Eigen::Vector3d baselink_angular_rate;
222  Eigen::Vector3d body_rate;
223  double thrust;
224 
225  // Set Thrust scaling in px4_config.yaml, setpoint_raw block.
226  // ignore thrust is false by default, unless no thrust scalling is set or thrust is zero
227  auto ignore_thrust = req->thrust != 0.0 && !sp_nh.getParam("thrust_scaling", thrust_scaling);
228 
229  if (ignore_thrust) {
230  // I believe it's safer without sending zero thrust, but actually ignoring the actuation.
231  ROS_FATAL_THROTTLE_NAMED(5, "setpoint_raw", "Recieved thrust, but ignore_thrust is true: "
232  "the most likely cause of this is a failure to specify the thrust_scaling parameters "
233  "on px4/apm_config.yaml. Actuation will be ignored.");
234  return;
235  } else {
236  if (thrust_scaling == 0.0) {
237  ROS_WARN_THROTTLE_NAMED(5, "setpoint_raw", "thrust_scaling parameter is set to zero.");
238  }
239  thrust = std::min(1.0, std::max(0.0, req->thrust * thrust_scaling));
240  }
241 
242  // Take care of attitude setpoint
243  desired_orientation = ftf::to_eigen(req->orientation);
244 
245  // Transform desired orientation to represent aircraft->NED,
246  // MAVROS operates on orientation of base_link->ENU
247  auto ned_desired_orientation = ftf::transform_orientation_enu_ned(
248  ftf::transform_orientation_baselink_aircraft(desired_orientation));
249 
251  ftf::to_eigen(req->body_rate));
252 
254  req->header.stamp.toNSec() / 1000000,
255  req->type_mask,
256  ned_desired_orientation,
257  body_rate,
258  thrust);
259 
260  }
261 };
262 } // namespace std_plugins
263 } // namespace mavros
264 
MAVROS Plugin base class.
Definition: mavros_plugin.h:37
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:88
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:161
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:75
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
void initialize(UAS &uas_)
Plugin initializer.
UAS for plugins.
Definition: mavros_uas.h:66
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)
Subscriptions get_subscriptions()
Return vector of MAVLink message subscriptions (handlers)
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:187
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d.
Definition: frame_tf.h:423
#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:214
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:196
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:48
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:152
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:179
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:170
This mixin adds set_position_target_global_int()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


mavros
Author(s): Vladimir Ermakov
autogenerated on Mon Aug 19 2019 03:38:02