Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added support for PacGoat robot for artf2591.

Browse Source 
This also includes support for solenoids (artf2592) in the gazebo plugin and WPILibJavaSim, fixes a concurrency issue with JavaGazebo.

Change-Id: I5bd19556a7511387852c98414e4a29fdfd68b8cd
This commit is contained in:
Alex Henning
2014-06-23 14:43:45 -07:00
parent 0f8c83f693
commit 40628a817d
34 changed files with 1807 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Node.java
View File

@@ -57,9 +57,10 @@ public class Node implements Runnable, ServerCallback {
initializeConnection();
new Thread(this).start();
LOG.info("Serving on: "+server.host+":"+server.port);
}
public <T extends Message> Publisher<T> advertise(String topic, T defaultMessage) {
public synchronized <T extends Message> Publisher<T> advertise(String topic, T defaultMessage) {
topic = fixTopic(topic);
LOG.info("ADV "+topic);
String type = defaultMessage.getDescriptorForType().getFullName();
@@ -76,7 +77,7 @@ public class Node implements Runnable, ServerCallback {
return pub;
}
public <T extends Message> Subscriber<T>
public synchronized <T extends Message> Subscriber<T>
subscribe(String topic, T defaultMessage, SubscriberCallback<T> cb) {
topic = fixTopic(topic);
LOG.info("SUB "+topic);
@@ -122,7 +123,7 @@ public class Node implements Runnable, ServerCallback {
}
}
private void initializeConnection() throws IOException {
private synchronized void initializeConnection() throws IOException {
Packet initData = master.read();
if (!initData.getType().equals("version_init")) {
throw new IOException("Expected 'version_init' packet, got '"+initData.getType()+"'.");
@@ -148,7 +149,7 @@ public class Node implements Runnable, ServerCallback {
}
}
public void processPacket(Packet packet) throws InvalidProtocolBufferException {
public synchronized void processPacket(Packet packet) throws InvalidProtocolBufferException {
if (packet.getType().equals("publisher_add")) {
PublisherRecord pub = new RemotePublisherRecord(Publish.parseFrom(packet.getSerializedData()));
@@ -158,6 +159,7 @@ public class Node implements Runnable, ServerCallback {
}
LOG.info("New Publisher: "+pub.getTopic());
LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
publishers.put(pub.getTopic(), pub);
} else if (packet.getType().equals("publisher_subscribe") ||
packet.getType().equals("publisher_advertise")) {
@@ -169,6 +171,7 @@ public class Node implements Runnable, ServerCallback {
}
LOG.info("PUBSUB found for "+pub.getTopic());
LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
subscriptions.get(pub.getTopic()).connect(pub);
} else if (packet.getType().equals("topic_namespace_add")) {
namespaces.add(GzString.String.parseFrom(packet.getSerializedData()).getData());
@@ -183,9 +186,10 @@ public class Node implements Runnable, ServerCallback {
@Override
public void handle(Connection conn) throws IOException {
LOG.fine("Handling new connection");
Packet msg = conn.read();
if (msg == null) {
LOG.severe("Read null message.");
LOG.warning("Read null message.");
return;
}
@@ -196,6 +200,7 @@ public class Node implements Runnable, ServerCallback {
+ sub.getTopic());
return;
}
LOG.fine("New connection for topic="+sub.getTopic());
PublisherRecord pub = publishers.get(sub.getTopic());
if (!pub.getMsgType().equals(sub.getMsgType())) {
@@ -204,7 +209,7 @@ public class Node implements Runnable, ServerCallback {
return;
}
LOG.info("PUB " + sub.getTopic());
LOG.info("CONN " + sub.getTopic());
pub.connect(conn);
} else {
LOG.warning("Unknown message type: " + msg.getType());

25
simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java
View File

@@ -3,6 +3,7 @@ package org.gazebosim.transport;
import java.io.IOException;
import java.util.LinkedList;
import java.util.List;
import java.util.logging.Logger;
import com.google.protobuf.Message;
@@ -13,7 +14,9 @@ public class Publisher<T extends Message> implements PublisherRecord {
private List<Connection> listeners;
private boolean latching = false;
private T lastMsg = null;
private static final Logger LOG = Logger.getLogger("Gazebo Transport");
public Publisher(String topic, String msgType, String localHost, int localPort) {
this.topic = topic;
this.msgType = msgType;
@@ -24,16 +27,21 @@ public class Publisher<T extends Message> implements PublisherRecord {
public synchronized void publish(T msg) {
lastMsg = msg;
List<Connection> toRemove = new LinkedList<>();
for (Connection listener : listeners) {
try {
listener.write(msg);
} catch (IOException e) {
try {
listener.close();
} catch (IOException e1) { /* Closing failed, probably not a big deal. */}
listeners.remove(listener);
toRemove.add(listener);
}
}
for (Connection listener : toRemove) {
LOG.info("Removing listener from topic="+topic);
try {
listener.close();
} catch (IOException e1) { /* Closing failed, probably not a big deal. */}
listeners.remove(listener);
}
}
@Override
@@ -58,11 +66,16 @@ public class Publisher<T extends Message> implements PublisherRecord {
@Override
public synchronized void connect(Connection conn) {
LOG.fine("Handling subscriber connection for topic: "+topic);
if (latching && lastMsg != null) {
try {
conn.write(lastMsg);
} catch (IOException e) {
return; // TODO: Log
LOG.warning("Writing latched message failed on topic="+topic);
try {
conn.close();
} catch (IOException e1) { /* Closing failed, probably not a big deal. */}
return;
}
}
listeners.add(conn);

2
simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Subscriber.java
View File

@@ -66,7 +66,7 @@ public class Subscriber<T extends Message> {
}
} catch (IOException e) {
// FIXME: Connection lost, let's make sure it's closed and complain.
// Hopefully they the reconnect, maybe we should try to recover better?
// Hopefully they try to reconnect, maybe we should try to recover better?
try {
conn.close();
} catch (IOException e1) {