Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Adjust critical sections

Browse Source 
Optimised the used of the the enter_critical and exit_critical sections
This commit is contained in:
Rune Søe-Knudsen
2019-06-24 12:04:17 +02:00
committed by Felix Mauch
parent 792c621f82
commit bd4df1ff5e
1 changed files with 4 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
ur_rtde_driver/resources/servoj.urscript
View File

@@ -12,19 +12,15 @@ global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_q = get_actual_joint_positions() global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions() global cmd_servo_q_last = get_actual_joint_positions()
def set_servo_setpoint(q): def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING cmd_servo_state = SERVO_RUNNING
cmd_servo_q_last = cmd_servo_q cmd_servo_q_last = cmd_servo_q
cmd_servo_q = q cmd_servo_q = q
exit_critical
end end
def extrapolate(): def extrapolate():
enter_critical
diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]] diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
cmd_servo_q_last = cmd_servo_q cmd_servo_q_last = cmd_servo_q
cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]] cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
exit_critical
return cmd_servo_q return cmd_servo_q
end end
@@ -42,7 +38,6 @@ thread servoThread():
if cmd_servo_state > SERVO_UNINITIALIZED: if cmd_servo_state > SERVO_UNINITIALIZED:
cmd_servo_state = SERVO_IDLE cmd_servo_state = SERVO_IDLE
end end
exit_critical
if do_extrapolate: if do_extrapolate:
textmsg("No new setpoint received. Extrapolating.") textmsg("No new setpoint received. Extrapolating.")
q = extrapolate() q = extrapolate()
@@ -52,6 +47,7 @@ thread servoThread():
else: else:
sync() sync()
end end
exit_critical
end end
textmsg("servo thread ended") textmsg("servo thread ended")
stopj(0.1) stopj(0.1)
@@ -63,6 +59,7 @@ keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket") params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7] keepalive = params_mult[7]
while keepalive > 0: while keepalive > 0:
enter_critical
socket_send_line(1, "reverse_socket") socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
if params_mult[0] > 0: if params_mult[0] > 0:
@@ -72,11 +69,10 @@ while keepalive > 0:
else: else:
keepalive = keepalive - 1 keepalive = keepalive - 1
end end
exit_critical
end end
textmsg("Stopping communication and servoing") textmsg("Stopping communication and servoing")
enter_critical
cmd_servo_state = SERVO_STOPPED cmd_servo_state = SERVO_STOPPED
exit_critical
sleep(.1) sleep(.1)
socket_close() socket_close()
kill thread_servo kill thread_servo