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

@@ -0,0 +1,17 @@
+#include "external_limit_switch.h"
+
+#include <gazebo/sensors/sensors.hh>
+#include <boost/pointer_cast.hpp>
+
+ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) {
+  sensor = boost::dynamic_pointer_cast<sensors::ContactSensor>(
+               sensors::get_sensor(sdf->Get<std::string>("sensor")));
+
+  gzmsg << "\texternal limit switch: " << " sensor=" << sensor->GetName() << std::endl;
+}
+
+ExternalLimitSwitch::~ExternalLimitSwitch() {}
+  
+bool ExternalLimitSwitch::Get() {
+  return sensor->GetContacts().contact().size() > 0;
+}

simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "switch.h"
+
+using namespace gazebo;
+
+class ExternalLimitSwitch : public Switch {
+public: 
+  ExternalLimitSwitch(sdf::ElementPtr sdf);
+  ~ExternalLimitSwitch();
+  
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get();
+
+private:
+  sensors::ContactSensorPtr sensor;
+};

simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp

@@ -0,0 +1,31 @@
+#include "internal_limit_switch.h"
+
+#include <gazebo/physics/physics.hh>
+
+InternalLimitSwitch::InternalLimitSwitch(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;
+}
+
+InternalLimitSwitch::~InternalLimitSwitch() {}
+  
+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/limit_switch/src/internal_limit_switch.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "switch.h"
+
+using namespace gazebo;
+
+class InternalLimitSwitch : public Switch {
+public: 
+  InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf);
+  ~InternalLimitSwitch();
+  
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get();
+
+private:
+  physics::JointPtr joint;
+  double min, max;
+  bool radians;
+};

simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp

@@ -0,0 +1,52 @@
+#include "limit_switch.h"
+#include "internal_limit_switch.h"
+#include "external_limit_switch.h"
+
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "msgs/msgs.h"
+
+GZ_REGISTER_MODEL_PLUGIN(LimitSwitch)
+
+LimitSwitch::LimitSwitch() {}
+
+LimitSwitch::~LimitSwitch() {}
+
+void LimitSwitch::Load(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();
+  }
+  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 = transport::NodePtr(new transport::Node());
+  node->Init(scoped_name);
+  pub = node->Advertise<msgs::Bool>(topic);
+
+  // Connect to the world update event.
+  // This will trigger the Update function every Gazebo iteration
+  updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&LimitSwitch::Update, this, _1));
+}
+
+void LimitSwitch::Update(const common::UpdateInfo &info) {
+  msgs::Bool msg;
+  msg.set_data(ls->Get());
+  pub->Publish(msg);
+}

simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <gazebo/gazebo.hh>
+
+#include "switch.h"
+
+using namespace gazebo;
+
+/**
+ * \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="libgz_limit_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="libgz_limit_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.Float64.
+ * - `type`: Required. The type of limit switch that this is
+ *
+ * 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 ModelPlugin {
+public: 
+  LimitSwitch();
+  ~LimitSwitch();
+  
+  /// \brief Load the limit switch and configures it according to the sdf.
+  void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
+  
+  /// \brief Sends out the limit switch reading each timestep.
+  void Update(const 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;
+  
+
+  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::PublisherPtr pub;     ///< \brief Publisher handle.
+};

simulation/frc_gazebo_plugins/limit_switch/src/switch.h

@@ -0,0 +1,9 @@
+#pragma once
+
+class Switch {
+public: 
+  virtual ~Switch() {}
+  
+  /// \brief Returns true when the switch is triggered.
+  virtual bool Get() = 0;
+};