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

@@ -0,0 +1,53 @@
+#include "dc_motor.h"
+
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+GZ_REGISTER_MODEL_PLUGIN(DCMotor)
+
+DCMotor::DCMotor() {}
+
+DCMotor::~DCMotor() {}
+
+void DCMotor::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();
+  }
+  
+  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 = transport::NodePtr(new 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 = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1));
+}
+
+void DCMotor::Update(const common::UpdateInfo &info) {
+  joint->SetForce(0, signal*multiplier);
+}
+
+void DCMotor::Callback(const msgs::ConstFloat64Ptr &msg) {
+  signal = msg->data();
+  if (signal < -1) { signal = -1; }
+  else if (signal > 1) { signal = 1; }
+}

simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "msgs/msgs.h"
+
+using namespace gazebo;
+
+/**
+ * \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="libgz_dc_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 ModelPlugin {
+public: 
+  DCMotor();
+  ~DCMotor();
+  
+  /// \brief Load the dc motor and configures it according to the sdf.
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  
+  /// \brief Update the torque on the joint from the dc motor each timestep.
+  void Update(const 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.
+  physics::JointPtr joint;
+
+  /// \brief Callback for receiving msgs and storing the signal.
+  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.
+};