Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/wpilibc/sim/src/simulation/SimEncoder.cpp b/wpilibc/sim/src/simulation/SimEncoder.cpp
new file mode 100644
index 0000000..141a371
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimEncoder.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimEncoder.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimEncoder::SimEncoder(std::string topic) {
+ commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
+ topic + "/control");
+
+ posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
+ &SimEncoder::positionCallback, this);
+ velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
+ &SimEncoder::velocityCallback, this);
+
+ if (commandPub->WaitForConnection(
+ gazebo::common::Time(5.0))) { // Wait up to five seconds.
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+ } else {
+ std::cerr << "Failed to initialize ~/simulator/" + topic +
+ ": does the encoder exist?"
+ << std::endl;
+ }
+}
+
+void SimEncoder::Reset() { sendCommand("reset"); }
+
+void SimEncoder::Start() { sendCommand("start"); }
+
+void SimEncoder::Stop() { sendCommand("stop"); }
+
+double SimEncoder::GetPosition() { return position; }
+
+double SimEncoder::GetVelocity() { return velocity; }
+
+void SimEncoder::sendCommand(std::string cmd) {
+ gazebo::msgs::GzString msg;
+ msg.set_data(cmd);
+ commandPub->Publish(msg);
+}
+
+void SimEncoder::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ position = msg->data();
+}
+
+void SimEncoder::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ velocity = msg->data();
+}