mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
63 lines
1.5 KiB
Plaintext
63 lines
1.5 KiB
Plaintext
{{BEGIN_REPLACE}}
|
|
|
|
global steptime = get_steptime()
|
|
global speedj_time = steptime - 0.1*steptime
|
|
textmsg("steptime=", steptime)
|
|
MULT_jointstate = {{JOINT_STATE_REPLACE}}
|
|
|
|
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|
cmd_speedj_active = True
|
|
keepalive = -2
|
|
|
|
def set_speed(qd):
|
|
enter_critical
|
|
cmd_servo_qd = qd
|
|
cmd_speedj_active = True
|
|
exit_critical
|
|
end
|
|
|
|
thread speedThread():
|
|
while True:
|
|
enter_critical
|
|
qd = cmd_servo_qd
|
|
exit_critical
|
|
#textmsg("loop")
|
|
if cmd_speedj_active:
|
|
# Having this shortly under steptime seems to help
|
|
speedj(qd, 40.0, speedj_time)
|
|
else:
|
|
stopj(5.0)
|
|
end
|
|
sync()
|
|
end
|
|
stopj(5.0)
|
|
end
|
|
|
|
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
|
|
|
|
thread_speed = run speedThread()
|
|
|
|
params_mult = socket_read_binary_integer(1+6, "reverse_socket")
|
|
keepalive = params_mult[1]
|
|
while keepalive > 0:
|
|
params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02)
|
|
if params_mult[0] > 0:
|
|
keepalive = params_mult[1]
|
|
if keepalive > 1:
|
|
qd = [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, params_mult[7] / MULT_jointstate]
|
|
set_speed(qd)
|
|
#speedj(qd, 40.0, 0.007)
|
|
else:
|
|
cmd_speedj_active = False
|
|
keepalive = 1
|
|
end
|
|
else:
|
|
textmsg("Missing package from remote pc")
|
|
keepalive = keepalive - 1
|
|
end
|
|
end
|
|
sleep(.1)
|
|
socket_close()
|
|
kill thread_speed
|
|
|