ahundt / grl

Robotics tools in C++11. Implements soft real time arm drivers for Kuka LBR iiwa plus V-REP, ROS, Constrained Optimization based planning, Hand Eye Calibration and Inverse Kinematics integration.
https://ahundt.github.io/grl/
BSD 2-Clause "Simplified" License
153 stars 72 forks source link

Sending data back with ZMQ #100

Closed ashk-on closed 8 years ago

ashk-on commented 8 years ago

Hi,

I have added the following code snippet to here: https://github.com/ahundt/grl/blob/master/include/grl/kuka/KukaJAVAdriver.hpp#L355

` kukaJavaDriverP->start_async_receive_buffers();

      if (kukaJavaDriverP->receive_buffers_empty()){
        std::cout << "apparently buffers are empty " << "\n";
      }

      while (!kukaJavaDriverP->receive_buffers_empty()) {

          auto rbP = kukaJavaDriverP->get_back_receive_buffer_with_data();
            auto rbPstart = &(rbP->begin()[0]);

            auto verifier = flatbuffers::Verifier(rbPstart,rbP->size());
            auto bufOK = grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier);

          if(bufOK){

                 auto fbKUKAiiwaStates = flatbuffer::GetKUKAiiwaStates(rbPstart);
             auto fbstates = fbKUKAiiwaStates->states();
             auto fbMonitorState = fbstates->Get(0)->monitorState();
             auto wrench = fbMonitorState->CartesianWrench();
             auto force = wrench->force();

              std::cout << "received: x:" << force.x() << "z:" << force.z() << "\n";
          } else {
            std::cout << "Failed verification. bufOk: " <<bufOK << "\n";
          }

         }`

and added this snippet to the JAVA code here: https://github.com/ahundt/grl/blob/master/src/java/grl/src/grl/ZMQManager.java#L104

public void sendMessage(ByteBuffer msg) { subscriber.send(msg.array()); }

The result is that kukaJavaDriverP->receive_buffers_empty() returns true.

Also tried binding the subscriber socket the local zmq address here: https://github.com/ahundt/grl/blob/master/src/java/grl/src/grl/ZMQManager.java#L59 which resulted in errors on the controller pointing to the line that contains binding.

ashk-on commented 8 years ago

It seems that AzmqFlatbuffersTest is working because the receive and send object, are the same object(So binding to same address does not cause problems)... Anyhow, the above problem has not been solved yet...

ahundt commented 8 years ago

I designed the message being sent from the arm to the computer to be the same message being sent from the computer to the arm. Are you trying to use different data types (shouldn't be needed) or is the problem that the test was never run on two separate computers?

You can check the azmq, jeromq, and flatbuffers documentation for additional examples of their respective APIs as well.

Sorry I don't have a great answer at the moment, have someone visiting this week so won't have much time to look into the details till next week.

On Monday, June 20, 2016, Ashkan372 notifications@github.com wrote:

It seems that AzmqFlatbuffersTest is working because the receive and send object, are the same object(So binding to same address does not cause problems)... Anyhow, the above problem has not been solved yet...

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/ahundt/grl/issues/100#issuecomment-227317680, or mute the thread https://github.com/notifications/unsubscribe/AADZwAVeKatXkfM7LEv0BliG_5CADCO7ks5qN0AbgaJpZM4I513D .

Cheers! Andrew Hundt

Ph.D. Student Johns Hopkins University

ashk-on commented 8 years ago

Both are KUKAiiwaStates flatbuffers... I studied zmq and flatbuffers documentations thoroughly... Still no idea... I have to finish this by tmrw... Any suggestion would be greatly appreciated

ahundt commented 8 years ago

I think there may have been problems when the sleep 1ms wasn't there... I don't remember why but that may be worth testing.

Have you verified that bytes are being sent both directions via wireshark?

Have you checked the error codes on your message send call?

The line you linked to looks like a blank line between functions: https://github.com/ahundt/grl/blob/master/src/java/grl/src/grl/ZMQManager.java#L104

Perhaps you mean to put it somewhere like in the while loop here? https://github.com/ahundt/grl/blob/master/src/java/grl/src/grl/ZMQManager.java#L133

ashk-on commented 8 years ago

Are you talking about the sleep in Java code?

I tried removing the whole azmq stuff, and replaced it with zmq to avoid using io_service and other stuff.. if both sides are in zmq.dealer I can see data being sent and received in wireShark.. however it's a mess, and commands would be executed a long time after we have sent them...(if we stop sending messages from the robot, this works fine.. But when we send, it causes problems, probably to the messag queues ..)

Another thing I tried is to put a subscriber and a publisher socket on each port... In this configuration, the publisher on the robot side cannot send anything for some reason(or probably this publisher cannot connect to the subscriber on other side for some reason) (I should mention that I cannot bind the publisher on the robot port, can only connect it)

ashk-on commented 8 years ago

Do you have any idea on why the java code does not allow binding sockets to robot ip address? I used a couple of random ports, and it gave the error "Address already in use"...

ahundt commented 8 years ago

There are only 10 ports open on the robot as specified in the robot documentation. The one I usually use is one of them, try the one already specified in my code +1. Random ports won't work.

On Thursday, June 23, 2016, Ashkan372 notifications@github.com wrote:

Do you have any idea on why the java code does not allow binding sockets to robot ip address? I used a couple of random ports, and it gave the error "Address already in use"...

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/ahundt/grl/issues/100#issuecomment-228030719, or mute the thread https://github.com/notifications/unsubscribe/AADZwLM6LCSmRKys6LolEVtnwmxbn3tbks5qOndzgaJpZM4I513D .

Cheers! Andrew Hundt

ashk-on commented 8 years ago

I tried +1 +2 -1 -2 of your port, which is 30010, still cannot bind... How can I know about these 10 open ports? I looked at the documentation, and there was nothing..

ahundt commented 8 years ago

Email kuka then I don't have access to my docs right now. Though sadly they may take days to respond. Risto also has access to my copy of the kuka docs you could ask him to search then

On Thursday, June 23, 2016, Ashkan372 notifications@github.com wrote:

I tried +1 +2 -1 -2 of your port, which is 30010, still cannot bind... How can I know about these 10 open ports? I looked at the documentation, and there was nothing..

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/ahundt/grl/issues/100#issuecomment-228095904, or mute the thread https://github.com/notifications/unsubscribe/AADZwLoEPiYTWZm6nwEq68_G2LbC4mMDks5qOqwpgaJpZM4I513D .

Cheers! Andrew Hundt

ashk-on commented 8 years ago

oh... so the other approach would be to use this current dealer-dealer pattern... which kindaa works! the problem is that from the moment you start sending data back from robot, the messages are not sent from the computer to robot(in wire shark you would only see data being sent back, nothing from pc to robot)... however after a long wait(20 s) the command gets executed on the robot... (also I should mention that before sending messages from robot to pc, the messages were perfectly sent from pc to robot... they are stopped from sending, only when we start sending messages from the robot to pc) I tried using different ports for each direction... the problem is still there, so it might not have anything to do with zmq message queues.. .

ahundt commented 8 years ago

wow that sounds very strange indeed... so when you enabled robot->computer it interfered with computer->robot?

I don't know if it is relevant, but part of what I had done on the C++ side is disable asynchronous messages because there were problems with them so even if it says async, the messages are currently synchronous. Perhaps that could be related?

You can see the change here: https://github.com/ahundt/grl/blob/master/include/grl/AzmqFlatbuffer.hpp#L75

The fix for async_send is hopefully to modify it to use weak_ptr to fix the hanging problem. I'm not 100% sure why it wouldn't always send... https://github.com/zeromq/azmq/issues/99#issuecomment-130098105

ahundt commented 8 years ago

One of the best approaches I found was to implement a very small loop that just sends something, like an int or like in my test app a control point, then just have another thing sending back. Instead of a simplified C++ => C++ example you could create a super simple C++ => Java => C++ to eliminate most of the confounding variables and establish something simple but working that can be built on.

Hopefully that suggestion helps.

ashk-on commented 8 years ago

yes, we are doing it in a simple way as well, thank you for your suggestion.... now we found out that if we send the messages from the robot with less frequency, it works better (but it would be slow) ... we suspect network buffers are being filled on the pc side,we should some how clear them... or manage to send messages back synchronously..