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

Fixed FRCSim artf2594 - JavaGazebo no longer crashes if gzserver hasn't started, and cleaned up some code in the area.

Browse Source 
Change-Id: I4daae199fb6dda6561c2cb85fc5254e36bcb3066
This commit is contained in:
Colby Skeggs
2014-06-20 10:18:42 -07:00
parent e9ade472e4
commit 698f38d404
6 changed files with 107 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Connection.java
View File

@@ -4,12 +4,14 @@ import gazebo.msgs.GzPacket.Packet;
import gazebo.msgs.GzTime.Time;
import java.io.IOException;
import java.net.ConnectException;
import java.io.InputStream;
import java.io.OutputStream;
import java.net.ServerSocket;
import java.net.Socket;
import java.net.UnknownHostException;
import java.util.logging.Logger;
import java.util.logging.Level;
import com.google.protobuf.ByteString;
import com.google.protobuf.Message;
@@ -34,11 +36,7 @@ public class Connection {
private OutputStream os;
private static final Logger LOG = Logger.getLogger("Gazebo Transport");
public Connection() {
}
public void connect(String host, int port) throws UnknownHostException, IOException {
this.host = host;
this.port = port;
@@ -47,13 +45,31 @@ public class Connection {
os = socket.getOutputStream();
}
public void connectAndWait(String host, int port) throws IOException, InterruptedException {
this.host = host;
this.port = port;
while (true) {
try {
socket = new Socket(host, port);
break;
} catch (ConnectException ex) {
// Retry.
LOG.log(Level.WARNING, "Cannot connect, retrying in five seconds.", ex);
Thread.sleep(5000);
}
}
is = socket.getInputStream();
os = socket.getOutputStream();
}
public void serve(final ServerCallback cb) throws IOException {
ssocket = new ServerSocket(0);
host = ssocket.getInetAddress().getHostAddress(); // TODO: get globally addressable name.
port = ssocket.getLocalPort();
new Thread(new Runnable() {
@Override public void run() {
new Thread("Gazebo Server Thread") {
@Override
public void run() {
LOG.config("Listening on "+host+":"+port);
while (true) {
Connection conn = new Connection();
@@ -64,12 +80,11 @@ public class Connection {
LOG.info("Handling connect from "+conn.socket.getInetAddress());
cb.handle(conn);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
LOG.log(Level.WARNING, "Cannot handle client", e);
}
}
}
}).start();
}.start();
}
public void close() throws IOException {

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

@@ -25,50 +25,36 @@ import com.google.protobuf.InvalidProtocolBufferException;
import com.google.protobuf.Message;
public class Node implements Runnable, ServerCallback {
private String name;
private Connection master;
private Connection server;
private List<String> namespaces;
private Map<String, PublisherRecord> publishers;
private final String name;
private final Connection master = new Connection();
private final Connection server = new Connection();
private final List<String> namespaces = new LinkedList<>();
private final Map<String, PublisherRecord> publishers = new HashMap<>();
@SuppressWarnings("rawtypes")
private Map<String, Subscriber> subscriptions;
private final Map<String, Subscriber> subscriptions = new HashMap<>();
private static final Logger LOG = Logger.getLogger("Gazebo Transport");
static {
// Get rid of the excess information
LOG.setLevel(Level.WARNING);
Handler[] handlers = LOG.getParent().getHandlers();
if (handlers[0] instanceof ConsoleHandler) {
((ConsoleHandler) handlers[0]).setFormatter(new Formatter() {
@Override
public String format(LogRecord record) {
return String.format("%s|%s: %s\n", record.getLevel(), record.getLoggerName(), record.getMessage());
}
});
}
}
public Node(String name) {
this.name = name;
namespaces = new LinkedList<>();
publishers = new HashMap<>();
subscriptions = new HashMap<>();
// Get rid of the excessive information
LOG.setLevel(Level.WARNING);
Handler[] handlers = LOG.getParent().getHandlers();
if (handlers[0] instanceof ConsoleHandler) {
((ConsoleHandler) handlers[0]).setFormatter(new Formatter() {
@Override public String format(LogRecord record) {
return String.format("%s|%s: %s\n", record.getLevel(), record.getLoggerName(), record.getMessage());
}
});
}
try {
master = new Connection();
master.connect("localhost", 11345);
server = new Connection();
server.serve(this);
initializeConnection();
} catch (SocketException e ) {
LOG.severe("Socket error: " + e);
return;
} catch (UnknownHostException e ) {
LOG.severe("Invalid Host");
return;
} catch (IOException e ) {
LOG.severe("I/O error: " + e);
LOG.severe(e.getStackTrace().toString());
return;
}
}
public void waitForConnection() throws IOException, InterruptedException {
server.serve(this);
master.connectAndWait("localhost", 11345);
initializeConnection();
new Thread(this).start();
}
@@ -120,7 +106,7 @@ public class Node implements Runnable, ServerCallback {
@Override
public void run() {
try {
try {
while (true) {
Packet packet = master.read();
if (packet == null) {