home_position.cpp
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2017 Thomas Stastny, 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 #include <mavros/mavros_plugin.h>
19 
20 #include <std_srvs/Trigger.h>
21 #include <mavros_msgs/CommandLong.h>
22 #include <mavros_msgs/HomePosition.h>
23 
24 
25 namespace mavros {
26 namespace std_plugins {
33 public:
35  hp_nh("~home_position"),
37  { }
38 
39  void initialize(UAS &uas_) override
40  {
41  PluginBase::initialize(uas_);
42 
43  hp_pub = hp_nh.advertise<mavros_msgs::HomePosition>("home", 2, true);
46 
48  poll_timer.stop();
50  }
51 
53  {
54  return {
56  };
57  }
58 
59 private:
61 
65 
67 
68  static constexpr int REQUEST_POLL_TIME_MS = 10000;
70 
72  {
73  using mavlink::common::MAV_CMD;
74 
75  bool ret = false;
76 
77  try {
78  ros::NodeHandle pnh("~");
79  auto client = pnh.serviceClient<mavros_msgs::CommandLong>("cmd/command");
80 
81  mavros_msgs::CommandLong cmd{};
82 
83  cmd.request.command = utils::enum_value(MAV_CMD::GET_HOME_POSITION);
84 
85  ret = client.call(cmd);
86  ret = cmd.response.success;
87  }
88  catch (ros::InvalidNameException &ex) {
89  ROS_ERROR_NAMED("home_position", "HP: %s", ex.what());
90  }
91 
92  return ret;
93  }
94 
95  void handle_home_position(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HOME_POSITION &home_position)
96  {
97  poll_timer.stop();
98 
99  auto hp = boost::make_shared<mavros_msgs::HomePosition>();
100 
101  auto pos = ftf::transform_frame_ned_enu(Eigen::Vector3d(home_position.x, home_position.y, home_position.z));
103  auto hp_approach_enu = ftf::transform_frame_ned_enu(Eigen::Vector3d(home_position.approach_x, home_position.approach_y, home_position.approach_z));
104 
105  hp->header.stamp = ros::Time::now();
106  hp->geo.latitude = home_position.latitude / 1E7; // deg
107  hp->geo.longitude = home_position.longitude / 1E7; // deg
108  hp->geo.altitude = home_position.altitude / 1E3 + m_uas->geoid_to_ellipsoid_height(&hp->geo); // in meters
109  tf::quaternionEigenToMsg(q, hp->orientation);
110  tf::pointEigenToMsg(pos, hp->position);
111  tf::vectorEigenToMsg(hp_approach_enu, hp->approach);
112 
113  ROS_DEBUG_NAMED("home_position", "HP: Home lat %f, long %f, alt %f", hp->geo.latitude, hp->geo.longitude, hp->geo.altitude);
114  hp_pub.publish(hp);
115  }
116 
118  {
119  mavlink::common::msg::SET_HOME_POSITION hp {};
120 
121  Eigen::Vector3d pos, approach;
122  Eigen::Quaterniond q;
123 
124  tf::pointMsgToEigen(req->position, pos);
125  pos = ftf::transform_frame_enu_ned(pos);
126 
127  tf::quaternionMsgToEigen(req->orientation, q);
129 
130  tf::vectorMsgToEigen(req->approach, approach);
131  approach = ftf::transform_frame_enu_ned(approach);
132 
133  hp.target_system = m_uas->get_tgt_system();
135 
136  hp.altitude = req->geo.altitude * 1e3 + m_uas->ellipsoid_to_geoid_height(&req->geo);
137  // [[[cog:
138  // for f, m in (('latitude', '1e7'), ('longitude', '1e7')):
139  // cog.outl("hp.{f} = req->geo.{f} * {m};".format(**locals()))
140  // for a, b in (('', 'pos'), ('approach_', 'approach')):
141  // for f in "xyz":
142  // cog.outl("hp.{a}{f} = {b}.{f}();".format(**locals()))
143  // ]]]
144  hp.latitude = req->geo.latitude * 1e7;
145  hp.longitude = req->geo.longitude * 1e7;
146  hp.x = pos.x();
147  hp.y = pos.y();
148  hp.z = pos.z();
149  hp.approach_x = approach.x();
150  hp.approach_y = approach.y();
151  hp.approach_z = approach.z();
152  // [[[end]]] (checksum: 9c40c5b3ac06b3b82016b4f07a8e12b2)
153 
154  UAS_FCU(m_uas)->send_message_ignore_drop(hp);
155  }
156 
157  bool req_update_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
158  {
159  res.success = call_get_home_position();
160  return true;
161  }
162 
163  void timeout_cb(const ros::TimerEvent &event)
164  {
165  ROS_INFO_NAMED("home_position", "HP: requesting home position");
167  }
168 
169  void connection_cb(bool connected) override
170  {
171  if (connected)
172  poll_timer.start();
173  else
174  poll_timer.stop();
175  }
176 };
177 } // namespace std_plugins
178 } // namespace mavros
179 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_INFO_NAMED(name,...)
double ellipsoid_to_geoid_height(T lla)
Conversion from height above ellipsoid (WGS-84) to height above geoid (AMSL)
Definition: mavros_uas.h:263
void publish(const boost::shared_ptr< M > &message) const
ssize_t pos
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.
string cmd
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
Eigen::Quaterniond mavlink_to_quaternion(const std::array< float, 4 > &q)
Convert Mavlink float[4] quaternion to Eigen.
Definition: frame_tf.h:384
void stop()
void start()
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
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
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_home_position(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HOME_POSITION &home_position)
#define ROS_DEBUG_NAMED(name,...)
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:159
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
UAS for plugins.
Definition: mavros_uas.h:67
const ros::Duration REQUEST_POLL_TIME_DT
position refresh poll interval
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void home_position_cb(const mavros_msgs::HomePosition::ConstPtr &req)
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void timeout_cb(const ros::TimerEvent &event)
void connection_cb(bool connected) override
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
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
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 quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
#define ROS_ERROR_NAMED(name,...)
static Time now()
bool req_update_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
double geoid_to_ellipsoid_height(T lla)
Conversion from height above geoid (AMSL) to height above ellipsoid (WGS-84)
Definition: mavros_uas.h:250
void initialize(UAS &uas_) override
Plugin initializer.
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