TEST(SemaphoreTest, test)
{
Semaphore sem;
ASSERT_EQ(sem.wait(100), false);
sem.post();
ASSERT_EQ(sem.wait(100), true);
thread th1([&] {
cout << "post1" << endl;
sem.post();
this_thread::sleep_for(chrono::milliseconds(1000));
cout << "post2" <<endl;
sem.post();
cout << "post3" << endl;
sem.post();
});
sem.wait();
cout << "wait1" << endl;
sem.wait();
cout << "wait2" << endl;
sem.wait();
cout << "wait3" << endl;
th1.join();
}
int main()
{
Queue<struct iMessage> * q;
q = new Queue<struct iMessage>(PATH, Q_FROM_INTERFACE, "terminador", false);
q->get();
q->remove();
q = new Queue<struct iMessage>(PATH, Q_TO_INTERFACE, "terminador", false);
q->get();
q->remove();
Semaphore * s;
s = new Semaphore(PATH, SEM_MUTEX, "terminador", false);
s->get();
s->remove();
s->post();
s = new Semaphore(PATH, SEM_LECTORES, "terminador", false);
s->get();
s->remove();
s = new Semaphore(PATH, SEM_ESCRITORES, "terminador", false);
s->get();
s->remove();
s = new Semaphore(PATH, SEM_COTIZ, "terminador", false);
s->get();
s->remove();
s->post();
SharedMemory<struct registro> * shm;
shm = new SharedMemory<struct registro>(PATH, SHM_REGISTRO, "terminador", false);
shm->get();
shm->remove();
}
virtual bool read(ConnectionReader& reader) {
go.wait();
mutex.wait();
bool ok = last.read(reader);
mutex.post();
gone.post();
return ok;
}
void wait() {
stateMutex.wait();
if (signalled) {
stateMutex.post();
return;
}
waiters++;
stateMutex.post();
action.wait();
if (autoReset) {
reset();
}
}
void signal(bool after = true) {
stateMutex.wait();
int w = waiters;
if (w>0) {
if (autoReset) { w = 1; }
for (int i=0; i<w; i++) {
action.post();
waiters--;
}
}
signalled = after;
stateMutex.post();
}
bool process() {
produce.wait();
if (!active) return false;
mutex.wait();
Bottle b = msgs.front();
msgs.pop_front();
mutex.post();
DummyConnector con;
b.write(con.getWriter());
owner.read(con.getReader());
mutex.wait();
available_threads++;
mutex.post();
return active;
}
int main()
{
// Create the queues
Queue<struct iMessage> * q;
q = new Queue<struct iMessage>(PATH, Q_FROM_INTERFACE, "iniciador");
q->create();
q = new Queue<struct iMessage>(PATH, Q_TO_INTERFACE, "iniciador");
q->create();
Semaphore * s;
s = new Semaphore(PATH, SEM_MUTEX, "iniciador");
s->create();
s->post();
s = new Semaphore(PATH, SEM_EMBOTELLADORAS, "iniciador");
s->create();
SharedMemory<struct registro> * shm;
shm = new SharedMemory<struct registro>(PATH, SHM_REGISTRO, "iniciador");
shm->create();
struct registro * reg = shm->attach();
reg->terminaron = 0;
pid_t pid = fork();
if (pid == 0)
{
execlp("./registro", "registro", NULL);
perror("registro - execlp: ");
exit(EXIT_FAILURE);
} else if (pid < 0)
{
perror("registro - fork: ");
}
}
NetworkClock::~NetworkClock() {
YARP_WARN(Logger::get(), "Destroying network clock");
listMutex.lock();
closing = true;
port.interrupt();
Waiters* waiters = static_cast<Waiters*>(pwaiters);
if (waiters) {
Waiters::iterator waiter_i;
waiter_i = waiters->begin();
while (waiter_i != waiters->end())
{
Semaphore *waiterSemaphore = waiter_i->second;
waiter_i = waiters->erase(waiter_i);
if (waiterSemaphore)
waiterSemaphore->post();
}
delete waiters;
pwaiters = nullptr;
}
listMutex.unlock();
yarp::os::ContactStyle style;
style.persistent = true;
NetworkBase::disconnect(clockName, port.getName(), style);
}
void onRead(Sound& sound) {
int ct = port.getPendingReads();
//printf("pending reads %d\n", ct);
while (ct>padding) {
ct = port.getPendingReads();
printf("Dropping sound packet -- %d packet(s) behind\n", ct);
port.read();
}
mutex.wait();
/*
if (muted) {
for (int i=0; i<sound.getChannels(); i++) {
for (int j=0; j<sound.getSamples(); j++) {
sound.put(0,j,i);
}
}
}
*/
if (!muted) {
if (put!=NULL) {
put->renderSound(sound);
}
}
if (saving) {
saveFrame(sound);
}
mutex.post();
Time::yield();
}
void sendNow(ActionItem *item)
{
mutex.wait();
actions.clear();
_send(item);
mutex.post();
}
ActionItem *pop()
{
mutex.wait();
ActionItem *ret=actions.pop();
mutex.post();
return ret;
}
bool saveFile(const char *name) {
mutex.wait();
saving = false;
Sound total;
total.resize(samples,channels);
long int at = 0;
while (!sounds.is_empty()) {
Sound tmp;
sounds.dequeue_head(tmp);
for (int i=0; i<channels; i++) {
for (int j=0; j<tmp.getSamples(); j++) {
total.set(tmp.get(j,i),at+j,i);
}
}
total.setFrequency(tmp.getFrequency());
at += tmp.getSamples();
}
mutex.post();
bool ok = write(total,name);
if (ok) {
printf("Wrote audio to %s\n", name);
}
samples = 0;
channels = 0;
return ok;
}
bool go(const string &fileName)
{
if (velPort==NULL)
return false;
if (fin.is_open())
fin.close();
fin.open(fileName.c_str());
if (fin.is_open())
{
fin.seekg(0,ios_base::beg);
// skip the first two lines
fin.getline(&line[0],sizeof(line),'\n');
fin.getline(&line[0],sizeof(line),'\n');
firstRun=true;
cout<<"File loaded"<<endl;
mutex.post();
return true;
}
else
{
cout<<"Unable to load file: "<<fileName<<endl;
return false;
}
}
void getObjectList(Bottle &reply)
{
double t0=Time::now();
int max_size=0;
vector<Blob> max_blobs;
while(Time::now()-t0<0.5)
{
mutex.wait();
reply.clear();
int size=0;
for(unsigned int i=0; i<blobs.size(); i++)
if(blobs[i].getBest()!="")
size++;
if(max_size<size)
{
max_size=size;
max_blobs=blobs;
}
mutex.post();
Time::delay(0.05);
}
reply.addInt(max_size);
for(unsigned int i=0; i<max_blobs.size(); i++)
if(max_blobs[i].getBest()!="")
reply.addString(max_blobs[i].getBest().c_str());
}
void onRead(Bottle &v)
{
mutex.wait();
Datum=v;
//Time::delay(5);
mutex.post();
fprintf(stderr, "Callback got: %s\n",Datum.toString().c_str());
}
virtual bool read(ConnectionReader& reader) override {
go.wait();
mutex.lock();
bool ok = last.read(reader);
mutex.unlock();
gone.post();
return ok;
}
void update() {
Bottle& status = port.prepare();
mutex.wait();
status.clear();
addQueue(status);
mutex.post();
port.write();
}
void drawObj(string &obj_name)
{
draw_mutex.wait();
if(obj_name=="draw")
blob_draw=!blob_draw;
else
current_draw_name=obj_name;
draw_mutex.post();
}
virtual void interrupt()
{
mutex.wait();
//some closure :P
port_out_code.interrupt();
port_out_img.interrupt();
BufferedPort<Image>::interrupt();
mutex.post();
}
virtual void resume()
{
mutex.wait();
//some closure :P
port_out_code.resume();
port_out_img.resume();
BufferedPort<Image>::resume();
mutex.post();
}
void resume()
{
mutex.wait();
port_out_code.resume();
port_out_img.resume();
BufferedPort<Image>::resume();
mutex.post();
}
void onRead(Image &img)
{
// Read at specified rate
if (Time::now() - last_read < rate)
return;
mutex.wait();
// If something arrived...
if (img.width()>0 && img.height()>0)
{
// Convert the image
cv::Mat tmp_mat=cv::cvarrToMat((IplImage*)img.getIplImage());
cv::cvtColor(tmp_mat, matImg, CV_RGB2BGR);
// Extract the feature vector
std::vector<float> codingVecFloat;
float msecPerImage = caffe_extractor->extract_singleFeat_1D(matImg, codingVecFloat);
std::vector<double> codingVec(codingVecFloat.begin(), codingVecFloat.end());
if (caffe_extractor->timing)
{
cout << msecPerImage << " msec" << endl;
}
// Dump if required
if(dump_code)
{
fwrite (&codingVec[0], sizeof(double), codingVec.size(), fout_code);
}
Stamp stamp;
this->getEnvelope(stamp);
if(port_out_code.getOutputCount())
{
port_out_code.setEnvelope(stamp);
Vector codingYarpVec(codingVec.size(), &codingVec[0]);
port_out_code.write(codingYarpVec);
}
if(port_out_img.getOutputCount())
{
port_out_img.write(img);
}
}
mutex.post();
}
void interrupt()
{
mutex.wait();
port_out_code.interrupt();
port_out_img.interrupt();
BufferedPort<Image>::interrupt();
mutex.post();
}
bool read(ConnectionReader &connection)
{
Bottle data;
data.read(connection);
if ((data.size()>=2) && enabled)
{
Vector xa,oa;
iarm->getPose(xa,oa);
Vector xe,oe;
if (eye=="left")
igaze->getLeftEyePose(xe,oe);
else
igaze->getRightEyePose(xe,oe);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Matrix He=axis2dcm(oe);
He.setSubcol(xe,0,3);
Matrix H=Prj*SE3inv(He)*Ha;
Vector p(2);
p[0]=data.get(0).asDouble();
p[1]=data.get(1).asDouble();
if (logPort.getOutputCount()>0)
{
Vector &log=logPort.prepare();
log=p;
for (int i=0; i<H.rows(); i++)
log=cat(log,H.getRow(i));
for (int i=0; i<Prj.rows(); i++)
log=cat(log,Prj.getRow(i));
for (int i=0; i<Ha.rows(); i++)
log=cat(log,Ha.getRow(i));
for (int i=0; i<He.rows(); i++)
log=cat(log,He.getRow(i));
logPort.write();
}
mutex.wait();
solver.addItem(p,H);
mutex.post();
}
return true;
}
void clear() {
active = false;
for (size_t i=0; i<threads.size(); i++) {
produce.post();
}
for (std::list<MessageStackThread *>::iterator it = threads.begin(); it != threads.end(); ++it) {
(*it)->stop();
delete (*it);
*it = YARP_NULLPTR;
}
threads.clear();
msgs.clear();
active = true;
}
void update() {
mutex.wait();
PlatformVector<PolyDriver *>& lst = drivers;
for (unsigned int i=0; i<lst.size(); i++) {
if (needDrive[i]) {
IService *service;
lst[i]->view(service);
if (service!=NULL) {
service->updateService();
}
}
}
mutex.post();
}
void clear() {
mutex.wait();
PlatformVector<PolyDriver *>& lst = drivers;
for (unsigned int i=0; i<lst.size(); i++) {
printf("*** Removing %s\n",names[i].c_str());
Drivers::factory().remove(names[i].c_str());
//printf("*** removed %s\n",names[i].c_str());
delete lst[i];
//printf("*** deleted %s\n",names[i].c_str());
}
lst.clear();
names.clear();
mutex.post();
}
void* tcp_tahoe_close_send_receive_thread(void* args) {
struct var* v = (struct var*) args;
AddressPort* to_bind = v->to_bind_;
Semaphore* sem = v->sem_;
gcstring message = v->expected_string;
// Create server
int server = wifu_socket(AF_INET, SOCK_STREAM, TCP_TAHOE);
int result = wifu_bind(server, (const struct sockaddr *) to_bind->get_network_struct_ptr(), sizeof (struct sockaddr_in));
EXPECT_EQ(0, result);
result = wifu_listen(server, 5);
EXPECT_EQ(0, result);
sem->post();
struct sockaddr_in addr;
socklen_t length = sizeof (addr);
int connection;
if ((connection = wifu_accept(server, (struct sockaddr *) & addr, &length)) < 0) {
// TODO: we need to check errors and make sure they happen when they should
ADD_FAILURE() << "Problem in Accept";
}
AddressPort ap(&addr);
gcstring address("127.0.0.1");
gcstring res = ap.get_address();
EXPECT_EQ(address, res);
// cout << "Connected to: " << ap.to_s() << endl;
// TODO: Check the results of wifu_accept, probably need to wait for send, recv to be implemented
int size = 50000;
char buffer[size];
memcpy(buffer, message.c_str(), message.length());
int count = 1;
int num_sent = 0;
// TODO: this only sends one character at a time
for (int i = 0; i < message.length(); i++) {
num_sent += wifu_send(connection, &(buffer[i]), count, 0);
}
EXPECT_EQ(message.length(), num_sent);
// cout << "SendReceivePassiveToActive, sent message: " << message << endl;
wifu_close(connection);
}
void stack(PortWriter& msg, const ConstString& tag) {
mutex.wait();
msgs.push_back(Bottle());
if (tag!="") {
Bottle b;
b.read(msg);
Bottle& back = msgs.back();
back.clear();
back.addString(tag);
back.append(b);
} else {
msgs.back().read(msg);
}
if (available_threads==0) {
if (threads.size()<max_threads || max_threads == 0) {
available_threads++;
threads.push_back(new MessageStackThread(*this));
threads.back()->start();
}
}
available_threads--;
mutex.post();
produce.post();
}
void close()
{
mutex.wait();
if (dump_code)
{
fclose(fout_code);
}
port_out_code.close();
port_out_img.close();
BufferedPort<Image>::close();
mutex.post();
}
