safety_area.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014 Nuno Marques.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <mavros/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 
00021 #include <geometry_msgs/PolygonStamped.h>
00022 
00023 namespace mavplugin {
00029 class SafetyAreaPlugin : public MavRosPlugin {
00030 public:
00031         SafetyAreaPlugin() :
00032                 safety_nh("~safety_area"),
00033                 uas(nullptr)
00034         { };
00035 
00036         void initialize(UAS &uas_)
00037         {
00038                 bool manual_def = false;
00039                 double p1x, p1y, p1z,
00040                         p2x, p2y, p2z;
00041 
00042                 uas = &uas_;
00043 
00044                 if (safety_nh.getParam("p1/x", p1x) &&
00045                                 safety_nh.getParam("p1/y", p1y) &&
00046                                 safety_nh.getParam("p1/z", p1z)) {
00047                         manual_def = true;
00048                         ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P1(%f %f %f)",
00049                                         p1x, p1y, p1z);
00050                 }
00051 
00052                 if (manual_def &&
00053                                 safety_nh.getParam("p2/x", p2x) &&
00054                                 safety_nh.getParam("p2/y", p2y) &&
00055                                 safety_nh.getParam("p2/z", p2z)) {
00056                         manual_def = true;
00057                         ROS_DEBUG_NAMED("safetyarea", "SA: Manual set: P2(%f %f %f)",
00058                                         p2x, p2y, p2z);
00059                 }
00060                 else
00061                         manual_def = false;
00062 
00063                 if (manual_def)
00064                         send_safety_set_allowed_area(
00065                                         p1x, p1y, p1z,
00066                                         p2x, p2y, p2z);
00067 
00068                 safetyarea_sub = safety_nh.subscribe("set", 10, &SafetyAreaPlugin::safetyarea_cb, this);
00069         }
00070 
00071         const message_map get_rx_handlers() {
00072                 return { /* Rx disabled */ };
00073                 
00075         }
00076 
00077 private:
00078         ros::NodeHandle safety_nh;
00079         UAS *uas;
00080 
00081         ros::Subscriber safetyarea_sub;
00082 
00083         /* -*- low-level send -*- */
00084 
00085         void safety_set_allowed_area(
00086                         uint8_t coordinate_frame,
00087                         float p1x, float p1y, float p1z,
00088                         float p2x, float p2y, float p2z) {
00089                 mavlink_message_t msg;
00090                 mavlink_msg_safety_set_allowed_area_pack_chan(UAS_PACK_CHAN(uas), &msg,
00091                                 UAS_PACK_TGT(uas),
00092                                 coordinate_frame,
00093                                 p1x, p1y, p1z,
00094                                 p2x, p2y, p2z);
00095                 UAS_FCU(uas)->send_message(&msg);
00096         }
00097 
00098         /* -*- mid-level helpers -*- */
00099 
00106         void send_safety_set_allowed_area(float p1x, float p1y, float p1z,
00107                         float p2x, float p2y, float p2z) {
00108                 ROS_INFO_NAMED("safetyarea", "SA: Set safty area: P1(%f %f %f) P2(%f %f %f)",
00109                                 p1x, p1y, p1z,
00110                                 p2x, p2y, p2z);
00111 
00112                 auto p1 = UAS::transform_frame_enu_ned(Eigen::Vector3d(p1x, p1y, p1z));
00113                 auto p2 = UAS::transform_frame_enu_ned(Eigen::Vector3d(p2x, p2y, p2z));
00114 
00115                 safety_set_allowed_area(
00116                                 MAV_FRAME_LOCAL_NED, // TODO: use enum from lib
00117                                 p1.x(), p1.y(), p1.z(),
00118                                 p2.x(), p2.y(), p2.z());
00119         }
00120 
00121         /* -*- callbacks -*- */
00122 
00123         void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req) {
00124                 if (req->polygon.points.size() != 2) {
00125                         ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points");
00126                         return;
00127                 }
00128 
00129                 send_safety_set_allowed_area(
00130                                 req->polygon.points[0].x,
00131                                 req->polygon.points[0].y,
00132                                 req->polygon.points[0].z,
00133                                 req->polygon.points[1].x,
00134                                 req->polygon.points[1].y,
00135                                 req->polygon.points[1].z);
00136         }
00137 };
00138 };      // namespace mavplugin
00139 
00140 PLUGINLIB_EXPORT_CLASS(mavplugin::SafetyAreaPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19