Removed modules from the simulation infrastructure and refactored FRCPlugin.

Pneumatics still have CAN modules. The refactored code is now eight
plugins for sensors and actuators. There is some code reuse that should
be refactored out, but that level of abstraction will wait until we
figure out how these plugins are integrating with gazebo proper.

Change-Id: I357e695ef05af6dda83a39ba60380686bd57d11a
Closes: artf2610, artf2623

simulation/frc_gazebo_plugins/gyro/src/gyro.cpp

@@ -0,0 +1,100 @@
+#include "gyro.h"
+
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+GZ_REGISTER_MODEL_PLUGIN(Gyro)
+
+Gyro::Gyro() {}
+
+Gyro::~Gyro() {}
+
+void Gyro::Load(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 = transport::NodePtr(new transport::Node());
+  node->Init(scoped_name);
+  command_sub = node->Subscribe(topic+"/control", &Gyro::Callback, this);
+  pos_pub = node->Advertise<msgs::Float64>(topic+"/position");
+  vel_pub = node->Advertise<msgs::Float64>(topic+"/velocity");
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::Update, this, _1));
+}
+
+void Gyro::Update(const common::UpdateInfo &info) {
+  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 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/gyro/src/gyro.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "msgs/msgs.h"
+
+using namespace gazebo;
+
+/// \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="libgz_gyro.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 ModelPlugin {
+public: 
+  Gyro();
+  ~Gyro();
+  
+  /// \brief Load the gyro and configures it according to the sdf.
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  
+  /// \brief Sends out the gyro reading each timestep.
+  void Update(const 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
+  physics::LinkPtr link;
+  
+  /// \brief Callback for handling control data
+  void Callback(const 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);
+
+  physics::ModelPtr model;                  ///< \brief The model that this is attached to. 
+  event::ConnectionPtr updateConn;          ///< \brief Pointer to the world update function.
+  transport::NodePtr node;                  ///< \brief The node we're advertising on.
+  transport::SubscriberPtr command_sub;     ///< \brief Subscriber handle.
+  transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles.
+};