def __init__(self):
self.serial_port = rospy.get_param("serial_port", "ttyACM0")
self.can_id = rospy.get_param("can_id", 1)
self.controller_rate = rospy.get_param("controller_rate", 20)
self.seed = SeedCommand(self.serial_port, 115200)
self.seed.COM_Open()
self.seed.Script_Go(self.can_id, 1) #Initialize
signal = 0
while (signal != 0xFF): #wait for initialize
response = self.seed.CAN_Read()
signal = int(response[13] + response[14], 16)
class SeedController:
def __init__(self):
self.serial_port = rospy.get_param("serial_port", "ttyACM0")
self.can_id = rospy.get_param("can_id", 1)
self.controller_rate = rospy.get_param("controller_rate", 20)
self.seed = SeedCommand(self.serial_port, 115200)
self.seed.COM_Open()
self.seed.Script_Go(self.can_id, 1) #Initialize
signal = 0
while (signal != 0xFF): #wait for initialize
response = self.seed.CAN_Read()
signal = int(response[13] + response[14], 16)
def hand_controller(self, req):
if ((req.command == "grasp") or (req.command == "open")):
if (req.command == "grasp"):
self.seed.Script_Go(self.can_id, 2)
elif (req.command == "open"):
self.seed.Script_Go(self.can_id, 3)
return HandControllerResponse("succeeded")
else:
return HandControllerResponse("failed")
def get_position(self):
position = self.seed.Get_Pos(self.can_id)[1] #pulse
if (position != "None"):
if (position <= 0):
joint_states.position[0] = 0
joint_states.position[1] = 0
joint_states.position[2] = 0
joint_states.position[3] = 0
elif (position <= 52000):
joint_states.position[0] = position * 1.2 / 52000
joint_states.position[1] = 0
joint_states.position[2] = 0
joint_states.position[3] = position * -2.0 / 52000
elif (position <= 60000):
joint_states.position[0] = 1.2
joint_states.position[1] = (position - 52000) * 0.8 / 8000
joint_states.position[2] = (position - 52000) * 0.8 / 8000
joint_states.position[3] = -2.0
else:
joint_states.position[0] = 1.2
joint_states.position[1] = 0.8
joint_states.position[2] = 0.8
joint_states.position[3] = -2.0
joint_states.header.stamp = rospy.Time.now()
pub.publish(joint_states)
rospy.Rate(self.controller_rate).sleep()
#!/usr/bin/env python
from struct import *
import rospy
import time
import array
import math
import curses
import threading
from seed_command import SeedCommand
from std_msgs.msg import *
ID = 1
seed = SeedCommand("/dev/tilt_motor", 57413)
mutex = threading.Lock()
SPD = 1000
def command_subscriber(data):
pulse = data.data * 90.62083333333334
mutex.acquire()
try:
seed.SEED_DX_MOV(ID, SPD, int(round(pulse)))
finally:
mutex.release()
self.POS[11] = (self.POS[11] - 32768.0) * fz_scale / 32768.0
self.POS[12] = (self.POS[12] - 32768.0) * tx_scale / 32768.0
self.POS[13] = (self.POS[13] - 32768.0) * ty_scale / 32768.0
self.POS[14] = (self.POS[14] - 32768.0) * tz_scale / 32768.0
self.POS[8] = 21112
#----------- main -----------------#
if __name__ == "__main__":
if len(sys.argv) > 1:
port = sys.argv[1]
else:
port = ("aero_upper")
print "port %s" % port
seed = SeedCommand(port, 1000000)
aero = Aero()
# Buffer Clear
seed.ser.flushInput() #flush input buffer
seed.ser.flushOutput() #flush output buffer
# Curses start
stdscr = curses.initscr()
curses.noecho()
curses.cbreak()
seed.AERO_Snd_Servo_all(0)
time.sleep(1)
import sys
import os
import time
R_HAND_Y = 13
L_HAND_Y = 28
time.sleep(30.0) # wait controller launch
pid = os.getpid()
launch_killed_before_check = True # a quick kill may indicate something unusual, don't send commands
print '---ready for safe exit'
while True:
status = int(subprocess.check_output("ps aux | grep aero_ros_controller | wc -l", shell=True)[:-1])
print '---check safe once %d' % status
if status < 3:
print '---detected controller kill!'
if not launch_killed_before_check: # init pose at end if controller was correctly launched
seed = SeedCommand('aero_upper', 1000000)
seed.AERO_Snd_Position(R_HAND_Y, 100, 0) #ID, time[*10msec], stroke[*0.01mm]
seed.AERO_Snd_Position(L_HAND_Y, 100, 0) #ID, time[*10msec], stroke[*0.01mm]
break
else:
launch_killed_before_check = False
time.sleep(2.0)
subprocess.check_output("kill " + str(pid), shell=True)
sys.exit()
pub = rospy.Publisher('aero_ft', Wrench, queue_size = 10)
rate = rospy.Rate(10) #10hz
# force torque reading scales. Specific to each ft sensor.
# Parameters for FT sensor 91155 (Tepura label "A")
# obtained from querying sensor through CAN-to-USB adaptor
# on 2016-12-15
fx_scale = 1466.0
fy_scale = 1651.0
fz_scale = 1994.0
tx_scale = 31.0
ty_scale = 33.0
tz_scale = 21.0
if os.path.islink('/dev/aero_upper'):
seed = SeedCommand("aero_upper",1000000)
aero=Aero()
# Buffer Clear
seed.ser.flushInput() #flush input buffer
seed.ser.flushOutput() #flush output buffer
time.sleep(1)
else:
seed = -1
print('Link /dev/aero_upper does not exist. Running dummy Aero ft publisher instead.')
print("Start publishing FT")
while not rospy.is_shutdown():
res = AeroFTSensorResponse()
res.wrench = wrench
prev_wrench = wrench
return res
#----------- main -----------------#
if __name__ == "__main__":
rospy.init_node('aero_ft_sensor_service_server', anonymous=True)
s = rospy.Service('aero_ft_sensor_service', AeroFTSensor, callback)
if os.path.islink('/dev/aero_upper'):
seed = SeedCommand("aero_upper", 1000000)
aero = Aero()
# Buffer Clear
seed.ser.flushInput() #flush input buffer
seed.ser.flushOutput() #flush output buffer
# seed.AERO_Snd_Servo_all(0);
time.sleep(1)
print "AeroFTServiceServer ready."
else:
seed = -1
print(
#!/usr/bin/env python
from struct import *
import time
import array
import math
import curses
from seed_command import SeedCommand
#----------- main -----------------#
if __name__ == "__main__":
seed = SeedCommand(0, 57143)
ID = seed.SEED_DX_PING()
if (ID == None):
print "False"
else:
seed.SEED_DX_MODE(ID, 4095, 4095)
print "Success!"
#!/usr/bin/env python
from struct import *
import time
import array
import math
import curses
from seed_command import SeedCommand
ID = 1
#----------- main -----------------#
if __name__ == "__main__":
seed = SeedCommand(0, 57143)
seed.SEED_DX_SRV(ID, 1)
time.sleep(0.5)
seed.SEED_DX_MOV(ID, 1000, 4095 * 1)