def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print ' Select field to plot...' comp=mcf.create_component(name) proxy.register_log_component(comp) field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data=[] for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) data.append(v) # m3t.plot(data) m3t.mplot(data,range(len(data)))
def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print ' Select field to plot...' comp = mcf.create_component(name) proxy.register_log_component(comp) field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data = [] for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) data.append(v) # m3t.plot(data) m3t.mplot(data, range(len(data)))
def get_field(self, comp): field = m3t.user_select_msg_field(comp.status) self.idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): self.repeated = True print 'Select index of repeated fields to monitor: [0]' self.idx = m3t.get_int(0) return field
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) print 'Num samples available: ', ns if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) comp = mcf.create_component(name) proxy.register_log_component(comp) scope = m3t.M3Scope(xwidth=ns - 1) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print 'Select field to monitor' field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) scope.plot(v) time.sleep(0.05)
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) print 'Num samples available: ',ns if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) comp=mcf.create_component(name) proxy.register_log_component(comp) scope=m3t.M3Scope(xwidth=ns-1) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print 'Select field to monitor' field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) scope.plot(v) time.sleep(0.05)
def run(self): try: rospy.loginfo("Setting up ROS publishers on parent scope: " + str(self.scope)) rospy.loginfo("Publisher details: " + str(self.component.name) + "/" + str(self.field) + " with type " + str(self.dataType) + " and rate " + str(self.rate)) try: dt = self.dataType if dt == "WrenchStamped": dt = WrenchStamped else: dt = Floats except AttributeError: dt = Floats except IndexError: dt = Floats self.publisher = rospy.Publisher(str(self.scope) + "/" + str(self.component.name) + "/" + str(self.field), dt, queue_size=1) self.ros_rate = rospy.Rate(self.rate) self.running = True while self.running and not rospy.is_shutdown(): try: PublisherThread.PublisherLock.acquire() tmp = m3t.get_msg_field_value(self.component.status, self.field) PublisherThread.PublisherLock.release() if dt == WrenchStamped: float_list = map(float, str(tmp).strip('[]').split(',')) msg = WrenchStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = str(self.component.name) msg.wrench.force.x = float_list[0]/1000 msg.wrench.force.y = float_list[1]/1000 msg.wrench.force.z = float_list[2]/1000 msg.wrench.torque.x = float_list[3]/1000 msg.wrench.torque.y = float_list[4]/1000 msg.wrench.torque.z = float_list[5]/1000 elif dt == Floats: msg = Floats() if hasattr(tmp, '__len__'): for val in tmp: msg.data.append(val) else: msg.data.append(tmp) else: rospy.logerr("unknown Data type " + dt) self.publisher.publish(msg) self.ros_rate.sleep() except IndexError: rospy.logerr("IndexError caught! Skipping publishing...") except ValueError: rospy.logerr("ValueError caught! Skipping publishing...") except ROSException as e: rospy.logerr("ROS exception caught! " + str(e)) except: e = sys.exc_info()[0] rospy.logerr("Exception caught! " + str(e))
def step(self): self.proxy.step() for c in self.actuator_ec: if self.do_scope and self.scope is None: self.scope=m3t.M3Scope2(xwidth=100,yrange=None) self.status_dict=self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) c.command.mode=int(self.mode[idx]) if self.mode[idx]==mec.ACTUATOR_EC_MODE_PWM: c.command.pwm_desired=int(self.pwm_desire[idx]) print 'Desired',c.name,c.command.pwm_desired if self.mode[idx]==mec.ACTUATOR_EC_MODE_CURRENT: c.command.current_desired=int(self.current_desire[idx]) print 'Desired',c.name,c.command.current_desired if self.do_scope and self.scope is not None: f1=self.scope_keys[self.scope_field1[0]] f2=self.scope_keys[self.scope_field2[0]] x1=x2=None if f1!='None' and f1!='base': x1=m3t.get_msg_field_value(self.actuator_ec[0].status,f1) print f1,':',x1 if f2!='None' and f2!='base': x2=m3t.get_msg_field_value(self.actuator_ec[0].status,f2) print f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' if (self.save and not self.save_last): c.write_config() self.save_last=self.save
proxy.publish_command(c) field=m3t.user_select_msg_field(comps[0].status) print 'Number of samples [100]?' ns=m3t.get_int(100) log=[] for i in range(len(comps)): log.append([]) try: ts=time.time() for i in range(ns): proxy.step() idx=0 print '---------' for c in comps: v=m3t.get_msg_field_value(c.status,field) log[idx].append(v) idx=idx+1 print i,':',v time.sleep(0.05) except (KeyboardInterrupt,EOFError): pass proxy.stop(force_safeop=False) #allow other clients to continue running print log res={} p2ps=[] for i in range(len(comps)): a=na.array(log[i],na.Float32) avg=float(a.mean()) std=a.std() p2p=max(a)-min(a)
def do_dump(proxy): print 'Starting dump...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' comp = mcf.create_component(name) proxy.register_log_component(comp) fields = [] print 'Dump all data?[n]' if m3t.get_yes_no('n'): '''print 'Fetching data...' fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200)) f.close()''' fields = comp.status.DESCRIPTOR.fields_by_name.keys() print fields else: print ' Select fields to plot...' while True: fields.append(m3t.user_select_msg_field(comp.status)) print 'Another field [n]?' if not m3t.get_yes_no('n'): break print 'Fetching data...' data = {} for k in fields: data[k] = [] print '---------------------------' print k print m3t.get_msg_field_value(comp.status, k) print dir(m3t.get_msg_field_value(comp.status, k)) if hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k].append([]) for i in range(start, end): proxy.load_log_sample(logname, i) for k in fields: repeated = False # skip protobuf subclasses (like base) for now. note we'll want this for humanoid if hasattr(m3t.get_msg_field_value(comp.status, k), '__metaclass__'): pass elif hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k][j].append( m3t.get_msg_field_value(comp.status, k)[j]) else: data[k].append(m3t.get_msg_field_value(comp.status, k)) fn = m3t.get_m3_log_path() + logname + '/' + logname + '_dump.yaml' f = file(fn, 'w') print 'Saving...', fn print data f.write(yaml.safe_dump(data, default_flow_style=False, width=200)) f.close()
print 'Select Y-Range? [n]' yrange=None if m3t.get_yes_no('n'): yrange=[] print 'Min?' yrange.append(m3t.get_int()) print 'Max?' yrange.append(m3t.get_int()) name=comps[cid] comp=mcf.create_component(name) proxy.subscribe_status(comp) #proxy.publish_param(comp) field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) scope=m3t.M3Scope(xwidth=100,yrange=yrange) try: ts=time.time() while True: proxy.step() if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) scope.plot(v)
proxy.subscribe_status(c) log[c.name]={} for f in fields: for x in f: log[c.name][x]=[] proxy.step() time.sleep(0.5) proxy.step() # ########################### ns=30 for i in range(ns): proxy.step() print '---------' for c in comp_ec: for s in sensor: v=m3t.get_msg_field_value(c.status,s) log[c.name][s].append(v) print c.name,':',s,':',v time.sleep(0.05) print log # ########################### for idx in range(len(comp_ec)): print '--------',comp_ec[idx].name,'---------' c=comp_ec[idx] rt=comp_rt[idx] if sensor[0]=='adc_current_a': avg_a=float(na.array(log[c.name]['adc_current_a'][6:],na.Float32).mean()) avg_b=float(na.array(log[c.name]['adc_current_b'][6:],na.Float32).mean()) current=M3CurrentSensor(rt.config['calib']['current']['type']) if rt.config['calib']['current']['type']=='adc_linear_5V' or rt.config['calib']['current']['type']=='adc_linear_5V_ns': io=current.raw_2_mA(rt.config['calib']['current'],avg_a,avg_b)
def get_values(self, req): """ Callback for the get_values service request """ rospy.loginfo("Requesting values for " + str(req.component) +" " + str(req.field) + " with " + str(req.hz) + " Hz.") values = [] retries = 3 while retries > 0: try: if req.component not in self.comps.keys(): comp = mcf.create_component(req.component) self.rt_proxy.subscribe_status(comp) self.comps[req.component] = comp break except (CannotSendRequest,ResponseNotReady): rospy.logwarn( "Response not ready, retrying. Retries remaining: " + str(retries)) retries -= 1 time.sleep(1.0) if retries == 0: print "Retries exausted, returning.." return while self.comps[req.component].status.base.timestamp == 0: #wait for status to be ready time.sleep(0.2) rt_field_vals = m3t.get_msg_field_value(self.comps[req.component].status, req.field) if hasattr(rt_field_vals, '__len__'): for val in rt_field_vals: values.append(str(val)) else: values.append(str(rt_field_vals)) resp = RequestValuesResponse() resp.values = values if (req.component, req.field, req.datatype) not in self.publishers.keys(): rospy.loginfo("Adding "+ str(req.hz)+ " Hz publisher thread for " + str((req.component, req.field)) + "...") t = PublisherThread(self.scope, self.comps[req.component], req.field, req.datatype, req.hz) t.start() timeout = 0 while not t.running and timeout <=5: time.sleep(1) #waiting timeout += 1 if t.running: with self.lock: self.publishers[req.component, req.field, req.datatype] = t self.set_max_rate() rospy.loginfo("..done!") else: rospy.logerr("Something went wrong, publisher not created") else: rospy.loginfo("publisher already exists") if req.hz != self.publishers[req.component, req.field, req.datatype].rate: rospy.loginfo("adjusting rate...") self.publishers[req.component, req.field, req.datatype].set_hz(req.hz) self.set_max_rate() return resp
def step(self): # print self.comps['act_ec']['comp'].status.timestamp if self.do_scope and self.scope is None: self.scope=m3t.M3Scope2(xwidth=100,yrange=None) self.proxy.step() self.cnt=self.cnt+1 self.status_dict=self.proxy.get_status_dict() if self.zero_joint_theta and not self.zero_joint_theta_last: self.joint_theta -= self.act.get_joint_theta() print 'New joint_theta zero',self.joint_theta if self.zero_joint_torque and not self.zero_joint_torque_last: self.joint_torque -= self.act.get_joint_torque() print 'New joint_torque zero',self.joint_torque # # if self.zero_joint_torque_lc and not self.zero_joint_torque_lc_last: # self.joint_torque_lc -= self.act.get_joint_torque_lc() # print 'New joint_torque_lc zero',self.joint_torque_lc # # self.param_dict[self.comp_name]['calibration']['zero_motor_theta'] = self.motor_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_theta'] = self.joint_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_torque'] = self.joint_torque # self.param_dict[self.comp_name]['calibration']['zero_joint_torque_lc'] = self.joint_torque_lc # # self.zero_joint_theta_last = self.zero_joint_theta # self.zero_joint_torque_last = self.zero_joint_torque # self.zero_joint_torque_lc_last = self.zero_joint_torque_lc self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: f1=self.scope_keys[self.scope_field1[0]] f2=self.scope_keys[self.scope_field2[0]] x1=x2=None if f1!='None' and f1!='base': x1=m3t.get_msg_field_value(self.act.status,f1) print f1,':',x1 if f2!='None' and f2!='base': x2=m3t.get_msg_field_value(self.act.status,f2) print f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' if self.mode[0] == 0: #Off self.act.set_mode(mec.ACTUATOR_MODE_OFF) elif self.mode[0] == 1: #Pwm self.act.set_mode(mec.ACTUATOR_MODE_PWM) self.act.set_pwm(self.pwm_desired[0]) elif self.mode[0] == 2: #Current self.act.set_mode(mec.ACTUATOR_MODE_CURRENT) self.act.set_i_desired(self.current_desired[0]*1000.0) else: self.act.set_mode(mec.ACTUATOR_MODE_OFF) if (self.save and not self.save_last): self.act.write_config() self.save_last=self.save
def step(self): if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100, yrange=None) self.proxy.step() self.cnt = self.cnt + 1 self.status_dict = self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: ctrl_f1 = self.ctrl_scope_keys[self.ctrl_scope_field1[0]] ctrl_f2 = self.ctrl_scope_keys[self.ctrl_scope_field2[0]] x1 = x2 = None if ctrl_f1 != 'None': x1 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f1) print ctrl_f1, ':', x1 if ctrl_f2 != 'None': x2 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f2) print ctrl_f2, ':', x2 if x1 == None: x1 = x2 if x2 == None: x2 = x1 if x1 != None and x2 != None: #Handle only one value or two self.scope.plot(x1, x2) print '-----------------' self.ctrl.set_traj_mode(self.traj[0]) if self.mode[0] == 0: self.ctrl.set_mode_off() # elif self.mode[0]==1: # self.ctrl.set_mode_pwm() # self.ctrl.set_pwm(self.current[0]) elif self.mode[0] == 1: self.ctrl.set_mode_current() self.ctrl.set_current(self.current[0]) elif self.mode[0] == mec.CTRL_MODE_THETA: self.ctrl.set_mode_theta() self.ctrl.set_theta_deg(self.theta[0]) # elif self.mode[0] == mec.CTRL_MODE_TORQUE: print "kp: " + str(self.ctrl.param.pid_torque.k_p) self.ctrl.set_mode_torque() self.ctrl.set_torque(self.torque[0]) # # elif self.mode[0]==mec.CTRL_MODE_TORQUE_LC: # self.ctrl.set_mode_torque_lc() # self.ctrl.set_torque_lc(self.torque_lc[0]) else: self.ctrl.set_mode_off() if (self.save and not self.save_last): self.ctrl.write_config() self.save_last = self.save
def step(self): if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100,yrange=None) self.proxy.step() self.cnt = self.cnt+1 self.status_dict = self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: ctrl_f1 = self.ctrl_scope_keys[self.ctrl_scope_field1[0]] ctrl_f2 = self.ctrl_scope_keys[self.ctrl_scope_field2[0]] x1=x2=None if ctrl_f1 != 'None': x1 = m3t.get_msg_field_value(self.ctrl.status,ctrl_f1) print ctrl_f1,':',x1 if ctrl_f2 != 'None': x2 = m3t.get_msg_field_value(self.ctrl.status,ctrl_f2) print ctrl_f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' self.ctrl.set_traj_mode(self.traj[0]) if self.mode[0]==0: self.ctrl.set_mode_off() # elif self.mode[0]==1: # self.ctrl.set_mode_pwm() # self.ctrl.set_pwm(self.current[0]) elif self.mode[0]==1: self.ctrl.set_mode_current() self.ctrl.set_current(self.current[0]) elif self.mode[0]==mec.CTRL_MODE_THETA: self.ctrl.set_mode_theta() self.ctrl.set_theta_deg(self.theta[0]) # elif self.mode[0]==mec.CTRL_MODE_TORQUE: print "kp: " + str(self.ctrl.param.pid_torque.k_p) self.ctrl.set_mode_torque() self.ctrl.set_torque(self.torque[0]) # # elif self.mode[0]==mec.CTRL_MODE_TORQUE_LC: # self.ctrl.set_mode_torque_lc() # self.ctrl.set_torque_lc(self.torque_lc[0]) else: self.ctrl.set_mode_off() if (self.save and not self.save_last): self.ctrl.write_config() self.save_last = self.save
print 'Select Y-Range? [n]' yrange = None if m3t.get_yes_no('n'): yrange = [] print 'Min?' yrange.append(m3t.get_int()) print 'Max?' yrange.append(m3t.get_int()) name = comps[cid] comp = mcf.create_component(name) proxy.subscribe_status(comp) #proxy.publish_param(comp) field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) scope = m3t.M3Scope(xwidth=100, yrange=yrange) try: ts = time.time() while True: proxy.step() if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) scope.plot(v) #print 'Time: ',60.0-(time.time()-ts),field,':',v
def step(self): # print self.comps['act_ec']['comp'].status.timestamp if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100, yrange=None) self.proxy.step() self.cnt = self.cnt + 1 self.status_dict = self.proxy.get_status_dict() if self.zero_joint_theta and not self.zero_joint_theta_last: self.joint_theta -= self.act.get_joint_theta() print 'New joint_theta zero', self.joint_theta if self.zero_joint_torque and not self.zero_joint_torque_last: self.joint_torque -= self.act.get_joint_torque() print 'New joint_torque zero', self.joint_torque # # if self.zero_joint_torque_lc and not self.zero_joint_torque_lc_last: # self.joint_torque_lc -= self.act.get_joint_torque_lc() # print 'New joint_torque_lc zero',self.joint_torque_lc # # self.param_dict[self.comp_name]['calibration']['zero_motor_theta'] = self.motor_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_theta'] = self.joint_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_torque'] = self.joint_torque # self.param_dict[self.comp_name]['calibration']['zero_joint_torque_lc'] = self.joint_torque_lc # # self.zero_joint_theta_last = self.zero_joint_theta # self.zero_joint_torque_last = self.zero_joint_torque # self.zero_joint_torque_lc_last = self.zero_joint_torque_lc self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: f1 = self.scope_keys[self.scope_field1[0]] f2 = self.scope_keys[self.scope_field2[0]] x1 = x2 = None if f1 != 'None' and f1 != 'base': x1 = m3t.get_msg_field_value(self.act.status, f1) print f1, ':', x1 if f2 != 'None' and f2 != 'base': x2 = m3t.get_msg_field_value(self.act.status, f2) print f2, ':', x2 if x1 == None: x1 = x2 if x2 == None: x2 = x1 if x1 != None and x2 != None: #Handle only one value or two self.scope.plot(x1, x2) print '-----------------' if self.mode[0] == 0: #Off self.act.set_mode(mec.ACTUATOR_MODE_OFF) elif self.mode[0] == 1: #Pwm self.act.set_mode(mec.ACTUATOR_MODE_PWM) self.act.set_pwm(self.pwm_desired[0]) elif self.mode[0] == 2: #Current self.act.set_mode(mec.ACTUATOR_MODE_CURRENT) self.act.set_i_desired(self.current_desired[0] * 1000.0) else: self.act.set_mode(mec.ACTUATOR_MODE_OFF) if (self.save and not self.save_last): self.act.write_config() self.save_last = self.save
def do_dump(proxy): print 'Starting dump...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' comp=mcf.create_component(name) proxy.register_log_component(comp) fields=[] print 'Dump all data?[n]' if m3t.get_yes_no('n'): '''print 'Fetching data...' fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200)) f.close()''' fields=comp.status.DESCRIPTOR.fields_by_name.keys() print fields else: print ' Select fields to plot...' while True: fields.append(m3t.user_select_msg_field(comp.status)) print 'Another field [n]?' if not m3t.get_yes_no('n'): break print 'Fetching data...' data={} for k in fields: data[k]=[] print '---------------------------' print k print m3t.get_msg_field_value(comp.status,k) print dir(m3t.get_msg_field_value(comp.status,k)) if hasattr(m3t.get_msg_field_value(comp.status,k),'__len__'): for j in range(len(m3t.get_msg_field_value(comp.status,k))): data[k].append([]) for i in range(start,end): proxy.load_log_sample(logname,i) for k in fields: repeated = False # skip protobuf subclasses (like base) for now. note we'll want this for humanoid if hasattr(m3t.get_msg_field_value(comp.status,k),'__metaclass__'): pass elif hasattr(m3t.get_msg_field_value(comp.status,k),'__len__'): for j in range(len(m3t.get_msg_field_value(comp.status,k))): data[k][j].append(m3t.get_msg_field_value(comp.status,k)[j]) else: data[k].append(m3t.get_msg_field_value(comp.status,k)) fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn print data f.write(yaml.safe_dump(data, default_flow_style=False,width=200)) f.close()