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)