Command Execution Delay UR5

[ehannigan]

We have been using the sawUniversalRobot library as the driver for our ur5. Another student had everything working a few months ago, but recently we have been having a problem with command delay. We are sending continuous commands at 50 Hz to /CartesianVelocityMove. The commands are all eventually executed, but as time goes on, the delay gets longer and longer. Sometimes the delay can be as much as 15 seconds.

I think it is important to also mention that the messages coming back to the computer are never delayed. The current position in rviz and the ur dashboard is always immediately updated.

Occasionally, restarting the ur5 and the computer will fix the problem for a little bit, but eventually it happens again. We think the problem is that there is a very large command queue somewhere that is storing every command we send and making sure each one is executed.

We have dug through the sawUniversalRobot code and found the publisher queue, but it seems to be of length 1 which should not accumulate any commands (see dig below). Are we looking the right spot? Does anyone have an suggestions of where this delay might be coming from or where else we could look in the code?

Dig through code to find queue:
Line 34 of universal_robot.cpp

sawUniversalRobot/ros/src/universal_robot.cpp

Lines 113 to 114 in eaefd30

	rosBridge->AddSubscriberToCommandWrite<prmVelocityCartesianSet, geometry_msgs::TwistStamped>
	("Component", "CartesianVelocityMove", "CartesianVelocityMove");

When we dig into rosBridge
in 757 of cisst_ros_bridge/mtsROSBridge.h
https://github.com/jhu-cisst/cisst-ros/blob/b8b124258678236ef2e89de93b363f2bbfa52139/cisst_ros_bridge/include/cisst_ros_bridge/mtsROSBridge.h#L756-L782
This line shows us what type the subscriber is:
mtsROSSubscriberWrite<_mtsType, _rosType> * newSubscriber = new mtsROSSubscriberWrite<_mtsType, _rosType>(topicName, *(this->Node));

The definition of mtsROSSubscriberWrite is in line 232 of cisst_ros_bridge/mtsROSBridge.h
https://github.com/jhu-cisst/cisst-ros/blob/b8b124258678236ef2e89de93b363f2bbfa52139/cisst_ros_bridge/include/cisst_ros_bridge/mtsROSBridge.h#L232-L254
This line shows us that the subscriber has a queue length of 1:
mSubscriber = node.subscribe(rosTopicName, 1, &ThisType::Callback, this)

Thanks for your help. Let me know what other information I should provide.

[ehannigan]

We shut everything down over the winter holiday. I just turned everything back on and all the problems are gone.... I'll comment again if the problems come back.

[ehannigan]

The problem has begun again. Restarting everything is not working. By running everything over port 3003, shouldn't we be able to command at 125 Hz? Has anyone else had this problem? Any hints of possible solutions are greatly appreciated.

[adeguet1]

Hello, I looked at the code and can find a few places in the ROS bridge that could be improved to reduce latency but nothing that would explain queuing and the latency becoming bigger and bigger. I might have time to improve the implementation in the coming months but for now, could you do a simple test? In the file mtsROSBridge.cpp, https://github.com/jhu-cisst/cisst-ros/blob/master/cisst_ros_bridge/src/mtsROSBridge.cpp#L105 Can you replace: if (mSpin) ros::spinOnce() By: if (mSpin) { for (size_t index = 0; index < 10; ++index) { ros::spinOnce(); } } I’m wondering if spinOnce dequeues all messages or only one per subscriber. Let us know if that help. Anton

…

--- Anton Deguet Associate Research Scientist www.lcsr.jhu.edu<http://www.lcsr.jhu.edu> - www.malonecenter.jhu.edu<http://www.malonecenter.jhu.edu> On Feb 6, 2020, at 11:34, Emily Hannigan <[email protected]<mailto:[email protected]>> wrote: The problem has begun again. Restarting everything is not working. By running everything over port 3003, shouldn't we be able to command at 125 Hz? Has anyone else had this problem? Any hints of possible solutions are greatly appreciated. — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub<#3?email_source=notifications&email_token=AAIRP46MIG7OSNW55COQ77LRBQ3ZVA5CNFSM4J6BIM62YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOEK7325Q#issuecomment-582991222>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/AAIRP47V5FXYXJLMBTHLUF3RBQ3ZVANCNFSM4J6BIM6Q>.

[ehannigan]

Thank you! Another student is using the ur5 this week, I will test this out next week and get back to you.

[ehannigan]

Hey, finally got the ur5 back. I changed the line in mtsROSBridge.cpp and then followed the build instructions again.

# make sure we are in the right place
cd ~/cisst_ws
# make sure you have the proper ROS environment variables
source /opt/ros/kinetic/setup.bash  # or whatever your ROS distribution is
# set default CMake build type
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
# build
catkin build
# set environment variables - MAKE SURE YOU SOURCE THE RIGHT FILE
source devel/setup.bash

The build succeeded:

[build] Summary: All 45 packages succeeded!
[build] Ignored: None.
[build] Warnings: 40 packages succeeded with warnings.
[build] Abandoned: None.
[build] Failed: None.
[build] Runtime: 11 minutes and 47.3 seconds total.

I then ran

cd ~/teleoperation_ws
source cisst_ws/devel_release/setup.py
catkin_make
source devel/setup.bash

It does not seem to have changed the lag when doing marker control with rviz. I will continue to monitor the problem. Let me know if I made any mistakes in the re building process.