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

merged from frcsim branch

Browse Source 
verified to work on real robots
adds sim eclipse plugins, fixed JavaGazebo, made wpilibC++Sim build on windows
 - Java and C++ simulation robot programs run on windows
 - simulation eclipse plugin delivers models and gazebo plugins
 - Java Gazebo now respects GAZEBO_IP variables and can work across networks
 - hal and network tables win32 hacked to work on windows
 - smart dashboard broken on windows due to network tables hacks
 - wpilibC++Sim, gz_msgs, and frcsim_gazebo_plugins build with CMake
 - removed constexpr for cross platform compatibility
 - msgs generated using .protos as a part of build process
 - some spare and unused cmake/pom files deleted
 - simulation ubuntu debians removed entirely
 - refactored CMake project flags and macros
 - updated to match non-sim C++ API
 - fixed and updated documentation
 - servo added to simulation

Change-Id: Ia702ff0f1fee10d77f543810ad88f56696443b05
This commit is contained in:
peter mitrano
2015-04-26 19:19:57 -04:00
parent 4e46692191
commit 29d029fa61
211 changed files with 2143 additions and 6491 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,6 +5,7 @@ import gazebo.msgs.GzTime.Time;
import java.io.IOException;
import java.net.ConnectException;
import java.net.InetAddress;
import java.io.InputStream;
import java.io.OutputStream;
import java.net.ServerSocket;
@@ -64,9 +65,17 @@ public class Connection {
public void serve(final ServerCallback cb) throws IOException {
ssocket = new ServerSocket(0);
host = ssocket.getInetAddress().getHostAddress(); // TODO: get globally addressable name.
host = ssocket.getInetAddress().getHostAddress();
port = ssocket.getLocalPort();
//enable user to change master uri via environment variable GAZEBO_MASTER_URI
//TODO : allow for automatic guesing of IP. Look at Connection.cc in gazebo for C++ example
String user_defined_ip = System.getenv("GAZEBO_IP");
if (user_defined_ip != null) {
host = InetAddress.getByName(user_defined_ip).getHostAddress();;
LOG.warning("Using custom host: "+host);
}
new Thread("Gazebo Server Thread") {
@Override
public void run() {

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

@@ -52,10 +52,30 @@ public class Node implements Runnable, ServerCallback {
}
public void waitForConnection() throws IOException, InterruptedException {
//enable user to change master uri via environment variable GAZEBO_MASTER_URI
String user_defined_uri = System.getenv("GAZEBO_MASTER_URI");
String gazebo_master_uri = "localhost";
int port = 11345;
if (user_defined_uri != null) {
String[] parts = user_defined_uri.split(":");
if (parts.length != 2){
LOG.severe("invalid GAZEBO_MASTER_URI " + user_defined_uri+ ". URI must be of the form HOSTNAME:PORT");
LOG.warning("using default GAZEBO_MASTER_URI=localhost:11345");
}
else {
gazebo_master_uri = parts[0];
port = Integer.parseInt(parts[1]);
}
}
server.serve(this);
master.connectAndWait("localhost", 11345);
LOG.info("GAZEBO_MASTER_URI is host=" + gazebo_master_uri + " port="+port);
master.connectAndWait(gazebo_master_uri, port);
initializeConnection();
new Thread(this).start();
LOG.info("Serving on: "+server.host+":"+server.port);
}
@@ -66,9 +86,9 @@ public class Node implements Runnable, ServerCallback {
String type = defaultMessage.getDescriptorForType().getFullName();
Publisher<T> pub = new Publisher<T>(topic, type, server.host, server.port);
publishers.put(topic, pub);
Publish req = Publish.newBuilder().setTopic(topic).setMsgType(type)
.setHost(server.host).setPort(server.port).build();
.setHost(server.host).setPort(server.port).build();
try {
master.writePacket("advertise", req);
} catch (IOException e) {
@@ -76,7 +96,7 @@ public class Node implements Runnable, ServerCallback {
}
return pub;
}
public synchronized <T extends Message> Subscriber<T>
subscribe(String topic, T defaultMessage, SubscriberCallback<T> cb) {
topic = fixTopic(topic);
@@ -84,7 +104,7 @@ public class Node implements Runnable, ServerCallback {
if (subscriptions.containsKey(topic)) {
throw new RuntimeException("Multiple subscribers for: "+topic);
}
String type = defaultMessage.getDescriptorForType().getFullName();
Subscribe req = Subscribe.newBuilder().setTopic(topic).setMsgType(type)
.setHost(server.host).setPort(server.port).setLatching(false).build();
@@ -93,7 +113,7 @@ public class Node implements Runnable, ServerCallback {
} catch (IOException e) {
e.printStackTrace(); // FIXME: Shouldn't happen, should probably complain louder
}
Subscriber<T> s = new Subscriber<>(topic, type, cb, defaultMessage,
server.host, server.port);
subscriptions.put(topic, s);
@@ -122,7 +142,7 @@ public class Node implements Runnable, ServerCallback {
e.printStackTrace(); // FIXME: Log
}
}
private synchronized void initializeConnection() throws IOException {
Packet initData = master.read();
if (!initData.getType().equals("version_init")) {
@@ -135,12 +155,12 @@ public class Node implements Runnable, ServerCallback {
String_V ns = String_V.parseFrom(namespaceData.getSerializedData());
namespaces.addAll(ns.getDataList());
LOG.info(namespaces.toString());
Packet publisherData = master.read();
Packet publisherData = master.read();
if (publisherData.getType().equals("publishers_init")) {
Publishers pubs = Publishers.parseFrom(publisherData.getSerializedData());
for (Publish pub : pubs.getPublisherList()) {
PublisherRecord record = new RemotePublisherRecord(pub);
PublisherRecord record = new RemotePublisherRecord(pub);
publishers.put(record.getTopic(), record);
}
LOG.info(publishers.toString());
@@ -148,7 +168,7 @@ public class Node implements Runnable, ServerCallback {
LOG.severe("No publisher data received.");
}
}
public synchronized void processPacket(Packet packet) throws InvalidProtocolBufferException {
if (packet.getType().equals("publisher_add")) {
PublisherRecord pub = new RemotePublisherRecord(Publish.parseFrom(packet.getSerializedData()));
@@ -157,7 +177,7 @@ public class Node implements Runnable, ServerCallback {
LOG.info("ACK "+pub.getTopic());
return; // This is us
}
LOG.info("New Publisher: "+pub.getTopic());
LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
publishers.put(pub.getTopic(), pub);
@@ -169,7 +189,7 @@ public class Node implements Runnable, ServerCallback {
LOG.info("Ignoring subscription request on (local) "+pub.getTopic());
return; // This is us
}
LOG.info("PUBSUB found for "+pub.getTopic());
LOG.fine("Publisher: "+Publish.parseFrom(packet.getSerializedData()));
subscriptions.get(pub.getTopic()).connect(pub);
@@ -185,6 +205,9 @@ public class Node implements Runnable, ServerCallback {
}
@Override
/**
* This is called when another node requests subscription to a topic we are publishing
*/
public void handle(Connection conn) throws IOException {
LOG.fine("Handling new connection");
Packet msg = conn.read();
@@ -192,7 +215,7 @@ public class Node implements Runnable, ServerCallback {
LOG.warning("Read null message.");
return;
}
if (msg.getType().equals("sub")) {
Subscribe sub = Subscribe.parseFrom(msg.getSerializedData());
if (!publishers.containsKey(sub.getTopic())) {
@@ -205,11 +228,12 @@ public class Node implements Runnable, ServerCallback {
PublisherRecord pub = publishers.get(sub.getTopic());
if (!pub.getMsgType().equals(sub.getMsgType())) {
LOG.severe(String.format("Message type mismatch requested=%d publishing=%s\n",
pub.getMsgType(), sub.getMsgType()));
pub.getMsgType(), sub.getMsgType()));
return;
}
LOG.info("CONN " + sub.getTopic());
//Tell the publisher that it has recieved a connection from a subscriver
pub.connect(conn);
} else {
LOG.warning("Unknown message type: " + msg.getType());

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

@@ -65,6 +65,10 @@ public class Publisher<T extends Message> implements PublisherRecord {
}
@Override
/**
* This function is called when another topic requests a subscription to a topic I am publishing
* Called in Node.java in the handle() function
*/
public synchronized void connect(Connection conn) {
LOG.fine("Handling subscriber connection for topic: "+topic);
if (latching && lastMsg != null) {