Create an Autonomy Plugin

SCRIMMAGE ships with a script that generates an Autonomy plugin template. The developer needs to provide the name of the Autonomy plugin and the root directory of your SCRIMMAGE plugins project. We will use the plugins project we created in the previous tutorial ( Create a SCRIMMAGE Plugins Project ) to build our new Autonomy plugin.

To create an Autonomy plugin, enter the following at the terminal:

$ cd /path/to/scrimmage/scripts
$ ./generate-plugin.sh autonomy MyFollowBehavior ~/scrimmage/my-scrimmage-plugins

Now, let’s build the plugin that was placed in the my-scrimmage-plugins project:

$ cd ~/scrimmage/my-scrimmage-plugins/build
$ cmake ..
$ make

If the plugin built successfuly, you will see the following output:

[100%] Built target MyFollowBehavior_plugin

Examine Autonomy Plugin

Autonomy Plugin Initialization

Let’s take a look at the header file ~/scrimmage/my-scrimmage-plugins/include/my-scrimmage-plugins/plugins/autonomy/MyFollowBehavior/MyFollowBehavior.h:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
 #ifndef INCLUDE_MY_SCRIMMAGE_PLUGINS_PLUGINS_AUTONOMY_MYFOLLOWBEHAVIOR_MYFOLLOWBEHAVIOR_H_
 #define INCLUDE_MY_SCRIMMAGE_PLUGINS_PLUGINS_AUTONOMY_MYFOLLOWBEHAVIOR_MYFOLLOWBEHAVIOR_H_
 #include <scrimmage/autonomy/Autonomy.h>

 #include <string>
 #include <map>

 namespace scrimmage {
 namespace autonomy {
 class MyFollowBehavior : public scrimmage::Autonomy {
  public:
     void init(std::map<std::string, std::string> &params) override;
     bool step_autonomy(double t, double dt) override;

  protected:
     int desired_alt_idx_ = 0;
     int desired_speed_idx_ = 0;
     int desired_heading_idx_ = 0;
 };
 } // namespace autonomy
 } // namespace scrimmage
 #endif // INCLUDE_MY_SCRIMMAGE_PLUGINS_PLUGINS_AUTONOMY_MYFOLLOWBEHAVIOR_MYFOLLOWBEHAVIOR_H_

Note that it inherits from the scrimmage base class Autonomy.h located at scrimmage/include/scrimmage/autonomy/Autonomy.h which provides a couple virtual methods, init and step, we will be overriding. The member variables desired_alt_idx_, desired_speed_idx_, and desired_heading_idx_ will be used to interact with the controller. For more details on interacting with controllers, see Variable IO.

Now let’s take a look at the Autonomy plugin template code to better understand the types of information that are available to the Autonomy developer. Use your favorite text editor to open the file at ~/scrimmage/my-scrimmage-plugins/src/plugins/autonomy/MyFollowBehavior/MyFollowBehavior.cpp:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
 #include <my-scrimmage-plugins/plugins/autonomy/MyFollowBehavior/MyFollowBehavior.h>

 #include <scrimmage/plugin_manager/RegisterPlugin.h>
 #include <scrimmage/entity/Entity.h>
 #include <scrimmage/math/State.h>
 #include <scrimmage/parse/ParseUtils.h>

 #include <limits>

 namespace sc = scrimmage;

 REGISTER_PLUGIN(scrimmage::Autonomy,
                 scrimmage::autonomy::MyFollowBehavior,
                 MyFollowBehavior_plugin)

After the relevant header files are included, there is a REGISTER_PLUGIN macro that adds information to your source file so scrimmage can dynamically load it at runtime. The plugin’s init function is pasted below:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
 void MyFollowBehavior::init(std::map<std::string, std::string> &params) {
     double initial_speed = sc::get<double>("initial_speed", params, 21);
     desired_alt_idx_ = vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out);
     desired_speed_idx_ = vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out);
     desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out);

     vars_.output(desired_speed_idx_, initial_speed);
     vars_.output(desired_alt_idx_, state_->pos()(2));
     vars_.output(desired_heading_idx_, state_->quat().yaw());
 }

This is just setting up the VariableIO to interact with the controller. See Variable IO for more details. The initial_speed tag is in the MyFollowBehavior.xml file, which we get from the function get found in the ParseUtils.h header available from scrimmage:

1
2
3
4
5
6
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
<params>
   <library>MyFollowBehavior_plugin</library>
   <initial_speed>24.3</initial_speed>
</params>

the sc::get<double>() function can convert the string value of initial_speed to a double value. The Autonomy plugin developer can add arbitrarly define tags in the Autonomy plugin’s XML file. The sc::get supports all standard C++ data types, such as int, double, std::string, bool, unsigned int, etc.

Set Loop Rate

Normally, the loop rate for a plugin should be set with the loop_rate xml tag inside the plugin’s block of the mission file. Using this API the user doesn’t have to write any code to set the loop rate. However, it’s also possible to change the loop rate for a plugin dynamically during a simulation. This is done by calling the plugin’s set_loop_rate() method, which sets the plugin’s loop rate in Hz:

1
2
3
 void MyFollowBehavior::update_loop_rate(double new_loop_rate) {
     set_loop_rate(new_loop_rate);
 }

The current loop rate can also be accessed with the loop_rate() function.

Step Autonomy

The real work of the Autonomy plugin is typically implemented in the step_autonomy method.

bool MyFollowBehavior::step_autonomy(double t, double dt)

The step_autonomy method takes the current simulation time, t, and the simulation step size dt, as inputs. If an error is detected during method execution, the Autonomy developer can return false to inform SCRIMMAGE’s simulation controller, but typically, true should be returned when the method finishes execution. The example Autonomy plugin that was generated implements the following simple behavior:

  1. Calculate the distance to each contact in the simulation.

  2. Find the the closest contact that is not on my team.

  3. Head in the direction of the closest contact.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
 bool MyFollowBehavior::step_autonomy(double t, double dt) {
     // Find nearest entity on other team. Loop through each contact, calculate
     // distance to entity, save the ID of the entity that is closest.
     int follow_id_ = -1;
     double min_dist = std::numeric_limits<double>::infinity();
     for (auto &kv : *contacts_) {

         int contact_id = kv.first;
         sc::Contact &contact = kv.second;

         // Skip if this contact is on the same team
         if (contact.id().team_id() == parent_->id().team_id()) {
             continue;
         }

         // Calculate distance to entity
         double dist = (contact.state()->pos() - state_->pos()).norm();

         if (dist < min_dist) {
             // If this is the minimum distance, save distance and reference to
             // entity
             min_dist = dist;
             follow_id_ = contact_id;
         }
     }

     // Head toward entity on other team
     if (contacts_->count(follow_id_) > 0) {
         // Get a reference to the entity's state.
         sc::StatePtr ent_state = contacts_->at(follow_id_).state();

         // Calculate the required heading to follow the other entity
         double heading = atan2(ent_state->pos()(1) - state_->pos()(1),
                                ent_state->pos()(0) - state_->pos()(0));

         // Set the heading
         vars_.output(desired_heading_idx_, heading);

         // Match entity's altitude
         vars_.output(desired_alt_idx_, ent_state->pos()(2));
     }

     return true;
 }

Line 6 is a common way to iterate over all contacts using range-based for loops available from c++-11. In line 12, we ignore contacts that are on the same team by checking team IDs. parent_ is a reference to the plugin’s parent entity. The parent entity holds references to the entity’s ID and other plugins. The scrimmage::ID class has three member variables that refer to the entity’s ID, the sub_swarm_id, and the team_id, respectively. In line 12, we use Eigen’s norm() function to calculate the distance between our entity and the contact. In lines 14 to 18, we determine if this is the small distance encountered so far and save the distance and ID of the contact if it is the closest distance.

In the second if block, we set the Autonomy’s controller inputs using Variable IO. Line 28 ensures that the ID of the contact that we want to follow exists. Next, we get a pointer to the contact’s scrimmage::State in line 30. Using basic trigonometry, we calculate the required heading to follow the contact in line 33. In line 37, we set the desired heading, similarly to how we set the heading in the initialization method. Finally, we set the desired altitude to the same altitude of the contact in line 40. If the closest contact search didn’t succeed in the first part of step_autonomy, then the controller inputs won’t be updated due to the guard.

By just using the state’s of other contacts, Autonomy plugins can implement formation controllers, spatial search algorithms, biologically-inspired algorithms, and many other algorithms associated with multi-robot problems. The contacts in this example are ground truth contacts without any noise. When you complete the Sensor plugin tutorial (Create a Sensor Plugin), you will learn how to add noise and filter out contacts based on a sensor model.

Running the Plugin

In our project, open missions/example.xml and go to the second entity block. Change count to 1 and autonomy to MyFollowBehavior and save the file:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
 <entity>
   <team_id>2</team_id>
   <color>255 0 0</color>
   <count>1</count>
   <health>1</health>
   <radius>2</radius>

   <!--
   <generate_rate> 1 / 2 </generate_rate>
   <generate_count>2</generate_count>
   <generate_start_time>0</generate_start_time>
   <generate_time_variance>0.10</generate_time_variance>
   -->

   <variance_x>20</variance_x>
   <variance_y>20</variance_y>
   <variance_z>20</variance_z>

   <x>50</x>
   <y>0</y>
   <z>200</z>

   <heading>180</heading>
   <altitude>200</altitude>
   <controller>SimpleAircraftControllerPID</controller>
   <motion_model>SimpleAircraft</motion_model>
   <visual_model>zephyr-red</visual_model>
   <autonomy>MyFollowBehavior</autonomy>
   <base>
     <latitude>35.719961</latitude>
     <longitude>-120.767304</longitude>
     <altitude>300</altitude>
     <radius>25</radius>
   </base>
 </entity>

Now we can run the simulation:

scrimmage missions/example.xml

You can change the viewer by pressing a once, scrolling out and moving the picture so both vehicles are in view. You can additionally press + so that the vehicles are larger. You should see two vehicles approach each other and circle.