Bring back the gazebo plugins (#1063)

The models and meshes are not included. We will need
to find an alternate way to reintegrate these and use them.

* Add simulation/gz_msgs back, and build with Gradle.

* Add back in the frc simulation plugins for gazebo.

* Add a new shared library, halsim_gazebo.

This library will become the interface between the
HAL sim layer and gazebo.

* Preserve the first channel number used in created Encoders in the Sim MockData.

This allows us to use the DIO channel number to connect with simulated encoders.

* Have the HAL Simulator set the reverse direction on creation.

This enables a simulator to be aware of the direction.

* Add a drive_motor plugin.

This is a bit of a 'magic' motor, which allows us to build robot
models that drive in a more realistic fashion.  It does this
by apply forces directly to the chassis, rather than relying on
the complex motion dynamics of a driven wheel.

This in turn allows the model to reduce wheel friction,
reducing scrub, and allowing for a more natural driving experience.

simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "clock.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(Clock)
+
+void Clock::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  gzmsg << "Initializing clock: " << topic << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  pub = node->Advertise<gazebo::msgs::Float64>(topic);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Clock::Update, this, _1));
+}
+
+void Clock::Update(const gazebo::common::UpdateInfo& info) {
+  gazebo::msgs::Float64 msg;
+  msg.set_data(info.simTime.Double());
+  pub->Publish(msg);
+}

simulation/frc_gazebo_plugins/src/clock/headers/clock.h

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for publishing the simulation time.
+ *
+ * This plugin publishes the simualtaion time in seconds every physics
+ * update.
+ *
+ * To add a clock to your robot, add the following XML to your robot
+ * model:
+ *
+ *     <plugin name="my_clock" filename="libclock.so">
+ *       <topic>~/my/topic</topic>
+ *     </plugin>
+ *
+ * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
+ *
+ * \todo Make WorldPlugin?
+ */
+class Clock : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the clock and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out time each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Publish the time on this topic.
+  std::string topic;
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Publisher handle.
+  gazebo::transport::PublisherPtr pub;
+};

simulation/frc_gazebo_plugins/src/clockTest/cpp/clockTest.cpp

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <simulation/gz_msgs/msgs.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "gtest/gtest.h"
+
+using namespace testing;
+
+static char* library;
+static char* world_sdf;
+static double latest_time;
+
+void cb(gazebo::msgs::ConstFloat64Ptr& msg) { latest_time = msg->data(); }
+
+TEST(ClockTests, test_clock) {
+  gazebo::physics::WorldPtr world;
+
+  ASSERT_TRUE(library);
+  ASSERT_TRUE(!access(library, X_OK | R_OK));
+  ASSERT_TRUE(world_sdf);
+  ASSERT_TRUE(!access(world_sdf, R_OK));
+
+  gazebo::addPlugin(library);
+  ASSERT_TRUE(gazebo::setupServer());
+
+  world = gazebo::loadWorld(world_sdf);
+  ASSERT_TRUE(world != NULL);
+
+  ASSERT_TRUE(gazebo::client::setup());
+
+  gazebo::transport::NodePtr node(new gazebo::transport::Node());
+  node->Init();
+
+  gazebo::transport::SubscriberPtr sub =
+      node->Subscribe("/gazebo/frc/time", cb);
+
+  gazebo::sensors::run_once(true);
+  gazebo::sensors::run_threads();
+  gazebo::common::Time::MSleep(1);
+
+  double start = latest_time;
+
+  for (unsigned int i = 0; i < 1000; ++i) {
+    gazebo::runWorld(world, 1);
+    gazebo::sensors::run_once();
+    gazebo::common::Time::MSleep(1);
+  }
+
+  double end = latest_time;
+
+  ASSERT_TRUE(end > start);
+  ASSERT_TRUE(gazebo::client::shutdown());
+  ASSERT_TRUE(gazebo::shutdown());
+}
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+
+  if (argc >= 1) library = argv[1];
+
+  if (argc >= 2) world_sdf = argv[2];
+
+  return RUN_ALL_TESTS();
+}

simulation/frc_gazebo_plugins/src/clockTest/world/clockTest.world

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+    <world name="default">
+        <scene>
+            <ambient>0.2 0.2 0.2 1</ambient>
+            <background>1 1 1 1</background>
+            <shadows>false</shadows>
+            <grid>false</grid>
+            <origin_visual>false</origin_visual>
+        </scene>
+        <model name='Clock'>
+            <plugin name='clock' filename='libclock.so'>
+                <topic>/gazebo/frc/time</topic>
+            </plugin>
+        </model>
+    </world>
+</sdf>

simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "dc_motor.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(DCMotor)
+
+void DCMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+  signal = 0;
+
+  // Parse SDF properties
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("multiplier")) {
+    multiplier = sdf->Get<double>("multiplier");
+  } else {
+    multiplier = 1;
+  }
+
+  gzmsg << "Initializing motor: " << topic << " joint=" << joint->GetName()
+        << " multiplier=" << multiplier << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  sub = node->Subscribe(topic, &DCMotor::Callback, this);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&DCMotor::Update, this, _1));
+}
+
+void DCMotor::Update(const gazebo::common::UpdateInfo& info) {
+  joint->SetForce(0, signal * multiplier);
+}
+
+void DCMotor::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  signal = msg->data();
+  if (signal < -1) {
+    signal = -1;
+  } else if (signal > 1) {
+    signal = 1;
+  }
+}

simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for controlling a joint with a DC motor.
+ *
+ * This plugin subscribes to a topic to get a signal in the range
+ * [-1,1]. Every physics update the joint's torque is set as
+ * multiplier*signal.
+ *
+ * To add a DC motor to your robot, add the following XML to your
+ * robot model:
+ *
+ *     <plugin name="my_motor" filename="libdc_motor.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <multiplier>Number</multiplier>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this Dc motor is attached to.
+ * - `topic`: Optional. Message type should be gazebo.msgs.Float64.
+ * - `multiplier`: Optional. Defaults to 1.
+ */
+class DCMotor : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the dc motor and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Update the torque on the joint from the dc motor each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Topic to read control signal from.
+  std::string topic;
+
+  /// \brief The pwm signal limited to the range [-1,1].
+  double signal;
+
+  /// \brief The magic torque multiplier. torque=multiplier*signal
+  double multiplier;
+
+  /// \brief The joint that this dc motor drives.
+  gazebo::physics::JointPtr joint;
+
+  /// \brief Callback for receiving msgs and storing the signal.
+  void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer toe the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Subscriber handle.
+  gazebo::transport::SubscriberPtr sub;
+};

simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp

@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "drive_motor.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(DriveMotor)
+
+void DriveMotor::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+  signal = 0;
+
+  // Parse SDF properties
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (!joint) {
+    gzerr << "Error initializing drive motor: could not get joint";
+    return;
+  }
+
+  parent = joint->GetParent();
+  if (!parent) {
+    gzerr << "Error initializing drive motor: could not get parent";
+    return;
+  }
+
+  child = joint->GetChild();
+  if (!child) {
+    gzerr << "Error initializing drive motor: could not get child";
+    return;
+  }
+
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("max_speed")) {
+    maxSpeed = sdf->Get<double>("max_speed");
+  } else {
+    maxSpeed = 0;
+  }
+
+  if (sdf->HasElement("multiplier")) {
+    multiplier = sdf->Get<double>("multiplier");
+  } else {
+    multiplier = 1;
+  }
+
+  if (sdf->HasElement("dx")) {
+    dx = sdf->Get<double>("dx");
+  } else {
+    dx = 0;
+  }
+
+  if (sdf->HasElement("dy")) {
+    dy = sdf->Get<double>("dy");
+  } else {
+    dy = 0;
+  }
+
+  if (sdf->HasElement("dz")) {
+    dz = sdf->Get<double>("dz");
+  } else {
+    dz = 0;
+  }
+
+  gzmsg << "Initializing drive motor: " << topic
+        << " parent=" << parent->GetName() << " directions=" << dx << " " << dy
+        << " " << dz << " multiplier=" << multiplier << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  sub = node->Subscribe(topic, &DriveMotor::Callback, this);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&DriveMotor::Update, this, _1));
+}
+
+static double computeForce(double input, double velocity, double max) {
+  double output = input;
+  if (max == 0.0) return output;
+  if (std::fabs(velocity) >= max) {
+    output = 0;
+  } else {
+    double reduce = (max - std::fabs(velocity)) / max;
+    output *= reduce;
+  }
+  return output;
+}
+
+void DriveMotor::Update(const gazebo::common::UpdateInfo& info) {
+#if GAZEBO_MAJOR_VERSION >= 8
+  ignition::math::Vector3d velocity = parent->RelativeLinearVel();
+#else
+  ignition::math::Vector3d velocity = parent->GetRelativeLinearVel().Ign();
+#endif
+
+  if (signal == 0) return;
+
+  double x = computeForce(signal * dx * multiplier, velocity.X(),
+                          std::fabs(maxSpeed * dx));
+  double y = computeForce(signal * dy * multiplier, velocity.Y(),
+                          std::fabs(maxSpeed * dy));
+  double z = computeForce(signal * dz * multiplier, velocity.Z(),
+                          std::fabs(maxSpeed * dz));
+
+  ignition::math::Vector3d force(x, y, z);
+#if GAZEBO_MAJOR_VERSION >= 8
+  parent->AddLinkForce(force, child->RelativePose().Pos());
+#else
+  parent->AddLinkForce(force, child->GetRelativePose().Ign().Pos());
+#endif
+}
+
+void DriveMotor::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  signal = msg->data();
+  if (signal < -1) {
+    signal = -1;
+  } else if (signal > 1) {
+    signal = 1;
+  }
+}

simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h

@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for simulating a drive motor
+ *
+ * This plugin attempts to overcome a limitation in gazebo.
+ * That is, most normal FRC robots rely on wheels that have good
+ * traction in one direction, and less traction in the opposite
+ * direction.
+ *
+ * Gazebo does not model that well (in fact, drive wheels are
+ * quite hard to simulate).
+ *
+ * So this plugin subscribes to a PWM output signal and applies
+ * a force to the chassis at the proscribed point in hopefully
+ * the correct direction.  The SDF model can then have lower friction,
+ * and it should turn more naturally.
+ *
+ * This plugin also attempts to simulate the limitations of a drive
+ * motor, most notably the maximum speed any given motor can spin at.
+ * The initial implemention is quite naive; just a linear reduction
+ * in force as a product of velocity/max velocity.
+ *
+ * Nicely, this plugin let's you generate a force in any of
+ * three axes.  That is helpful for simulating a mecanum drive.
+ *
+ * This plugin subscribes to a topic to get a signal in the range
+ * [-1,1]. Every physics update the joint's torque is set as
+ * multiplier*signal*direction.
+ *
+ * To add a drive motor to your robot, add the following XML to your
+ * robot model:
+ *
+ *     <plugin name="my_motor" filename="libdrive_motor.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <multiplier>Number</multiplier>
+ *       <max_speed>Number</max_speed>
+ *       <dx>-1, 0, or 1</dx>
+ *       <dy>-1, 0, or 1</dy>
+ *       <dz>-1, 0, or 1</dz>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this Dc motor is attached to.
+ * - `topic`: Optional. Message type should be gazebo.msgs.Float64.
+ *            A typical topic looks like this:
+ *              /gazebo/frc/simulator/pwm/<n>
+ * - `multiplier`: Optional. Defaults to 1.  Force applied by this motor.
+ *            This is force in Newtons.
+ * - `max_speed`:  Optional. Defaults to no maximum.
+ *            This is, in theory, meters/second.  Note that friction
+ *            and other forces will also slow down a robot.
+ *            In practice, this term can be tuned until the robot feels right.
+ * - `dx`:  These three constants must be set to either -1, 0, or 1
+ * - `dy`:  This controls whether or not the motor produces force
+ * - `dz`:  along a given axis, and what direction.  Each defaults to 0.
+ *          These are relative to the frame of the parent link of the joint.
+ *          So they are usually relative to a chassis.
+ *          The force is applied at the point that the joint connects to
+ *          the parent link.
+ */
+class DriveMotor : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the dc motor and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Update the force on the parent of the joint from each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Topic to read control signal from.
+  std::string topic;
+
+  /// \brief The pwm signal limited to the range [-1,1].
+  double signal;
+
+  /// \brief The robot's maximum speed
+  double maxSpeed;
+
+  /// \brief The magic drive force multipliers. force=multiplier*signal
+  double multiplier;
+
+  /// \brief The directional constants limited to -1, 0, or 1.
+  double dx;
+  double dy;
+  double dz;
+
+  /// \brief The joint that this motor drives.
+  gazebo::physics::JointPtr joint;
+
+  /// \brief The parent of this joint; usually a chassis
+  gazebo::physics::LinkPtr parent;
+
+  /// \brief The child of this joint; usually a wheel
+  gazebo::physics::LinkPtr child;
+
+  /// \brief Callback for receiving msgs and storing the signal.
+  void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer toe the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Subscriber handle.
+  gazebo::transport::SubscriberPtr sub;
+};

simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "encoder.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(Encoder)
+
+void Encoder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("units")) {
+    radians = sdf->Get<std::string>("units") != "degrees";
+  } else {
+    radians = true;
+  }
+  multiplier = 1.0;
+  zero = GetAngle();
+  stopped = true;
+  stop_value = 0;
+
+  if (sdf->HasElement("multiplier"))
+    multiplier = sdf->Get<double>("multiplier");
+
+  gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
+        << " radians=" << radians << " multiplier=" << multiplier << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  command_sub = node->Subscribe(topic + "/control", &Encoder::Callback, this);
+  pos_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/position");
+  vel_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/velocity");
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Encoder::Update, this, _1));
+}
+
+void Encoder::Update(const gazebo::common::UpdateInfo& info) {
+  gazebo::msgs::Float64 pos_msg, vel_msg;
+  if (stopped) {
+    pos_msg.set_data(stop_value * multiplier);
+    pos_pub->Publish(pos_msg);
+    vel_msg.set_data(0);
+    vel_pub->Publish(vel_msg);
+  } else {
+    pos_msg.set_data((GetAngle() - zero) * multiplier);
+    pos_pub->Publish(pos_msg);
+    vel_msg.set_data(GetVelocity() * multiplier);
+    vel_pub->Publish(vel_msg);
+  }
+}
+
+void Encoder::Callback(const gazebo::msgs::ConstStringPtr& msg) {
+  std::string command = msg->data();
+  if (command == "reset") {
+    zero = GetAngle();
+  } else if (command == "start") {
+    stopped = false;
+    zero = (GetAngle() - stop_value);
+  } else if (command == "stop") {
+    stopped = true;
+    stop_value = GetAngle();
+  } else {
+    gzerr << "WARNING: Encoder got unknown command '" << command << "'."
+          << std::endl;
+  }
+}
+
+double Encoder::GetAngle() {
+  if (radians) {
+    return joint->GetAngle(0).Radian();
+  } else {
+    return joint->GetAngle(0).Degree();
+  }
+}
+
+double Encoder::GetVelocity() {
+  if (radians) {
+    return joint->GetVelocity(0);
+  } else {
+    return joint->GetVelocity(0) * (180.0 / M_PI);
+  }
+}

simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h

@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for reading the speed and relative angle of a joint.
+ *
+ * This plugin publishes the angle since last reset and the speed of a
+ * given joint to subtopics of the given topic every physics
+ * update. There is also a control subtopic that takes three commands:
+ * "start", "stop" and "reset":
+ *
+ * - "start": Start counting ticks from the current count.
+ * - "stop":  Stop counting ticks, pauses updates.
+ * - "reset": Set the current angle to zero.
+ *
+ * To add a encoder to your robot, add the following XML to your
+ * robot model:
+ *
+ *     <plugin name="my_encoder" filename="libencoder.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <units>{degrees, radians}</units>
+ *       <multiplier>Number</multiplier>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this encoder is attached to.
+ * - `topic`: Optional. Used as the root for subtopics. `topic`/position
+ * (gazebo.msgs.Float64),
+ *            `topic`/velocity (gazebo.msgs.Float64), `topic`/control
+ * (gazebo.msgs.String)
+ *    The suggested value for topic is of the form
+ *       ~/simulator/encoder/dio/<n>
+ *    where <n> is the number of the first digital input channel
+ *    used to formulate the encoder
+ *
+ * - `units`: Optional. Defaults to radians.
+ * - `multiplier`: Optional. Defaults to 1.
+ *     This can be used to make the simulated encoder
+ *     return a comparable number of ticks to a 'real' encoder
+ *     Useful facts:  A 'degrees' encoder will report 360 ticks/revolution.
+ *     The k4X encoder type can add another multiple of 4 into the mix.
+ */
+class Encoder : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the encoder and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out the encoder reading each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Root topic for subtopics on this topic.
+  std::string topic;
+
+  /// \brief Whether or not this encoder measures radians or degrees.
+  bool radians;
+
+  /// \brief A factor to mulitply this output by.
+  double multiplier;
+
+  /// \brief Whether or not the encoder has been stopped.
+  bool stopped;
+
+  /// \brief The zero value of the encoder.
+  double zero;
+
+  /// \brief The value the encoder stopped counting at
+  double stop_value;
+
+  /// \brief The joint that this encoder measures
+  gazebo::physics::JointPtr joint;
+
+  /// \brief Callback for handling control data
+  void Callback(const gazebo::msgs::ConstStringPtr& msg);
+
+  /// \brief Gets the current angle, taking into account whether to
+  ///        return radians or degrees.
+  double GetAngle();
+
+  /// \brief Gets the current velocity, taking into account whether to
+  ///        return radians/second or degrees/second.
+  double GetVelocity();
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Subscriber handle.
+  gazebo::transport::SubscriberPtr command_sub;
+
+  /// \brief Publisher handles.
+  gazebo::transport::PublisherPtr pos_pub, vel_pub;
+};

simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "gyro.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(Gyro)
+
+void Gyro::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  link = model->GetLink(sdf->Get<std::string>("link"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  std::string axisString = sdf->Get<std::string>("axis");
+  if (axisString == "roll") axis = Roll;
+  if (axisString == "pitch") axis = Pitch;
+  if (axisString == "yaw") axis = Yaw;
+
+  if (sdf->HasElement("units")) {
+    radians = sdf->Get<std::string>("units") != "degrees";
+  } else {
+    radians = true;
+  }
+  zero = GetAngle();
+
+  gzmsg << "Initializing gyro: " << topic << " link=" << link->GetName()
+        << " axis=" << axis << " radians=" << radians << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  command_sub = node->Subscribe(topic + "/control", &Gyro::Callback, this);
+  pos_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/position");
+  vel_pub = node->Advertise<gazebo::msgs::Float64>(topic + "/velocity");
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Gyro::Update, this, _1));
+}
+
+void Gyro::Update(const gazebo::common::UpdateInfo& info) {
+  gazebo::msgs::Float64 pos_msg, vel_msg;
+  pos_msg.set_data(Limit(GetAngle() - zero));
+  pos_pub->Publish(pos_msg);
+  vel_msg.set_data(GetVelocity());
+  vel_pub->Publish(vel_msg);
+}
+
+void Gyro::Callback(const gazebo::msgs::ConstStringPtr& msg) {
+  std::string command = msg->data();
+  if (command == "reset") {
+    zero = GetAngle();
+  } else {
+    gzerr << "WARNING: Gyro got unknown command '" << command << "'."
+          << std::endl;
+  }
+}
+
+double Gyro::GetAngle() {
+  if (radians) {
+    return link->GetWorldCoGPose().rot.GetAsEuler()[axis];
+  } else {
+    return link->GetWorldCoGPose().rot.GetAsEuler()[axis] * (180.0 / M_PI);
+  }
+}
+
+double Gyro::GetVelocity() {
+  if (radians) {
+    return link->GetRelativeAngularVel()[axis];
+  } else {
+    return link->GetRelativeAngularVel()[axis] * (180.0 / M_PI);
+  }
+}
+
+double Gyro::Limit(double value) {
+  if (radians) {
+    while (true) {
+      if (value < -M_PI)
+        value += 2 * M_PI;
+      else if (value > M_PI)
+        value -= 2 * M_PI;
+      else
+        break;
+    }
+  } else {
+    while (true) {
+      if (value < -180)
+        value += 360;
+      else if (value > 180)
+        value -= 360;
+      else
+        break;
+    }
+  }
+  return value;
+}

simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h

@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/// \brief The axis about which to measure rotation.
+typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION;
+
+/**
+ * \brief Plugin for reading the speed and relative angle of a link.
+ *
+ * This plugin publishes the angle since last reset and the speed
+ * which a link is rotating about some axis to subtopics of the given
+ * topic every physics update. There is also a control topic that
+ * takes one command: "reset", which sets the current angle as zero.
+ *
+ * To add a gyro to your robot, add the following XML to your robot
+ * model:
+ *
+ *     <plugin name="my_gyro" filename="libgyro.so">
+ *       <link>Joint Name</link>
+ *       <topic>~/my/topic</topic>
+ *       <units>{degrees, radians}</units>
+ *     </plugin>
+ *
+ * - `link`: Name of the link this potentiometer is attached to.
+ * - `topic`: Optional. Used as the root for subtopics. `topic`/position
+ * (gazebo.msgs.Float64),
+ *            `topic`/velocity (gazebo.msgs.Float64), `topic`/control
+ * (gazebo.msgs.String)
+ * - `units`; Optional, defaults to radians.
+ */
+class Gyro : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the gyro and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out the gyro reading each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Publish the angle on this topic.
+  std::string topic;
+
+  /// \brief Whether or not this gyro measures radians or degrees.
+  bool radians;
+
+  /// \brief The axis to measure rotation about.
+  ROTATION axis;
+
+  /// \brief The zero value of the gyro.
+  double zero;
+
+  /// \brief The link that this gyro measures
+  gazebo::physics::LinkPtr link;
+
+  /// \brief Callback for handling control data
+  void Callback(const gazebo::msgs::ConstStringPtr& msg);
+
+  /// \brief Gets the current angle, taking into account whether to
+  ///        return radians or degrees.
+  double GetAngle();
+
+  /// \brief Gets the current velocity, taking into account whether to
+  ///        return radians/second or degrees/second.
+  double GetVelocity();
+
+  /// \brief Limit the value to either [-180,180] or [-PI,PI]
+  ///       depending on whether or radians or degrees are being used.
+  double Limit(double value);
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Subscriber handle.
+  gazebo::transport::SubscriberPtr command_sub;
+
+  /// \brief Publisher handles.
+  gazebo::transport::PublisherPtr pos_pub, vel_pub;
+};

simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp

@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "external_limit_switch.h"
+
+#include <string>
+
+ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) {
+  sensor = std::dynamic_pointer_cast<gazebo::sensors::ContactSensor>(
+      gazebo::sensors::get_sensor(sdf->Get<std::string>("sensor")));
+
+  gzmsg << "\texternal limit switch: "
+        << " sensor=" << sensor->Name() << std::endl;
+}
+
+bool ExternalLimitSwitch::Get() {
+  return sensor->Contacts().contact().size() > 0;
+}

simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "internal_limit_switch.h"
+
+#include <string>
+
+InternalLimitSwitch::InternalLimitSwitch(gazebo::physics::ModelPtr model,
+                                         sdf::ElementPtr sdf) {
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  min = sdf->Get<double>("min");
+  max = sdf->Get<double>("max");
+
+  if (sdf->HasElement("units")) {
+    radians = sdf->Get<std::string>("units") != "degrees";
+  } else {
+    radians = true;
+  }
+
+  gzmsg << "\tinternal limit switch: "
+        << " type=" << joint->GetName() << " range=[" << min << "," << max
+        << "] radians=" << radians << std::endl;
+}
+
+bool InternalLimitSwitch::Get() {
+  double value;
+  joint->GetAngle(0).Normalize();
+  if (radians) {
+    value = joint->GetAngle(0).Radian();
+  } else {
+    value = joint->GetAngle(0).Degree();
+  }
+  return value >= min && value <= max;
+}

simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "limit_switch.h"
+
+#include <boost/algorithm/string/replace.hpp>
+
+GZ_REGISTER_MODEL_PLUGIN(LimitSwitch)
+
+void LimitSwitch::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+  invert = sdf->HasElement("invert");
+  std::string type = sdf->Get<std::string>("type");
+
+  gzmsg << "Initializing limit switch: " << topic << " type=" << type
+        << std::endl;
+  if (type == "internal") {
+    ls = new InternalLimitSwitch(model, sdf);
+  } else if (type == "external") {
+    ls = new ExternalLimitSwitch(sdf);
+  } else {
+    gzerr << "WARNING: unsupported limit switch type " << type;
+  }
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  pub = node->Advertise<gazebo::msgs::Bool>(topic);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&LimitSwitch::Update, this, _1));
+}
+
+void LimitSwitch::Update(const gazebo::common::UpdateInfo& info) {
+  gazebo::msgs::Bool msg;
+  if (invert)
+    msg.set_data(!ls->Get());
+  else
+    msg.set_data(ls->Get());
+  pub->Publish(msg);
+}

simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <boost/pointer_cast.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/sensors/sensors.hh>
+
+#ifdef _WIN32
+#include <Winsock2.h>
+#endif
+
+#include "switch.h"
+
+class ExternalLimitSwitch : public Switch {
+ public:
+  explicit ExternalLimitSwitch(sdf::ElementPtr sdf);
+
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get();
+
+ private:
+  gazebo::sensors::ContactSensorPtr sensor;
+};

simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+#ifdef _WIN32
+#include <Winsock2.h>
+#endif
+
+#include "switch.h"
+
+class InternalLimitSwitch : public Switch {
+ public:
+  InternalLimitSwitch(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get();
+
+ private:
+  gazebo::physics::JointPtr joint;
+  double min, max;
+  bool radians;
+};

simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h

@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "external_limit_switch.h"
+#include "internal_limit_switch.h"
+#include "simulation/gz_msgs/msgs.h"
+#include "switch.h"
+
+/**
+ * \brief Plugin for reading limit switches.
+ *
+ * This plugin publishes whether or not the limit switch has been
+ * triggered every physics update. There are two types of limit switches:
+ *
+ * - Internal: Measure joint limits. Triggerd if the joint is within
+ *             some range.
+ * - External: Measure interactions with the outside world. Triggerd
+ *             if some link is in collision.
+ *
+ * To add a limit swithch to your robot, add the following XML to your
+ * robot model.
+ *
+ * Internal:
+ *
+ *     <plugin name="my_limit_switch" filename="liblimit_switch.so">
+ *       <topic>~/my/topic</topic>
+ *       <type>internal</type>
+ *       <joint>Joint Name</joint>
+ *       <units>{degrees, radians}</units>
+ *       <min>Number</min>
+ *       <max>Number</max>
+ *     </plugin>
+ *
+ * External:
+ *
+ *     <plugin name="my_limit_switch" filename="liblimit_switch.so">
+ *       <topic>~/my/topic</topic>
+ *       <type>external</type>
+ *       <sensor>Sensor Name</sensor>
+ *     </plugin>
+ *
+ * Common:
+ * - `topic`: Optional. Message will be published as a gazebo.msgs.Bool.
+ *            Recommended values are of the form:
+ *              /gazebo/frc/simulator/dio/n
+ * - `type`: Required. The type of limit switch that this is
+ * - `invert`: Optional. If given, the output meaning will be inverted
+ *
+ * Internal
+ * - `joint`: Name of the joint this potentiometer is attached to.
+ * - `units`: Optional. Defaults to radians.
+ * - `min`: Minimum angle considered triggered.
+ * - `max`: Maximum angle considered triggered.
+ *
+ * External
+ * - `sensor`: Name of the contact sensor that this limit switch uses.
+ */
+class LimitSwitch : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the limit switch and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out the limit switch reading each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Publish the limit switch value on this topic.
+  std::string topic;
+
+  /// \brief LimitSwitch object, currently internal or external.
+  Switch* ls;
+
+  /// \brief Indicate if we should invert the output
+  bool invert;
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Publisher handle.
+  gazebo::transport::PublisherPtr pub;
+};

simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h

@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+class Switch {
+ public:
+  virtual ~Switch() = default;
+
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get() = 0;
+};

simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "pneumatic_piston.h"
+
+#include <boost/algorithm/string/replace.hpp>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#ifdef _WIN32
+// Ensure that Winsock2.h is included before Windows.h, which can get
+// pulled in by anybody (e.g., Boost).
+#include <Winsock2.h>
+#endif
+
+GZ_REGISTER_MODEL_PLUGIN(PneumaticPiston)
+
+void PneumaticPiston::Load(gazebo::physics::ModelPtr model,
+                           sdf::ElementPtr sdf) {
+  this->model = model;
+  forward_signal = reverse_signal = false;
+
+  // Parse SDF properties
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("reverse-topic")) {
+    reverse_topic = sdf->Get<std::string>("reverse-topic");
+  } else {
+    reverse_topic.clear();
+  }
+
+  forward_force = sdf->Get<double>("forward-force");
+  if (sdf->HasElement("reverse-force"))
+    reverse_force = -1.0 * sdf->Get<double>("reverse-force");
+
+  if (sdf->HasElement("direction") &&
+      sdf->Get<std::string>("direction") == "reversed") {
+    forward_force = -forward_force;
+    reverse_force = -reverse_force;
+  }
+
+  gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName()
+        << " forward_force=" << forward_force
+        << " reverse_force=" << reverse_force << std::endl;
+  if (!reverse_topic.empty())
+    gzmsg << "Reversing on topic " << reverse_topic << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  sub = node->Subscribe(topic, &PneumaticPiston::ForwardCallback, this);
+  if (!reverse_topic.empty())
+    sub_reverse =
+        node->Subscribe(reverse_topic, &PneumaticPiston::ReverseCallback, this);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&PneumaticPiston::Update, this, _1));
+}
+
+void PneumaticPiston::Update(const gazebo::common::UpdateInfo& info) {
+  double force = 0.0;
+  if (forward_signal) {
+    force = forward_force;
+  } else {
+    /* For DoubleSolenoids, the second signal must be present
+       for us to apply the reverse force.  For SingleSolenoids,
+       the lack of the forward signal suffices.
+       Note that a true simulation would not allow a SingleSolenoid to
+       have reverse force, but we put that in the hands of the model builder.*/
+    if (reverse_topic.empty() || reverse_signal) force = reverse_force;
+  }
+  joint->SetForce(0, force);
+}
+
+void PneumaticPiston::ForwardCallback(const gazebo::msgs::ConstBoolPtr& msg) {
+  forward_signal = msg->data();
+}
+
+void PneumaticPiston::ReverseCallback(const gazebo::msgs::ConstBoolPtr& msg) {
+  reverse_signal = msg->data();
+}

simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for controlling a joint with a pneumatic piston.
+ *
+ * This plugin subscribes to topics to get the solenoid state for a piston
+ *    It needs one signal for the forward signal.  For a double solenoid,
+ *    a second signal can be sent.
+ *    Each signal is a boolean.
+ *
+ * Every physics update the joint's torque is set to reflect the
+ * signal, using the configured force
+ *
+ * To add a pneumatic piston to your robot, add the following XML to
+ * your robot model:
+ *
+ *     <plugin name="my_piston" filename="libpneumatic_piston.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>/gazebo/frc/simulator/pneumatics/1/1</topic>
+ *       <reverse-topic>/gazebo/frc/simulator/pneumatics/1/2</reverse-topic>
+ *       <direction>{forward, reversed}</direction>
+ *       <forward-force>Number</forward-force>
+ *       <reverse-force>Number</reverse-force>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this piston is attached to.
+ * - `topic`: Optional. Forward Solenoid signal name. type gazebo.msgs.Bool.
+ *            If not given, the name given for the plugin will be used.
+ *            A pattern of /gazebo/frc/simulator/pneumatics/1/n is good.
+ *            The first number represents the PCM module.  Only 1 is supported.
+ *            The second number represents the channel on the PCM.
+ * - `topic-reverse`: Optional. If given, represents the reverse channel.
+ *            Message type should be gazebo.msgs.Bool.
+ * - `direction`: Optional. Defaults to forward. Reversed if the piston
+ *                pushes in the opposite direction of the joint axis.
+ * - `forward-force`: Force to apply in the forward direction.
+ * - `reverse-force`: Force to apply in the reverse direction.
+ *                    For a single solenoid, you would expect '0',
+ *                    but we allow model builders to provide a value.
+ *
+ */
+class PneumaticPiston : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the pneumatic piston and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Updat the force the piston applies on the joint.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Topic to read forward control signal from.
+  std::string topic;
+
+  /// \brief Topic to read reverse control signal from.
+  std::string reverse_topic;
+
+  /// \brief Whether the solenoid to open forward is on
+  bool forward_signal;
+
+  /// \brief Whether the solenoid to open in reverse is on
+  bool reverse_signal;
+
+  /// \brief The magic force multipliers for each direction.
+  double forward_force, reverse_force;
+
+  /// \brief The joint that this pneumatic piston actuates.
+  gazebo::physics::JointPtr joint;
+
+  /// \brief Callback for receiving msgs and updating the solenoid state
+  void ForwardCallback(const gazebo::msgs::ConstBoolPtr& msg);
+
+  /// \brief Callback for receiving msgs and updating the reverse solenoid state
+  void ReverseCallback(const gazebo::msgs::ConstBoolPtr& msg);
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Subscriber handle.
+  gazebo::transport::SubscriberPtr sub;
+
+  /// \brief Subscriber handle for reverse topic
+  gazebo::transport::SubscriberPtr sub_reverse;
+};

simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "potentiometer.h"
+
+#include <boost/algorithm/string/replace.hpp>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#ifdef _WIN32
+// Ensure that Winsock2.h is included before Windows.h, which can get
+// pulled in by anybody (e.g., Boost).
+#include <Winsock2.h>
+#endif
+
+GZ_REGISTER_MODEL_PLUGIN(Potentiometer)
+
+void Potentiometer::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("units")) {
+    radians = sdf->Get<std::string>("units") != "degrees";
+  } else {
+    radians = true;
+  }
+
+  multiplier = 1.0;
+  if (sdf->HasElement("multiplier"))
+    multiplier = sdf->Get<double>("multiplier");
+
+  gzmsg << "Initializing potentiometer: " << topic
+        << " joint=" << joint->GetName() << " radians=" << radians
+        << " multiplier=" << multiplier << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  pub = node->Advertise<gazebo::msgs::Float64>(topic);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Potentiometer::Update, this, _1));
+}
+
+void Potentiometer::Update(const gazebo::common::UpdateInfo& info) {
+  joint->GetAngle(0).Normalize();
+  gazebo::msgs::Float64 msg;
+  if (radians) {
+    msg.set_data(joint->GetAngle(0).Radian() * multiplier);
+  } else {
+    msg.set_data(joint->GetAngle(0).Degree() * multiplier);
+  }
+  pub->Publish(msg);
+}

simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for reading the angle of a joint.
+ *
+ * This plugin publishes the angle of a joint to the topic every
+ * physics update. Supports reading in either radians or degrees.
+ *
+ * To add a potentiometer to your robot, add the following XML to your
+ * robot model:
+ *
+ *     <plugin name="my_pot" filename="libpotentiometer.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <units>{degrees, radians}</units>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this potentiometer is attached to.
+ * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
+ *            The default topic name will be the plugin name.
+ *            Recommended topic names are of the form:
+ *              /gazebo/frc/simulator/analog/n
+ *            where n is the analog channel for the pot.
+ * - `multiplier`: Optional.  Amount to multiply output with.
+ *            Useful when a rotary pot returns something other than an angle
+ * - `units`: Optional. Defaults to radians.
+ */
+class Potentiometer : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the potentiometer and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out the potentiometer reading each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Publish the angle on this topic.
+  std::string topic;
+
+  /// \brief Whether or not this potentiometer measures radians or degrees.
+  bool radians;
+
+  /// \brief A scaling factor to apply to this potentiometer.
+  double multiplier;
+
+  /// \brief The joint that this potentiometer measures
+  gazebo::physics::JointPtr joint;
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Publisher handle.
+  gazebo::transport::PublisherPtr pub;
+};

simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "rangefinder.h"
+
+#include <boost/algorithm/string/replace.hpp>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <gazebo/transport/transport.hh>
+
+#ifdef _WIN32
+// Ensure that Winsock2.h is included before Windows.h, which can get
+// pulled in by anybody (e.g., Boost).
+#include <Winsock2.h>
+#endif
+
+GZ_REGISTER_MODEL_PLUGIN(Rangefinder)
+
+void Rangefinder::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+
+  // Parse SDF properties
+  sensor = std::dynamic_pointer_cast<gazebo::sensors::SonarSensor>(
+      gazebo::sensors::get_sensor(sdf->Get<std::string>("sensor")));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  gzmsg << "Initializing rangefinder: " << topic << " sensor=" << sensor->Name()
+        << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  pub = node->Advertise<gazebo::msgs::Float64>(topic);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Rangefinder::Update, this, _1));
+}
+
+void Rangefinder::Update(const gazebo::common::UpdateInfo& info) {
+  gazebo::msgs::Float64 msg;
+  msg.set_data(sensor->Range());
+  pub->Publish(msg);
+}

simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for reading the range of obstacles.
+ *
+ * This plugin publishes the range of obstacles detected by a sonar
+ * rangefinder every physics update.
+ *
+ * To add a rangefinder to your robot, add the following XML to your
+ * robot model:
+ *
+ *     <plugin name="my_rangefinder" filename="librangefinder.so">
+ *       <sensor>Sensor Name</sensor>
+ *       <topic>~/my/topic</topic>
+ *     </plugin>
+ *
+ * - `sensor`: Name of the sonar sensor that this rangefinder uses.
+ * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
+ */
+class Rangefinder : public gazebo::ModelPlugin {
+ public:
+  /// \brief Load the rangefinder and configures it according to the sdf.
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Sends out the rangefinder reading each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Publish the range on this topic.
+  std::string topic;
+
+  /// \brief The sonar sensor that this rangefinder uses
+  gazebo::sensors::SonarSensorPtr sensor;
+
+  /// \brief The model to which this is attached.
+  gazebo::physics::ModelPtr model;
+
+  /// \brief Pointer to the world update function.
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising.
+  gazebo::transport::NodePtr node;
+
+  /// \brief Publisher handle.
+  gazebo::transport::PublisherPtr pub;
+};

simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "servo.h"
+
+#include <boost/algorithm/string/replace.hpp>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#ifdef _WIN32
+// Ensure that Winsock2.h is included before Windows.h, which can get
+// pulled in by anybody (e.g., Boost).
+#include <Winsock2.h>
+#endif
+
+GZ_REGISTER_MODEL_PLUGIN(Servo)
+
+void Servo::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) {
+  this->model = model;
+  signal = 0;
+
+  // parse SDF Properries
+  joint = model->GetJoint(sdf->Get<std::string>("joint"));
+  if (sdf->HasElement("topic")) {
+    topic = sdf->Get<std::string>("topic");
+  } else {
+    topic = "~/" + sdf->GetAttribute("name")->GetAsString();
+  }
+
+  if (sdf->HasElement("torque")) {
+    torque = sdf->Get<double>("torque");
+  } else {
+    torque = 5;
+  }
+
+  gzmsg << "initializing servo: " << topic << " joint=" << joint->GetName()
+        << " torque=" << torque << std::endl;
+
+  // Connect to Gazebo transport for messaging
+  std::string scoped_name =
+      model->GetWorld()->GetName() + "::" + model->GetScopedName();
+  boost::replace_all(scoped_name, "::", "/");
+  node = gazebo::transport::NodePtr(new gazebo::transport::Node());
+  node->Init(scoped_name);
+  sub = node->Subscribe(topic, &Servo::Callback, this);
+
+  // connect to the world update event
+  // this will call update every iteration
+  updateConn = gazebo::event::Events::ConnectWorldUpdateBegin(
+      boost::bind(&Servo::Update, this, _1));
+}
+
+void Servo::Update(const gazebo::common::UpdateInfo& info) {
+  // torque is in kg*cm
+  // joint->SetAngle(0,signal*180);
+  if (joint->GetAngle(0) < signal) {
+    joint->SetForce(0, torque);
+  } else if (joint->GetAngle(0) > signal) {
+    joint->SetForce(0, torque);
+  }
+  joint->SetForce(0, 0);
+}
+
+void Servo::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  signal = msg->data();
+  if (signal < -1) {
+    signal = -1;
+  } else if (signal > 1) {
+    signal = 1;
+  }
+}

simulation/frc_gazebo_plugins/src/servo/headers/servo.h

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <gazebo/gazebo.hh>
+
+#include "simulation/gz_msgs/msgs.h"
+
+/**
+ * \brief Plugin for controlling a servo.
+ *
+ * This plugin subscribes to a topic to get a signal in the range
+ * [-1,1]. Every physics update the joint's torque is set as
+ * multiplier*signal.
+ *
+ * To add a servo to your robot, add the following XML to your robot
+ * model:
+ *
+ *     <plugin name="my_servo" filename="libservo.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>/gzebo/frc/simulator/pwm/1</topic>
+ *       <zero_position>0</zero_position>
+ *     </plugin>
+ *
+ * - `link`: Name of the link the servo is attached to.
+ * - `topic`: Optional. Message type should be gazebo.msgs.Float64.
+ */
+class Servo : public gazebo::ModelPlugin {
+ public:
+  /// \brief load the servo and configure it according to the sdf
+  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
+
+  /// \brief Update the torque on the joint from the dc motor each timestep.
+  void Update(const gazebo::common::UpdateInfo& info);
+
+ private:
+  /// \brief Topic to read control signal from.
+  std::string topic;
+
+  /// \brief the pwm signal limited to the range [-1,1]
+  double signal;
+
+  /// \brief the torque of the motor in kg/cm
+  double torque;
+
+  /// \brief the joint that this servo moves
+  gazebo::physics::JointPtr joint;
+
+  /// \brief Callback for receiving msgs and storing the signal
+  void Callback(const gazebo::msgs::ConstFloat64Ptr& msg);
+
+  /// \brief The model to which this is attached
+  gazebo::physics::ModelPtr model;
+
+  /// \brief The pointer to the world update function
+  gazebo::event::ConnectionPtr updateConn;
+
+  /// \brief The node on which we're advertising torque
+  gazebo::transport::NodePtr node;
+
+  /// \brief The subscriber for the PWM signal
+  gazebo::transport::SubscriberPtr sub;
+};