def wait_and_kill(self, sec):
self.wait2(sec)
if self.returncode is None:
eprint(
self.args[0] + " TERM signal didn't worked, escalating to KILL"
)
self.send_signal(signal.SIGKILL)
def send_serial(self, message_type, data):
if self.msp_dict[message_type][0]==0:
data_raw_chr=''
else:
data_raw_chr=toraw_data(data, self.msp_dict[message_type][2])
#print "Data raw chr", data_raw_chr
data_raw=list(struct.unpack('B'*self.msp_dict[message_type][0], data_raw_chr))
#print "Data raw", data_raw
cmd_data_size=self.msp_dict[message_type][0]
#print "Cmd data size", cmd_data_size
if len(data_raw) != cmd_data_size:
eprint("Error: data size incorrect for this message type. MSG type: ", message_type, " data size:", len(data), " expected data size:", cmd_data_size)
return(-1)
if message_type not in self.msp_dict:
eprint("Invalid message: ", message_type)
return(-1)
#data_raw=[]
# for i in data:
# data_raw+=[data[i] & 0xff, (data[i] >> 8) & 0xff]
if len(data_raw)>0:
data_raw_chr=reduce(lambda x,y:x+y, map(chr, data_raw))
else:
data_raw_chr=''
checksum=(reduce(lambda x,y:x^y,data_raw+[message_type]+[cmd_data_size]) & 0xff)
ser_data=self.HEADER+chr(cmd_data_size)+chr(message_type)+data_raw_chr+chr(checksum)
#print "Data: ", data
#print "data_raw: ", data_raw
#print "Checksum: ", hex(checksum)
#print "data hex", map(hex,data_raw)
#print "ser_data", map(str,ser_data)
#print "ser_data_hex", map(ord,ser_data)
self.ser.write(ser_data)
def start(self):
for process_args in self.processes_args:
filename=which(process_args[0])
if filename and os.path.exists(filename) and os.access(filename, os.X_OK):
dprint("Starting process: ", process_args[0])
self.processes.append(MyPopen([filename]+process_args[1:]))
self.processes[-1].args=process_args
else:
eprint("Executable", process_args[0], " not found, not starting process")
def monitor(self):
stop=False
while not stop:
for process in self.processes:
process.poll()
if process.returncode != None:
eprint("Process: ", process.args[0], " died! Closing all processes")
stop=True
sleep(0.5)
#dprint("Looping")
if stop:
self.stop()
def monitor(self):
stop = False
while not stop:
for process in self.processes:
process.poll()
if process.returncode is not None:
eprint(
'Process: ', process.args[0],
' died! Closing all processes')
stop = True
sleep(0.5)
# dprint('Looping')
if stop:
self.stop()
def approach_until_touch(hand_handle, finger, arm_handle, force_handle, touch_point, force_threshold, goal_precision, speed, start_pose, sim, force_factor, extra_tool=identity(4)):
filters_order=3
filters_freq=0.2
max_noise_force, max_noise_norm_force=find_force_noise_levels(force_handle, finger, filter_order=filters_order, filter_freq=filters_freq, measurements=50)
print "noise levels", max_noise_force, max_noise_norm_force
#if False:
if not sim:
force_threshold=max_noise_norm_force*1.3
#raw_input()
force=array([0.]*3)
init_time=time.time()
old_time=init_time
first=True
force_filter=Filter_vector(dim=3, order=filters_order, freq=filters_freq)
while norm(force)<force_threshold:
cur_time=time.time()
period=cur_time-old_time
old_time=cur_time
data=force_handle.get_data()[finger]
print "data", data, force_factor
force=array(data[-3:])*force_factor
force=force_filter.filter(force)
eprint("FORCE: ", norm(force), "Threshold", force_threshold)
cur_pose=quat.to_matrix_fixed(data[3:7], r=data[:3])
if first:
vel_vector=(touch_point[:2]-start_pose[:2,3])
vel_vector/=norm(vel_vector)
first=False
pose=start_pose
step=vel_vector*speed*period
pose[:2,3]+=step
move_robot(hand_handle, finger, arm_handle, pose, goal_precision, wait=0.1, extra_tool=extra_tool)
print "Pos", cur_pose, "Force", force
#stop robot
dprint("STop movement")
cur_pose=quat.to_matrix_fixed(data[3:7], r=data[:3])
cur_pose[:3,:3]=pose[:3,:3]
move_robot(hand_handle, finger, arm_handle, cur_pose, goal_precision, extra_tool=extra_tool)
def send_serial(self, message_type, data):
if self.msp_dict[message_type][0] == 0:
data_raw_chr = ''
else:
data_raw_chr = toraw_data(data, self.msp_dict[message_type][2])
#print "Data raw chr", data_raw_chr
data_raw = list(
struct.unpack('B' * self.msp_dict[message_type][0], data_raw_chr))
#print "Data raw", data_raw
cmd_data_size = self.msp_dict[message_type][0]
#print "Cmd data size", cmd_data_size
if len(data_raw) != cmd_data_size:
eprint(
"Error: data size incorrect for this message type. MSG type: ",
message_type, " data size:", len(data), " expected data size:",
cmd_data_size)
return (-1)
if message_type not in self.msp_dict:
eprint("Invalid message: ", message_type)
return (-1)
#data_raw=[]
# for i in data:
# data_raw+=[data[i] & 0xff, (data[i] >> 8) & 0xff]
if len(data_raw) > 0:
data_raw_chr = reduce(lambda x, y: x + y, map(chr, data_raw))
else:
data_raw_chr = ''
checksum = (reduce(lambda x, y: x ^ y,
data_raw + [message_type] + [cmd_data_size]) & 0xff)
ser_data = self.HEADER + chr(cmd_data_size) + chr(
message_type) + data_raw_chr + chr(checksum)
#print "Data: ", data
#print "data_raw: ", data_raw
#print "Checksum: ", hex(checksum)
#print "data hex", map(hex,data_raw)
#print "ser_data", map(str,ser_data)
#print "ser_data_hex", map(ord,ser_data)
self.ser.write(ser_data)
def rc_cmd(self, cmd):
self.send_serial(self.MSP_SET_RAW_RC, cmd)
error,cmd_resp=self.recv_serial()
if error!=0:
eprint("RC command didn't work!")
return(error, cmd_resp)
def recv_serial(self):
data_to_read=1
state=self.S_HEADER
checksum=0
input_packet=''
while (state!=self.S_END) and (state!=self.S_ERROR):
#print "in waiting", self.ser.inWaiting()
#print "Data to read", data_to_read
c=self.ser.read(data_to_read)
input_packet+=c
#print "c: ", map(str,c), map(ord,c)
if state==self.S_HEADER:
if c=='$':
c=self.ser.read(2)
input_packet+=c
#print "c: ", map(str,c), map(ord,c)
if c=='M>':
#print "header received"
state=self.S_SIZE
else:
print "Error in header1"
state=self.S_ERROR
else:
print "Error: No header received"
state=self.S_ERROR
elif state==self.S_SIZE:
data_size=c
checksum^=ord(data_size)
state=self.S_CMD
#print "Data size: ", ord(data_size)
elif state==self.S_CMD:
cmd=c
checksum^=ord(cmd)
if ord(data_size)==0:
data_to_read=1
state=self.S_CHECKSUM
data_raw=''
else:
data_to_read=ord(data_size)
state=self.S_DATA
#print "Command: ", ord(cmd)
elif state==self.S_DATA:
data_raw=c
#print "Data received", map(ord,data_raw)
checksum^=reduce(lambda x,y:x^y, map(ord,data_raw))
state=self.S_CHECKSUM
data_to_read=1
elif state==self.S_CHECKSUM:
#print "Calculated checksum: ", checksum, " Received checksum: ", ord(c)
if checksum!=ord(c):
eprint("Error in checksum")
state=self.S_ERROR
else:
#print "Checksum is right"
state=self.S_END
if state==self.S_END:
error=0
if len(data_raw)!= self.msp_dict[ord(cmd)][1]:
eprint("Error: incorrect response size! Got:", len(data_raw), " should be:", self.msp_dict[ord(cmd)][1])
return((-1,[]))
#print "Cmd", ord(cmd)
#print "Raw data", data_raw, "test"
if self.msp_dict[ord(cmd)][1]!=0:
data=unraw_data(data_raw, self.msp_dict[ord(cmd)][3])
if len(self.msp_dict[ord(cmd)])>=5:
data=[i*mult for i, mult in zip(self.msp_dict[ord(cmd)][4], data)]
else:
data=[]
#os.write(master,input_packet)
else: #error
eprint("input_packet: ", input_packet, map(ord, input_packet))
data=[]
cmd=chr(0)
error=1
self.ser.flushInput()
return((error,[ord(cmd),data]))
def get_status(self):
self.send_serial(self.MSP_STATUS, [])
error, cmd_resp = self.recv_serial()
if error != 0:
eprint("STATUS didn't work!")
return (error, cmd_resp)
def get_altitude(self):
self.send_serial(self.MSP_ALTITUDE, [])
error, cmd_resp = self.recv_serial()
if error != 0:
eprint("RC ALTITUDE didn't work!")
return (error, cmd_resp)
def get_altitude(self):
self.send_serial(self.MSP_ALTITUDE, [])
error, cmd_resp=self.recv_serial()
if error!=0:
eprint("RC ALTITUDE didn't work!")
return(error, cmd_resp)
def rc_cmd(self, cmd):
self.send_serial(self.MSP_SET_RAW_RC, cmd)
error, cmd_resp = self.recv_serial()
if error != 0:
eprint("RC command didn't work!")
return (error, cmd_resp)
def recv_serial(self):
data_to_read = 1
state = self.S_HEADER
checksum = 0
input_packet = ''
while (state != self.S_END) and (state != self.S_ERROR):
#print "in waiting", self.ser.inWaiting()
#print "Data to read", data_to_read
c = self.ser.read(data_to_read)
input_packet += c
#print "c: ", map(str,c), map(ord,c)
if state == self.S_HEADER:
if c == '$':
c = self.ser.read(2)
input_packet += c
#print "c: ", map(str,c), map(ord,c)
if c == 'M>':
#print "header received"
state = self.S_SIZE
else:
print "Error in header1"
state = self.S_ERROR
else:
print "Error: No header received"
state = self.S_ERROR
elif state == self.S_SIZE:
data_size = c
checksum ^= ord(data_size)
state = self.S_CMD
#print "Data size: ", ord(data_size)
elif state == self.S_CMD:
cmd = c
checksum ^= ord(cmd)
if ord(data_size) == 0:
data_to_read = 1
state = self.S_CHECKSUM
data_raw = ''
else:
data_to_read = ord(data_size)
state = self.S_DATA
#print "Command: ", ord(cmd)
elif state == self.S_DATA:
data_raw = c
#print "Data received", map(ord,data_raw)
checksum ^= reduce(lambda x, y: x ^ y, map(ord, data_raw))
state = self.S_CHECKSUM
data_to_read = 1
elif state == self.S_CHECKSUM:
#print "Calculated checksum: ", checksum, " Received checksum: ", ord(c)
if checksum != ord(c):
eprint("Error in checksum")
state = self.S_ERROR
else:
#print "Checksum is right"
state = self.S_END
if state == self.S_END:
error = 0
if len(data_raw) != self.msp_dict[ord(cmd)][1]:
eprint("Error: incorrect response size! Got:", len(data_raw),
" should be:", self.msp_dict[ord(cmd)][1])
return ((-1, []))
#print "Cmd", ord(cmd)
#print "Raw data", data_raw, "test"
if self.msp_dict[ord(cmd)][1] != 0:
data = unraw_data(data_raw, self.msp_dict[ord(cmd)][3])
if len(self.msp_dict[ord(cmd)]) >= 5:
data = [
i * mult
for i, mult in zip(self.msp_dict[ord(cmd)][4], data)
]
else:
data = []
#os.write(master,input_packet)
else: #error
eprint("input_packet: ", input_packet, map(ord, input_packet))
data = []
cmd = chr(0)
error = 1
self.ser.flushInput()
return ((error, [ord(cmd), data]))
def control(self, cmd):
self.send_serial(self.MSP_CONTROL, cmd)
error,cmd_resp=self.recv_serial()
if error!=0:
eprint("CONTROL didn't work!")
return(error, cmd_resp)
def rc_read(self):
self.send_serial(self.MSP_RC, [])
error, cmd_resp=self.recv_serial()
if error!=0:
eprint("RC read didn't work!")
return(error, cmd_resp)
def control(self, cmd):
self.send_serial(self.MSP_CONTROL, cmd)
error, cmd_resp = self.recv_serial()
if error != 0:
eprint("CONTROL didn't work!")
return (error, cmd_resp)
def get_status(self):
self.send_serial(self.MSP_STATUS, [])
error, cmd_resp=self.recv_serial()
if error!=0:
eprint("STATUS didn't work!")
return(error, cmd_resp)
def rc_read(self):
self.send_serial(self.MSP_RC, [])
error, cmd_resp = self.recv_serial()
if error != 0:
eprint("RC read didn't work!")
return (error, cmd_resp)
