Basic Formation Control

include/scrimmage/plugins/autonomy/Formation/Formation.h

@@ -0,0 +1,127 @@
+/*!
+ * @file
+ *
+ * @section LICENSE
+ *
+ * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI)
+ *
+ * This file is part of SCRIMMAGE.
+ *
+ *   SCRIMMAGE is free software: you can redistribute it and/or modify it under
+ *   the terms of the GNU Lesser General Public License as published by the
+ *   Free Software Foundation, either version 3 of the License, or (at your
+ *   option) any later version.
+ *
+ *   SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT
+ *   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ *   License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with SCRIMMAGE.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
+ * @author Eric Squires <eric.squires@gtri.gatech.edu>
+ * @date 31 July 2017
+ * @version 0.1.0
+ * @brief Brief file description.
+ * @section DESCRIPTION
+ * A Long description goes here.
+ *
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FORMATION_FORMATION_H_
+#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FORMATION_FORMATION_H_
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/entity/Contact.h>
+
+#include <Eigen/Dense>
+
+#include <map>
+#include <string>
+#include <memory>
+#include <map>
+
+namespace scrimmage {
+
+namespace interaction {
+class BoundaryBase;
+}
+
+namespace autonomy {
+class Formation : public scrimmage::Autonomy{
+ public:
+    void init(std::map<std::string, std::string> &params) override;
+    bool step_autonomy(double t, double dt) override;
+
+ protected:
+    bool leader_;
+    double leader_speed_;
+    int leader_id;
+    float leader_x_pos;
+    float leader_y_pos;
+    float leader_z_pos;
+    float safety_dist_;
+
+    double follower_speed_;
+    double follow_v_k_;
+    double x_disp_;
+    double y_disp_;
+    double z_disp_;
+    double lead_to_follow_dist_;
+
+    // Entity avoidance
+    bool avoid_non_team_;
+    double sphere_of_influence_;
+    double minimum_range_;
+    Eigen::Vector3d desired_vector_;
+    double fat_guard_;
+    bool avoid_state;
+
+    bool show_shapes_;
+    scrimmage_proto::ShapePtr circle_shape_;
+
+    scrimmage::PublisherPtr leader_pub_;
+    scrimmage::PublisherPtr follower_track_pub_;
+
+    int ent_id;
+    //std::map<int,int> follower_track; //ent_id, follower number
+
+    struct FollowerData {
+      double x_pos;
+      double y_pos;
+      double z_pos;
+      int ent_id;
+    };
+
+    std::map<int, FollowerData> follower_map;
+
+    Eigen::Vector3d goal_;
+
+    int frame_number_;
+
+    bool enable_boundary_control_ = false;
+    std::shared_ptr<scrimmage::interaction::BoundaryBase> boundary_;
+
+    int desired_alt_idx_ = 0;
+    int desired_speed_idx_ = 0;
+    int desired_heading_idx_ = 0;
+
+    scrimmage_proto::ShapePtr text_shape_;
+    scrimmage_proto::ShapePtr sphere_shape_;
+
+    bool noisy_state_set_ = false;
+    State noisy_state_;
+
+    ContactMap noisy_contacts_;
+
+    double desired_z_ = 0;
+
+    double prev_gen_time_ = -1.0;
+
+    void avoidance_vectors(ContactMap &contacts,
+                           std::vector<Eigen::Vector3d> &O_vecs);
+};
+} // namespace autonomy
+} // namespace scrimmage
+#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_FORMATION_FORMATION_H_

include/scrimmage/plugins/autonomy/Formation/Formation.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
+<params>
+  <library>Formation_plugin</library>
+
+  <leader_speed>15</leader_speed>
+  <leader>false</leader>
+
+  <!-- Safety distance is always a positive value -->
+  <safety_dist>10</safety_dist>
+
+  add an assert that checks if the avoidance dist, safety dist, follow dist etc make sense together
+
+  <follower_speed>15</follower_speed>
+  <follow_v_k>0.55</follow_v_k>
+  <x_disp>0</x_disp>
+  <y_disp>0</y_disp>
+  <z_disp>0</z_disp>
+  <lead_to_follow_dist>50</lead_to_follow_dist>
+
+  <show_shapes>false</show_shapes>
+
+  <avoid_non_team>true</avoid_non_team_>
+  <sphere_of_influence>30</sphere_of_influence>
+  <minimum_range>5</minimum_range>
+  <fat_guard>5</fat_guard>
+
+  <enable_boundary_control>false</enable_boundary_control>
+</params>

include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h

@@ -70,6 +70,10 @@ class SimpleAircraftControllerPID : public Controller {
     uint8_t output_throttle_idx_ = 0;
     uint8_t output_roll_rate_idx_ = 0;
     uint8_t output_pitch_rate_idx_ = 0;
+
+    uint8_t desired_pitch_idx_ = 0;
+    uint8_t desired_roll_idx_ = 0;
+    uint8_t desired_speed_idx_ = 0;
 };
 }  // namespace controller
 }  // namespace scrimmage