safety_area.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 
20 #include <geometry_msgs/PolygonStamped.h>
21 
22 namespace mavros {
23 namespace std_plugins {
30 public:
32  safety_nh("~safety_area")
33  { }
34 
35  void initialize(UAS &uas_) override
36  {
37  PluginBase::initialize(uas_);
38 
39  safety_nh.param<std::string>("frame_id", frame_id, "safety_area");
40 
41  bool manual_def = false;
42  double p1x, p1y, p1z,
43  p2x, p2y, p2z;
44 
45  if (safety_nh.getParam("p1/x", p1x) &&
46  safety_nh.getParam("p1/y", p1y) &&
47  safety_nh.getParam("p1/z", p1z)) {
48  manual_def = true;
49  ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P1(%f %f %f)",
50  p1x, p1y, p1z);
51  }
52 
53  if (manual_def &&
54  safety_nh.getParam("p2/x", p2x) &&
55  safety_nh.getParam("p2/y", p2y) &&
56  safety_nh.getParam("p2/z", p2z)) {
57  manual_def = true;
58  ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P2(%f %f %f)",
59  p2x, p2y, p2z);
60  }
61  else
62  manual_def = false;
63 
64  if (manual_def)
66  Eigen::Vector3d(p1x, p1y, p1z),
67  Eigen::Vector3d(p2x, p2y, p2z));
68 
70  safetyarea_pub = safety_nh.advertise<geometry_msgs::PolygonStamped>("get",10);
71  }
72 
74  {
75  return {
77  };
78  }
79 
80 private:
82 
83  std::string frame_id;
84 
87 
88  /* -*- rx handlers -*- */
89  void handle_safety_allowed_area(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SAFETY_ALLOWED_AREA &saa)
90  {
91  auto saa_msg = boost::make_shared<geometry_msgs::PolygonStamped>();
92 
93  Eigen::Vector3d p1(saa.p1x, saa.p1y, saa.p1z);
94  Eigen::Vector3d p2(saa.p2x, saa.p2y, saa.p2z);
95 
98 
99  saa_msg->header.stamp = ros::Time::now();
100  saa_msg->header.frame_id = frame_id;
101  //saa_msg->header.frame_id = utils::to_string(saa.frame); @TODO: after #311 merged this will work
102 
103  saa_msg->polygon.points.resize(2);
104  saa_msg->polygon.points[0].x = p1.x();
105  saa_msg->polygon.points[0].y = p1.y();
106  saa_msg->polygon.points[0].z = p1.z();
107  saa_msg->polygon.points[1].x = p2.x();
108  saa_msg->polygon.points[1].y = p2.y();
109  saa_msg->polygon.points[1].z = p2.z();
110 
111  safetyarea_pub.publish(saa_msg);
112  }
113 
114  /* -*- mid-level helpers -*- */
115 
122  void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
123  {
124  ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safety area: P1 " << p1 << " P2 " << p2);
125 
128 
129  mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s = {};
130  m_uas->msg_set_target(s);
131 
132  // TODO: use enum from lib
133  s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);
134 
135  // [[[cog:
136  // for p in range(1, 3):
137  // for v in ('x', 'y', 'z'):
138  // cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
139  // ]]]
140  s.p1x = p1.x();
141  s.p1y = p1.y();
142  s.p1z = p1.z();
143  s.p2x = p2.x();
144  s.p2y = p2.y();
145  s.p2z = p2.z();
146  // [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)
147 
148  UAS_FCU(m_uas)->send_message_ignore_drop(s);
149  }
150 
151  /* -*- callbacks -*- */
152 
154  {
155  if (req->polygon.points.size() != 2) {
156  ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points");
157  return;
158  }
159 
160  // eigen_conversions do not have convertor for Point32
161  // [[[cog:
162  // for p in range(2):
163  // cog.outl("Eigen::Vector3d p%d(%s);" % (p + 1, ', '.join([
164  // 'req->polygon.points[%d].%s' % (p, v) for v in ('x', 'y', 'z')
165  // ])))
166  // ]]]
167  Eigen::Vector3d p1(req->polygon.points[0].x, req->polygon.points[0].y, req->polygon.points[0].z);
168  Eigen::Vector3d p2(req->polygon.points[1].x, req->polygon.points[1].y, req->polygon.points[1].z);
169  // [[[end]]] (checksum: c3681d584e02f7d91d6b3b48f87b1771)
170 
172  }
173 };
174 } // namespace std_plugins
175 } // namespace mavros
176 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req)
std::shared_ptr< MAVConnInterface const > ConstPtr
void publish(const boost::shared_ptr< M > &message) const
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
Safety allopwed area plugin.
Definition: safety_area.cpp:29
void msg_set_target(_T &msg)
Definition: mavros_uas.h:388
#define ROS_INFO_STREAM_NAMED(name, args)
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
#define ROS_DEBUG_NAMED(name,...)
UAS for plugins.
Definition: mavros_uas.h:67
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
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)
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
void handle_safety_allowed_area(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SAFETY_ALLOWED_AREA &saa)
Definition: safety_area.cpp:89
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: safety_area.cpp:73
void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
Send a safety zone (volume), which is defined by two corners of a cube, to the FCU.
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
static Time now()
void initialize(UAS &uas_) override
Plugin initializer.
Definition: safety_area.cpp:35
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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