From e7c97150d5ab060a940447d65454f00e951f8460 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 8 May 2019 08:46:30 +0200 Subject: [PATCH] notify on missing packages --- .../ur_rtde_driver/comm/reverse_interface.h | 3 ++- .../src/ros/hardware_interface_node.cpp | 6 ++++-- ur_rtde_driver/src/ur/ur_driver.cpp | 21 ++++++++++--------- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h index 18dfc3c..272250a 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h @@ -65,7 +65,8 @@ public: b_pos += append(b_pos, val); } - int32_t val = htobe32(1); + // TODO: Make this a parameter: Number of allowed missed packages + int32_t val = htobe32(5); append(b_pos, val); size_t written; diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp index 8944407..d936a4e 100644 --- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp @@ -78,8 +78,10 @@ int main(int argc, char** argv) if (!control_rate.sleep()) { - // ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << - // "ms"); + ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << + "ms"); + ROS_WARN_STREAM("Actual cycle time:" << control_rate.cycleTime().toNSec() / 1000000.0 << + "ms"); } } diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 1a18f7b..95543bf 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -73,27 +73,28 @@ def myProg(): sync() elif state == SERVO_RUNNING: servoj(q, {{SERVO_J_REPLACE}}) - servoj(q, t=0.008, lookahead_time=0.03, gain=750) else: sync() end end end - socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}) + socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") thread_servo = run servoThread() keepalive = -2 - params_mult = socket_read_binary_integer(6+1) + params_mult = socket_read_binary_integer(6+1, "reverse_socket") keepalive = params_mult[7] while keepalive > 0: - params_mult = socket_read_binary_integer(6+1) - keepalive = params_mult[7] - if keepalive > 0: - if params_mult[0] > 0: - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - set_servo_setpoint(q) - end + params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.002) + if params_mult[0] > 0: + keepalive = params_mult[7] + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + set_servo_setpoint(q) + else: + # TODO: Extrapolation goes here + keepalive = keepalive - 1 end + sync() end sleep(.1) socket_close()