Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed FRCSim artf2619, and misc reformatting.

Browse Source 
Change-Id: I7133f46f88f7e2cb2451c2a6714daa8f3f368b40
This commit is contained in:
Colby Skeggs
2014-07-22 13:10:23 -07:00
parent 461e359484
commit 2dd45c3ea6
5 changed files with 86 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,6 +79,27 @@ public class Publisher<T extends Message> 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) {

33
wpilibc/wpilibC++Sim/include/simulation/MainNode.h
View File

@@ -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<typename M>
static transport::PublisherPtr Advertise(const std::string &topic,
template<typename M>
static transport::PublisherPtr Advertise(const std::string &topic,
unsigned int _queueLimit = 10,
bool _latch = false) {
return GetInstance()->main->Advertise<M>(topic, _queueLimit, _latch);
return GetInstance()->main->Advertise<M>(topic, _queueLimit, _latch);
}
template<typename M, typename T>
static transport::SubscriberPtr Subscribe(const std::string &topic,
template<typename M, typename T>
static transport::SubscriberPtr Subscribe(const std::string &topic,
void(T::*fp)(const boost::shared_ptr<M const> &), T *obj,
bool _latching = false) {
return GetInstance()->main->Subscribe(topic, fp, obj, _latching);
return GetInstance()->main->Subscribe(topic, fp, obj, _latching);
}
template<typename M>
static transport::SubscriberPtr Subscribe(const std::string &topic,
template<typename M>
static transport::SubscriberPtr Subscribe(const std::string &topic,
void(*fp)(const boost::shared_ptr<M const> &),
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();
}
};

27
wpilibc/wpilibC++Sim/include/simulation/SimEncoder.h
View File

@@ -5,6 +5,7 @@
#include "simulation/msgs/msgs.h"
#include <gazebo/transport/transport.hh>
#include <gazebo/common/Time.hh>
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

40
wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp
View File

@@ -3,50 +3,52 @@
#include "simulation/MainNode.h"
SimEncoder::SimEncoder(std::string topic) {
commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
posSub = MainNode::Subscribe("~/simulator/"+topic+"/position",
&SimEncoder::positionCallback, this);
velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity",
&SimEncoder::velocityCallback, this);
commandPub = MainNode::Advertise<msgs::GzString>("~/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();
}

18
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java
View File

@@ -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<Float64>() {
@@ -21,15 +20,26 @@ public class SimEncoder {
position = msg.getData();
}
}
);
MainNode.subscribe(topic+"/velocity", Msgs.Float64(),
);
MainNode.subscribe(topic+"/velocity", Msgs.Float64(),
new SubscriberCallback<Float64>() {
@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() {