How to send trajectory points and create a buffer

[emielkemmm]

Disclaimer: I'm not using ROS for reasons I won't go into here.

That being said, I've created my own streamer that sends simple_msg format over TCP, the same as would be found within ROS. I am sending the simple_msg of a trajectory point, and I am waiting until I receive a SUCCESS response before sending the next point. If I receive a BUSY response, I do not move on to the next trajectory point.

What I am finding is that the motoplus side is interpolating and filling up the inc queue and is essentially blocking until it finishes processing. This is found here. This will not release until either it fails to add the pulse increment OR finishes such that curTrajData->time >= endTrajData->time. In the meantime, this is sending back BUSY messages over TCP on the motion server.

This, to me, makes it seem like I cannot send another point until I have received back that the previous point has been reached. To me, it seems like I already should have the next point in a buffer somewhere, so that once the first point is reached, it's not waiting for the next point to come in to continue the motion.

Is my interpretation wrong? In practice, I'm doing a simple trajectory moving one joint 1 degree every 2 seconds (with the appropriate velocity at the intermediate points) for 10 points. I get the overall shape of the motion (it indeed goes through 10 degrees of motion) but it takes longer than the 20 seconds I would expect, and it looks like it still stops in between each waypoint.

I'm trying to get behavior more like RSI, where I send a point every 4ms, but not finding success.

[ted-miller]

Hi @emielkemmm
When a point is received, it is broken down into 4ms increments. These increments get stored in an internal queue. That queue holds up to 200 increments.

When a subsequent point is received, the code will return BUSY until all of the increments from the first point have been populated into the queue. Once the first point has been increment-ized, then the code will accept another incoming point and start breaking it into increments.

So as long as your second point comes in before the queue is empty, then you should get smooth continuous motion. Since there are 200 four-millisecond increments in the queue, you've got a window of 800 ms to submit the followup point before the queue is empty.

[emielkemmm]

Ah, i see. So the curTrajData->time isn't what has been achieved, but what has been interpolated thus far and stored in the internal queue. So if I sent a point that was 2 seconds, it would split it up into chunks of 4ms, or split into 500 points. It would only return a SUCCESS instead of BUSY after it had run through 300 points and 200 were left in the queue, and I would then have ~800ms to send the next point. Is that correct?

So theoretically, I compute the trajectory, and split up into chunks with some time discretization and send over TCP. If the time discretization is 4ms, it would just run the while loop one time and add that to the internal queue and return SUCCESS. I'll have a tight window to send the next point, since the queue will only have one point in it.

If the time discretization is >4ms, I'll have some iterations of the loop as it adds to the internal queue, which may take some time, depending on my point discretization, and I'll have a larger window to send the next point.

[ted-miller]

It would only return a SUCCESS instead of BUSY after it had run through 300 points and 200 were left in the queue, and I would then have ~800ms to send the next point. Is that correct?

Correct.

If the time discretization is 4ms, it would just run the while loop one time and add that to the internal queue and return SUCCESS. I'll have a tight window to send the next point, since the queue will only have one point in it.

Yes. Given the overhead of processing the incoming point and queueing it up, I don't know if you could deterministically hit every cycle. If you wanted to go about real-time streaming, I would recommend optimizing the code for packet handling/break-down.

[gavanderhoorn]

@emielkemmm: we're interested in adding more low-latency interfaces to MotoROS2 (so the ROS 2 interface).

I'm planning on starting up a discussion with some other users that have either done similar things in the past with MotoROS1, or have expressed interest in such interfaces.

Note that we won't be able to make anything faster than the controller supports (see #219 fi), but at least we can try to add as little additional latency with the interfaces we can make available.

Would you be interested to join that discussion?

[emielkemmm]

@gavanderhoorn yes I'd be interested.