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/encoder/src/encoder.cpp

@@ -0,0 +1,94 @@
+#include "encoder.h"
+
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+GZ_REGISTER_MODEL_PLUGIN(Encoder)
+
+Encoder::Encoder() {}
+
+Encoder::~Encoder() {}
+
+void Encoder::Load(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;
+  }
+  zero = GetAngle();
+  stopped = true;
+  stop_value = 0;
+  
+  gzmsg << "Initializing encoder: " << topic << " joint=" << joint->GetName()
+        << " 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", &Encoder::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(&Encoder::Update, this, _1));
+}
+
+void Encoder::Update(const common::UpdateInfo &info) {
+  msgs::Float64 pos_msg, vel_msg;
+  if (stopped) {
+    pos_msg.set_data(stop_value);
+    pos_pub->Publish(pos_msg);
+    vel_msg.set_data(0);
+    vel_pub->Publish(vel_msg);
+  } else {
+    pos_msg.set_data(GetAngle() - zero);
+    pos_pub->Publish(pos_msg);
+    vel_msg.set_data(GetVelocity());
+    vel_pub->Publish(vel_msg);
+  }
+}
+
+void Encoder::Callback(const 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/encoder/src/encoder.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "msgs/msgs.h"
+
+using namespace gazebo;
+
+/**
+ * \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="libgz_encoder.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <units>{degrees, radians}</units>
+ *     </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)
+ * - `units`: Optional. Defaults to radians.
+ */
+class Encoder: public ModelPlugin {
+public: 
+  Encoder();
+  ~Encoder();
+  
+  /// \brief Load the encoder and configures it according to the sdf.
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  
+  /// \brief Sends out the encoder reading each timestep.
+  void Update(const 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 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
+  physics::JointPtr joint;
+
+  /// \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();
+
+  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.
+};