setpoint_velocity.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 
22 #include <geometry_msgs/TwistStamped.h>
23 #include <geometry_msgs/Twist.h>
24 
25 #include <mavros_msgs/SetMavFrame.h>
26 
27 namespace mavros {
28 namespace std_plugins {
29 using mavlink::common::MAV_FRAME;
36  private plugin::SetPositionTargetLocalNEDMixin<SetpointVelocityPlugin> {
37 public:
39  sp_nh("~setpoint_velocity")
40  { }
41 
42  void initialize(UAS &uas_) override
43  {
44  PluginBase::initialize(uas_);
45 
46  //cmd_vel usually is the topic used for velocity control in many controllers / planners
47  vel_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointVelocityPlugin::vel_cb, this);
50 
51  // mav_frame
52  std::string mav_frame_str;
53  if (!sp_nh.getParam("mav_frame", mav_frame_str)) {
54  mav_frame = MAV_FRAME::LOCAL_NED;
55  } else {
56  mav_frame = utils::mav_frame_from_str(mav_frame_str);
57  }
58  }
59 
61  {
62  return { /* Rx disabled */ };
63  }
64 
65 private:
68 
72 
74 
75  /* -*- mid-level helpers -*- */
76 
82  void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate)
83  {
88  uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
89  auto vel = [&] () {
90  if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
92  } else {
93  return ftf::transform_frame_enu_ned(vel_enu);
94  }
95  } ();
96 
97  auto yr = [&] () {
98  if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
99  return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate));
100  } else {
101  return ftf::transform_frame_ned_enu(Eigen::Vector3d(0.0, 0.0, yaw_rate));
102  }
103  } ();
104 
105  set_position_target_local_ned(stamp.toNSec() / 1000000,
106  utils::enum_value(mav_frame),
107  ignore_all_except_v_xyz_yr,
108  Eigen::Vector3d::Zero(),
109  vel,
110  Eigen::Vector3d::Zero(),
111  0.0, yr.z());
112  }
113 
114  /* -*- callbacks -*- */
115 
117  Eigen::Vector3d vel_enu;
118 
119  tf::vectorMsgToEigen(req->twist.linear, vel_enu);
120  send_setpoint_velocity(req->header.stamp, vel_enu,
121  req->twist.angular.z);
122  }
123 
125  Eigen::Vector3d vel_enu;
126 
127  tf::vectorMsgToEigen(req->linear, vel_enu);
129  req->angular.z);
130  }
131 
132  bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
133  {
134  mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
135  const std::string mav_frame_str = utils::to_string(mav_frame);
136  sp_nh.setParam("mav_frame", mav_frame_str);
137  res.success = true;
138  return true;
139  }
140 };
141 } // namespace std_plugins
142 } // namespace mavros
143 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
void vel_unstamped_cb(const geometry_msgs::Twist::ConstPtr &req)
mavlink::common::MAV_FRAME mav_frame_from_str(const std::string &mav_frame)
Retreive MAV_FRAME from name.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
MAVROS Plugin base.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
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
Mixin for setpoint plugins.
bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
std::string to_string(timesync_mode e)
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
T transform_frame_baselink_aircraft(const T &in)
Transform data expressed in Baselink frame to Aircraft frame.
Definition: frame_tf.h:242
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate)
Send velocity to FCU velocity controller.
This mixin adds set_position_target_local_ned()
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
bool getParam(const std::string &key, std::string &s) const
static Time now()
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &req)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void initialize(UAS &uas_) override
Plugin initializer.
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
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