diff --git a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java index 8bf973e793..6f9c622fe3 100644 --- a/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java +++ b/simulation/JavaGazebo/src/main/java/org/gazebosim/transport/Publisher.java @@ -79,6 +79,27 @@ public class Publisher implements PublisherRecord { } } listeners.add(conn); + this.notifyAll(); + } + + public synchronized void waitForConnection() throws InterruptedException { + while (this.listeners.isEmpty()) { + this.wait(); + } + } + + public synchronized boolean waitForConnection(long timeout_millis) throws InterruptedException { + long start = System.currentTimeMillis(); + + while (this.listeners.isEmpty()) { + long remain = timeout_millis - (System.currentTimeMillis() - start); + if (remain <= 0) { + break; + } + this.wait(remain); + } + + return !this.listeners.isEmpty(); } public void setLatchMode(boolean b) { diff --git a/wpilibc/wpilibC++Sim/include/simulation/MainNode.h b/wpilibc/wpilibC++Sim/include/simulation/MainNode.h index 696670b1f4..12923c893f 100644 --- a/wpilibc/wpilibC++Sim/include/simulation/MainNode.h +++ b/wpilibc/wpilibC++Sim/include/simulation/MainNode.h @@ -10,43 +10,42 @@ using namespace gazebo; class MainNode { public: static MainNode* GetInstance() { - if (instance == NULL) { + if (instance == NULL) { instance = new MainNode(); } return instance; } - - template - static transport::PublisherPtr Advertise(const std::string &topic, + template + static transport::PublisherPtr Advertise(const std::string &topic, unsigned int _queueLimit = 10, bool _latch = false) { - return GetInstance()->main->Advertise(topic, _queueLimit, _latch); + return GetInstance()->main->Advertise(topic, _queueLimit, _latch); } - template - static transport::SubscriberPtr Subscribe(const std::string &topic, + template + static transport::SubscriberPtr Subscribe(const std::string &topic, void(T::*fp)(const boost::shared_ptr &), T *obj, bool _latching = false) { - return GetInstance()->main->Subscribe(topic, fp, obj, _latching); + return GetInstance()->main->Subscribe(topic, fp, obj, _latching); } - template - static transport::SubscriberPtr Subscribe(const std::string &topic, + template + static transport::SubscriberPtr Subscribe(const std::string &topic, void(*fp)(const boost::shared_ptr &), bool _latching = false) { - return GetInstance()->main->Subscribe(topic, fp, _latching); - } + return GetInstance()->main->Subscribe(topic, fp, _latching); + } - transport::NodePtr main; + transport::NodePtr main; private: static MainNode* instance; MainNode() { - gazebo::transport::init(); - main = transport::NodePtr(new transport::Node()); - main->Init("frc"); - gazebo::transport::run(); + gazebo::transport::init(); + main = transport::NodePtr(new transport::Node()); + main->Init("frc"); + gazebo::transport::run(); } }; diff --git a/wpilibc/wpilibC++Sim/include/simulation/SimEncoder.h b/wpilibc/wpilibC++Sim/include/simulation/SimEncoder.h index 5527d3d013..6f551d27be 100644 --- a/wpilibc/wpilibC++Sim/include/simulation/SimEncoder.h +++ b/wpilibc/wpilibC++Sim/include/simulation/SimEncoder.h @@ -5,6 +5,7 @@ #include "simulation/msgs/msgs.h" #include +#include using namespace gazebo; @@ -12,20 +13,20 @@ class SimEncoder { public: SimEncoder(std::string topic); - void Reset(); - void Start(); - void Stop(); - double GetPosition(); - double GetVelocity(); - + void Reset(); + void Start(); + void Stop(); + double GetPosition(); + double GetVelocity(); + private: - void sendCommand(std::string cmd); - - double position, velocity; - transport::SubscriberPtr posSub, velSub; - transport::PublisherPtr commandPub; - void positionCallback(const msgs::ConstFloat64Ptr &msg); - void velocityCallback(const msgs::ConstFloat64Ptr &msg); + void sendCommand(std::string cmd); + + double position, velocity; + transport::SubscriberPtr posSub, velSub; + transport::PublisherPtr commandPub; + void positionCallback(const msgs::ConstFloat64Ptr &msg); + void velocityCallback(const msgs::ConstFloat64Ptr &msg); }; #endif diff --git a/wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp b/wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp index 51602f7087..b72b8f1152 100644 --- a/wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp +++ b/wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp @@ -3,50 +3,52 @@ #include "simulation/MainNode.h" SimEncoder::SimEncoder(std::string topic) { - commandPub = MainNode::Advertise("~/simulator/"+topic+"/control"); - - posSub = MainNode::Subscribe("~/simulator/"+topic+"/position", - &SimEncoder::positionCallback, this); - velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity", - &SimEncoder::velocityCallback, this); + commandPub = MainNode::Advertise("~/simulator/"+topic+"/control"); - commandPub->WaitForConnection(); - - std::cout << "Initialized ~/simulator/"+topic << std::endl; + posSub = MainNode::Subscribe("~/simulator/"+topic+"/position", + &SimEncoder::positionCallback, this); + velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity", + &SimEncoder::velocityCallback, this); + + if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds. + std::cout << "Initialized ~/simulator/" + topic << std::endl; + } else { + std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the encoder exist?" << std::endl; + } } void SimEncoder::Reset() { - sendCommand("reset"); + sendCommand("reset"); } void SimEncoder::Start() { - sendCommand("start"); + sendCommand("start"); } void SimEncoder::Stop() { - sendCommand("stop"); + sendCommand("stop"); } double SimEncoder::GetPosition() { - return position; + return position; } double SimEncoder::GetVelocity() { - return velocity; + return velocity; } void SimEncoder::sendCommand(std::string cmd) { - msgs::GzString msg; - msg.set_data(cmd); - commandPub->Publish(msg); + msgs::GzString msg; + msg.set_data(cmd); + commandPub->Publish(msg); } void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) { - position = msg->data(); + position = msg->data(); } void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) { - velocity = msg->data(); + velocity = msg->data(); } diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java index 0a5f69caae..9f09f5cb6a 100644 --- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java +++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java @@ -13,7 +13,6 @@ public class SimEncoder { public SimEncoder(String topic) { command_pub = MainNode.advertise(topic+"/control", Msgs.String()); - command_pub.setLatchMode(true); MainNode.subscribe(topic+"/position", Msgs.Float64(), new SubscriberCallback() { @@ -21,15 +20,26 @@ public class SimEncoder { position = msg.getData(); } } - ); - - MainNode.subscribe(topic+"/velocity", Msgs.Float64(), + ); + + MainNode.subscribe(topic+"/velocity", Msgs.Float64(), new SubscriberCallback() { @Override public void callback(Float64 msg) { velocity = msg.getData(); } } ); + + try { + if (command_pub.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(); + } } public void reset() {