Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +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

4
eclipse-plugins/edu.wpi.first.wpilib.plugins.java/src/main/resources/java-zip/ant/build.xml
View File

@@ -123,14 +123,12 @@
</exec>
</sequential>
<sequential>
<sleep seconds="5"/>
<echo>[simulate] Running DriverStation.</echo>
<java jar="${wpilib.sim.tools}/SimDS.jar" fork="true">
<jvmarg value="-Djava.library.path=${wpilib.sim.lib}" />
</java>
</sequential>
<sequential>
<sleep seconds="5"/>
<echo>[simulate] Running Code.</echo>
<java jar="${simulation.dist.jar}" fork="true">
<jvmarg value="-Djava.library.path=${wpilib.sim.lib}" />
@@ -148,14 +146,12 @@
</exec>
</sequential>
<sequential>
<sleep seconds="5"/>
<echo>[debug-simulate] Running DriverStation.</echo>
<java jar="${wpilib.sim.tools}/SimDS.jar" fork="true">
<jvmarg value="-Djava.library.path=${wpilib.sim.lib}" />
</java>
</sequential>
<sequential>
<sleep seconds="5"/>
<echo>[debug-simulate] Running Code.</echo>
<java jar="${simulation.dist.jar}" fork="true">
<jvmarg value="-Xdebug" />

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) {

22
simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java
View File

@@ -3,13 +3,21 @@ package edu.wpi.first.wpilibj.simulation.ds;
import org.gazebosim.transport.Node;
public class Main {
public static void main(String args[]) {
Node node = new Node("frc");
JoystickProvider provider = new JoystickProvider();
@SuppressWarnings("unused")
public static void main(String args[]) {
Node node = new Node("frc");
try {
node.waitForConnection();
} catch (Throwable thr) {
System.err.println("Could not connect to Gazebo.");
thr.printStackTrace();
System.exit(1);
return;
}
JoystickProvider provider = new JoystickProvider();
DS ds = new DS(provider);
ds.advertise(node);
ds.advertise(node);
while (true) {
ds.publish();
for (int i = 0; i < provider.getJoysticks().size(); i++) {
@@ -25,5 +33,5 @@ public class Main {
e.printStackTrace();
}
}
}
}
}

13
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
View File

@@ -12,6 +12,8 @@ import java.net.URL;
import java.util.Enumeration;
import java.util.jar.Manifest;
import edu.wpi.first.wpilibj.simulation.MainNode;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
/**
@@ -175,9 +177,18 @@ public abstract class RobotBase {
* the robot.
* @throws javax.microedition.midlet.MIDletStateChangeException
*/
public static void main(String args[]) { // TODO: expose main to teams?{
public static void main(String args[]) { // TODO: expose main to teams?
boolean errorOnExit = false;
try {
MainNode.openGazeboConnection();
} catch (Throwable e) {
System.err.println("Could not connect to Gazebo.");
e.printStackTrace();
System.exit(1);
return;
}
ds = DriverStation.getInstance();
String robotName = "";

48
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/MainNode.java
View File

@@ -1,5 +1,8 @@
package edu.wpi.first.wpilibj.simulation;
import java.io.IOException;
import java.util.logging.Logger;
import org.gazebosim.transport.Node;
import org.gazebosim.transport.Publisher;
import org.gazebosim.transport.Subscriber;
@@ -7,27 +10,34 @@ import org.gazebosim.transport.SubscriberCallback;
import com.google.protobuf.Message;
public class MainNode{
private Node main;
public class MainNode {
private MainNode() {
main = new Node("frc");
}
private static MainNode instance;
public static MainNode getInstance() {
if (instance == null) {
instance = new MainNode();
}
return instance;
}
public static <T extends Message> Publisher<T> advertise(String topic, T defaultMessage) {
return getInstance().main.advertise(topic, defaultMessage);
}
public static <T extends Message> Subscriber<T>
subscribe(String topic, T defaultMessage, SubscriberCallback<T> cb) {
return getInstance().main.subscribe(topic, defaultMessage, cb);
private static final Logger LOG = Logger.getLogger("Simulation MainNode");
private static Node mainNode;
public static synchronized void openGazeboConnection() throws IOException, InterruptedException {
if (mainNode != null) {
LOG.warning("MainNode.openGazeboConnection() was already called!");
return;
}
mainNode = new Node("frc");
mainNode.waitForConnection();
}
public static <T extends Message> Publisher<T> advertise(String topic, T defaultMessage) {
if (mainNode == null) {
throw new IllegalStateException("MainNode.openGazeboConnection() should have already been called by RobotBase.main()!");
}
return mainNode.advertise(topic, defaultMessage);
}
public static <T extends Message> Subscriber<T> subscribe(String topic, T defaultMessage, SubscriberCallback<T> cb) {
if (mainNode == null) {
throw new IllegalStateException("MainNode.openGazeboConnection() should have already been called by RobotBase.main()!");
}
return mainNode.subscribe(topic, defaultMessage, cb);
}
}