I am trying to control a robot using a template-based controller class written in c++. Essentially I have a UDP connection setup with the robot to receive the state of the robot and send new torque commands to the robot. I receive new observations at a higher frequency (say 2000Hz) and my controller takes about 1ms (1000Hz) to calculate new torque commands to send to the robot. The problem I am facing is that I don't want my main code to wait to send the old torque commands while my controller is still calculating new commands to send. From what I understand I can use Ubuntu with RT-Linux kernel, multi-thread the code so that my getTorques() method runs in a different thread, set priorities for the process, and use mutexes and locks to avoid data race between the 2 threads, but I was hoping to learn what the best strategies to write hard-realtime code for such a problem are.
// main.cpp
#include "CONTROLLER.h"
#include "llapi.h"
void main{
...
CONTROLLERclass obj;
...
double new_observation;
double u;
...
while(communicating){
get_newObs(new_observation); // Get new state of the robot (2000Hz)
obj.getTorques(new_observation, u); // Takes about 1ms to calculate new torques
send_newCommands(u); // Send the new torque commands to the robot
}
...
}
Thanks in advance!
Okay, so first of all, it sounds to me like you need to deal with the fact that you receive input at 2 KHz, but can only compute results at about 1 KHz.
Based on that, you're apparently going to have to discard roughly half the inputs, or else somehow (in a way that makes sense for your application) quickly combine the inputs that have arrived since the last time you processed the inputs.
But as the code is structured right now, you're going to fetch and process older and older inputs, so even though you're producing outputs at ~1 KHz, those outputs are constantly being based on older and older data.
For the moment, let's assume you want to receive inputs as fast as you can, and when you're ready to do so, you process the most recent input you've received, produce an output based on that input, and repeat.
In that case, you'd probably end up with something on this general order (using C++ threads and atomics for the moment):
std::atomic<double> new_observation;
std::thread receiver = [&] {
double d;
get_newObs(d);
new_observation = d;
};
std::thread sender = [&] {
auto input = new_observation;
auto u = get_torques(input);
send_newCommands(u);
};
I've assumed that you'll always receive input faster than you can consume it, so the processing thread can always process whatever input is waiting, without receiving anything to indicate that the input has been updated since it was last processed. If that's wrong, things get a little more complex, but I'm not going to try to deal with that right now, since it sounds like it's unnecessary.
As far as the code itself goes, the only thing that may not be obvious is that instead of passing a reference to new_input to either of the existing functions, I've read new_input into variable local to the thread, then passed a reference to that.
Related
I'm building a tetris game and I need the pieces to fall every x seconds; something like:
while(true){
moveDown();
sleep(x)
}
The problem is, I need to be able to move the pieces left and right in the meantime, i.e., call a function while it's sleeping.
How can I do that in c++?
Both time and key presses can be events which can be used to wait on. On UNIXes you'd use something like poll() with a suitable time for timeout and the input device used to recognize key presses. On other systems there are similar facilities (I'm a UNIX persons and I have never worked on Windows specific stuff although it seems the Windows facilities are actually more flexible). Depending on the result of poll() (timeout or activity on the I/O device in that case) you'd do the appropriate action.
This problem is solvable in multiple ways (another idea that comes to mind is multithreading, but that seems overkill). One approach would be to keep track of the number of "game cycles" and execute some function every n-th cycle like this:
for(int32_t count{1};;count++)
{
if (!count % 5)
{
// do something every 5th cycle
}
// do something every cycle
sleep(x);
}
you can measure how much time has passed since last fall and move piece down after given amount and then reset counter. In pseudo-code it could look like this:
while(true)
{
counter.update();
if(counter.value() == fall_period)
{
move_piece_down();
couter.reset();
}
// rotate pieces
}
If you are using typical implementation of game loop your counter can just accumulate elapsed time since last frame.
Given an Observable<Input> and a mapping function Function<Input, Output> that is expensive but takes variable time, is there a way to call the mapping function in parallel on multiple inputs, and receive the outputs in the order they're produced?
I've tried using observeOn() with a multi-threaded Scheduler:
PublishSubject<Input> inputs = PublishSubject.create();
Function<Input, Output> mf = ...
Observer<Output> myObserver = ...
// Note: same results with newFixedThreadPool(2)
Executor exec = Executors.newWorkStealingThreadPool();
// Use ConnectableObservable to make sure mf is called only once
// no matter how many downstream observers
ConnectableObservable<Output> outputs = inputs
.observeOn(SchedulersFrom(exec))
.map(mf)
.publish();
outputs.subscribe(myObserver1);
outputs.subscribe(myObserver2);
outputs.connect();
inputs.onNext(slowInput); // `mf.apply()` takes a long time to complete on this input
inputs.onNext(fastInput); // `mf.apply()` takes a short time to complete on this input
but in testing, mf.apply(fastInput) is never called till after mf.apply(slowInput) completes.
If I play some tricks in my test with CountDownLatch to ensure mf.apply(slowInput) can't complete until after mf.apply(fastInput), the program deadlocks.
Is there some simple operator I should be using here, or is getting Observables out of order just against the grain of RxJava, and I should be using a different technology?
ETA: I looked at using ParallelFlowable (converting it back to a plain Flowable with .sequential() before subscribing myObserver1/2, or rather mySubscriber1/2), but then I get extra mf.apply() calls, one per input per Subscriber. There's ConnectableFlowable, but I'm not having much luck figuring out how to mix it with .parallel().
I guess observeOn operator does not support concurrent execution for alone. So, how about using flatMap? Assume the mf function needs a lot time.
ConnectableObservable<Output> outputs = inputs
.flatMap(it -> Observable.just(it)
.observeOn(SchedulersFrom(exec))
.map(mf))
.publish();
or
ConnectableObservable<Output> outputs = inputs
.flatMap(it -> Observable.just(it)
.map(mf))
.subscribeOn(SchedulersFrom(exec))
.publish();
Edit 2019-12-30
If you want to run tasks concurrently, but supposed to keep the order, use concatMapEager operator instead of flatMap.
ConnectableObservable<Output> outputs = inputs
.concatMapEager(it -> Observable.just(it) // here
.observeOn(SchedulersFrom(exec))
.map(mf))
.publish();
Doesn't sound possible to me, unless Rx has some very specialised operator to do so. If you're using flatMap to do the mapping, then the elements will arrive out-of-order. Or you could use concatMap but then you'll lose the parallel mapping that you want.
Edit: As mentioned by another poster, concatMapEager should work for this. Parallel subscription and in-order results.
I am streaming data as a string over UDP, into a Socket class inside Unreal engine. This is threaded, and runs in the background.
My read function is:
float translate;
void FdataThread::ReceiveUDP()
{
uint32 Size;
TArray<uint8> ReceivedData;
if (ReceiverSocket->HasPendingData(Size))
{
int32 Read = 0;
ReceivedData.SetNumUninitialized(FMath::Min(Size, 65507u));
ReceiverSocket->RecvFrom(ReceivedData.GetData(), ReceivedData.Num(), Read, *targetAddr);
}
FString str = FString(bytesRead, UTF8_TO_TCHAR((const UTF8CHAR *)ReceivedData));
translate = FCString::Atof(*str);
}
I then call the translate variable from another class, on a Tick, or timer.
My test case sends an incrementing number from another application.
If I print this number from inside the above Read function, it looks as expected, counting up incrementally.
When i print it from the other thread, it is missing some of the numbers.
I believe this is because I call it on the Tick, so it misses out some data due to processing time.
My question is:
Is there a way to queue the incoming data, so that when i pull the value, it is the next incremental value and not the current one? What is the best way to go about this?
Thank you, please let me know if I have not been clear.
Is this the complete code? ReceivedData isn't used after it's filled with data from the socket. Instead, an (in this code) undefined variable 'buffer' is being used.
Also, it seems that the while loop could run multiple times, overwriting old data in the ReceivedData buffer. Add some debugging messages to see whether RecvFrom actually reads all bytes from the socket. I believe it reads only one 'packet'.
Finally, especially when you're using UDP sockets over the network, note that the UDP protocol isn't guaranteed to actually deliver its packets. However, I doubt this is causing your problems if you're using it on a single computer or a local network.
Your read loop doesn't make sense. You are reading and throwing away all datagrams but the last in any given sequence that happen to be in the socket receive buffer at the same time. The translate call should be inside the loop, and the loop should be while(true), or while (running), or similar.
I have a somewhat simple multithreaded application written using the C++ std::thread library for both Ubuntu 14.04 and Windows 8.1. The code is nearly completely identical except that I'm using the operating system respective libraries windows.h and unistd.h to use Sleep/sleep to pause execution for a time. They both actually begin to run and the Ubuntu version does keep running for a short time but then hangs. I am using the proper arguments to the sleep/Sleep functions since I know Windows Sleep takes milliseconds, while Unix sleep takes seconds.
I've run the code multiple times and on Ubuntu it never makes it past two minutes whereas I've run it on windows twice for 20 minutes and then multiple times for roughly five minutes each to see if I was just lucky. Is this just an incompatibility with the thread library or does sleep not do what I think it does, or something else? The infinite loops are there because this is a school project and is expected to run without deadlocks or crashing.
The gist is that this is a modified 4-way stop where cars who arrive first don't have to slow down and stop. We only had to let one car through the intersection at a time which takes 3 seconds to cross, hence Sleep(3000), and don't have to worry about turns. Three threads run the spawnCars function and there are four other threads that each monitor one of the four directions N, E, S, and W. I hope that it's understandable why I can't post the entire code in the chance some other student stumbles upon this. These two functions are the only place where code is different aside from the operating system dependent library inclusion at the top. Thanks.
edit: Since I've just gone and posted all the code for the project, if the problem does end up being a deadlock, may I request that you only say so, and not post an in depth solution? I'm new here so if that's against the spirit of SO then fire away and I'll try to figure it out without reading the details.
/* function clearIntersection
Makes a car go through the intersection. The sleep comes before the removal from the queue
because my understanding is that the wait condition simulates the go signal for drivers.
It wouldn't make sense for the sensors to tell a car to go if the intersection isn't yet
clear even if the lock here would prevent that.
*/
void clearIntersection(int direction)
{
lock->lock();
Sleep(3000);
dequeue(direction);
lock->unlock();
}
/* function atFront(int direction)
Checks whether the car waiting at the intersection from a particular direction
has permission to pass, meaning it is at the front of the list of ALL waiting cars.
This is the waiting condition.
*/
bool isAtFront(int direction)
{
lock->lock();
bool isAtFront = cardinalDirections[direction].front() == list->front();
lock->unlock();
return isAtFront;
}
void waitInLine()
{
unique_lock<mutex> conditionLock(*lock);
waitForTurn->wait(conditionLock);
conditionLock.unlock();
}
//function broadcast(): Let all waiting threads know they can check whether or not their car can go.
void broadcast()
{
waitForTurn->notify_all();
}
};
/* function monitorDirection(intersectionQueue,int,int)
Threads will run this function. There are four threads that run this function
in total, one for each of the cardinal directions. The threads check to see
if the car at the front of the intersectionQueue, which contains the arrival order
of cars regardless of direction, is the car at the front of the queue for the
direction the thread is assigned to monitor. If not, it waits on a condition
variable until it is the case. It then calls the function to clear the intersection.
Broadcast is then used on the condition variable so all drivers will check if they
are allowed to pass, which one will unless there are 0 waiting cars, waiting again if not the case.
*/
void monitorDirection(intersectionQueue *intersection, int direction, int id)
{
while (true) //Do forever to see if crashes can occur.
{
//Do nothing if there are no cars coming from this direction.
//Possibly add more condition_variables for each direction?
if (!intersection->empty(direction))
{
while (!intersection->isAtFront(direction))
intersection->waitInLine();
intersection->clearIntersection(direction);
cout << "A car has gone " << numberToDirection(direction) << endl;
//All cars at the intersection will check the signal to see if it's time to go so broadcast is used.
intersection->broadcast();
}
}
}
Your culprit is likely your while (!isAtFront(...)) loop. If another thread gets scheduled between the check and the subsequent call to waitInLine(), the state of your queues could change, causing all of your consumer threads to end up waiting. At that point there's no thread to signal your condition_variable, so they will wait forever.
I need help making a gnuradio OOT module. I am actually trying to extending one code.
I am trying to make 2 TX 1Rx tagged stream block (OOT). For 1Tx 1Rx, it
is working fine. I am trying to extend it. Now the problem is, I am not
being able to configure the send() function. With this code, one transmitter transmits, but other does not work. The subdev specification and frequency and other parameters are allocated correctly. I checked.
If I try make test, it does not show any problem. I checked the every
port of my USRP X310, it is working fine.
Here's the code. I putting a short part which deals with the send and receive buffer.
void
usrp_echotimer_cc_impl::send()
{
// Data to USRP
num_tx_samps = d_tx_stream->send(d_in_send1, total_num_samps,
d_metadata_tx, total_num_samps/(float)d_samp_rate+d_timeout_tx);
num_tx_samps = d_tx_stream->send(d_in_send0, total_num_samps,
d_metadata_tx, total_num_samps/(float)d_samp_rate+d_timeout_tx);
}
int
usrp_echotimer_cc_impl::work (int noutput_items,
gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
gr_complex *in0 = (gr_complex *) input_items[0];
gr_complex *in1 = (gr_complex *) input_items[1];
gr_complex *out = (gr_complex *) output_items[0];
// Set output items on packet length
noutput_items = ninput_items[0]=ninput_items[1];
// Resize output buffer
if(d_out_buffer.size()!=noutput_items)
d_out_buffer.resize(noutput_items);
// Send thread
d_in_send0 = in0;
d_in_send1 = in1;
d_noutput_items_send = noutput_items;
d_thread_send =
gr::thread::thread(boost::bind(&usrp_echotimer_cc_impl::send, this));
// Receive thread
d_out_recv = &d_out_buffer[0];
d_noutput_items_recv = noutput_items;
d_thread_recv =
gr::thread::thread(boost::bind(&usrp_echotimer_cc_impl::receive, this));
My system config is X310, daughterboard SBX-120 and I am using UHD-3.9. I checked the subdev specification, gain and frequency assignment. Those
are fine.
For completeness:
This has been asked yesterday on the GNU Radio mailing list; Sanjoy has already gotten two responses:
Martin Braun wrote:
Sorry, Sanjoy,
we'll need some more information before we can give you better
feedback. It's not clear exactly what you're trying to achieve, and
what exactly is failing.
Perhaps this helps getting started:
http://gnuradio.org/redmine/projects/gnuradio/wiki/ReportingErrors
Cheers, Martin
And my answer is a bit longish, but it's available here. In excerpts:
Hi Sanjoy,
I am trying to make 2 TX 1Rx tagged stream block(OOT). For 1Tx 1Rx, it was working fine. I am trying to extend it. Now the problem is, I
am not being able to configure the send() function.
Is there a
particular reason you're creating your own block? Is there a feature
missing on the USRP sink and source that come with gr-uhd?
...
your send() function looks a bit strange; what you're doing is
transmitting two things on a single channel after each other. Also,
what you should be doing (from a programming style point of view) is
pass the buffers you want to send as references or something, but not
save them to class properties and then call send(). To send two
buffers to two different channels, you will need to use a vector
containing the two buffers -- have a look at rx_multi_samples; that of
course recv()s rather than sends(), but the semantics are the same.
...
noutput_items is given to you to let your work now how much it may produce, that's why it's a parameter.
...
There's no reason GNU Radio couldn't already call your work function again while usrp_echotimer_cc_impl::send() hasn't even started to transmit samples. Then, your d_send variables will be overwritten.
...
Since a single X310 is inherently coherent and has the same time, you can simply use a USRP sink and a source, use set_start_time(...) on both with the same time spec, and have your flow graph consume and produce samples in different threads, coherently.