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

@@ -0,0 +1,55 @@
+#include "pneumatic_piston.h"
+
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+GZ_REGISTER_MODEL_PLUGIN(PneumaticPiston)
+
+PneumaticPiston::PneumaticPiston() {}
+
+PneumaticPiston::~PneumaticPiston() {}
+
+void PneumaticPiston::Load(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();
+  }
+
+  forward_force = sdf->Get<double>("forward-force");
+  reverse_force = -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;
+
+  // 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);
+  sub = node->Subscribe(topic, &PneumaticPiston::Callback, this);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1));
+}
+
+void PneumaticPiston::Update(const common::UpdateInfo &info) {
+  joint->SetForce(0, signal);
+}
+
+void PneumaticPiston::Callback(const msgs::ConstFloat64Ptr &msg) {
+  if (msg->data() < -0.001) { signal = -reverse_force; }
+  else if (msg->data() > 0.001) { signal = forward_force; }
+}

simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "msgs/msgs.h"
+
+using namespace gazebo;
+
+/**
+ * \brief Plugin for controlling a joint with a pneumatic piston.
+ *
+ * This plugin subscribes to a topic to get a signal. It accepts three
+ * values:
+ *
+ * -  1: Apply the forward force to the joint.
+ * -  0: Maintain last applied force
+ * - -1: Apply the reverse force to the joint.
+ *
+ * Every physics update the joint's torque is set to reflect the
+ * signal.
+ *
+ * To add a pneumatic piston to your robot, add the following XML to
+ * your robot model:
+ *
+ *     <plugin name="my_piston" filename="libgz_pneumatic_piston.so">
+ *       <joint>Joint Name</joint>
+ *       <topic>~/my/topic</topic>
+ *       <direction>{forward, reversed}</direction>
+ *       <forward-force>Number</forward-force>
+ *       <reverse-force>Number</reverse-force>
+ *     </plugin>
+ *
+ * - `joint`: Name of the joint this Dc motor is attached to.
+ * - `topic`: Optional. Message type should be gazebo.msgs.Float64.
+ * - `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.
+ *
+ * \todo Signal should probably be made a tri-state message.
+ */
+class PneumaticPiston: public ModelPlugin {
+public: 
+  PneumaticPiston();
+  ~PneumaticPiston();
+  
+  /// \brief Load the pneumatic piston and configures it according to the sdf.
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  
+  /// \brief Updat the force the piston applies on the joint.
+  void Update(const common::UpdateInfo &info);
+
+private:
+  /// \brief Topic to read control signal from.
+  std::string topic;
+
+  /// \brief The signal is one of: {-1,0,1}.
+  double signal;
+  
+  /// \brief The magic force multipliers for each direction.
+  double forward_force, reverse_force;
+
+  /// \brief The joint that this pneumatic piston actuates.
+  physics::JointPtr joint;
+
+  /// \brief Callback for receiving msgs and updating the torque.
+  void Callback(const msgs::ConstFloat64Ptr &msg);
+
+  
+  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 sub;    ///< \brief Subscriber handle.
+};