@SuppressWarnings("JavadocMethod")
public SimEncoder(String topic) {
m_commandPub = MainNode.advertise(topic + "/control", Msgs.String());
MainNode.subscribe(
topic + "/position",
Msgs.Float64(),
new SubscriberCallback<Float64>() {
@Override
public void callback(Float64 msg) {
m_position = msg.getData();
}
});
MainNode.subscribe(
topic + "/velocity",
Msgs.Float64(),
new SubscriberCallback<Float64>() {
@Override
public void callback(Float64 msg) {
m_velocity = msg.getData();
}
});
try {
if (m_commandPub.waitForConnection(5000)) { // Wait up to five seconds.
System.out.println("Initialized " + topic);
} else {
System.err.println("Failed to initialize " + topic + ": does the encoder exist?");
}
} catch (InterruptedException ex) {
ex.printStackTrace(); // TODO: Better way to handle this?
Thread.currentThread().interrupt();
}
}
private void sendCommand(String cmd) {
m_commandPub.publish(Msgs.String(cmd));
}