def setup(): global dtarget,dval,dcycles,breakstate running = True dval =0 dtarget=5 dcycles=10000 breakstate = False global realitypub,targetsub,breaksub realitypub = rospy.Publisher("/N2/reality",IntArr) targetsub = rospy.Subscriber("/N2/target",IntArr,target_callback) breaksub = rospy.Subscriber("/breakServo",Bool,break_callback) rospy.init_node("dummy_rightteensy",anonymous=False) rospy.loginfo("> succesfully initialised") while running: if(not breakstate): if dcycles>0: dcycles-=1 noise = np.random.randint(-2,2) dval = (8*dval+2*dtarget+noise)/10 msg = IntArr() msg.ticks = dval msg.cycles = dcycles realitypub.publish(msg) rospy.loginfo("ticks: "+str(msg.ticks)+ " || cycles: "+str(msg.cycles)) else: rospy.loginfo("cycles empty") else: rospy.loginfo("____breaking____") time.sleep(0.01)
def simplemainloop(self): while True: self.scalc() msg = IntArr() msg.ticks = self.actualticks * self.sens msg.cycles = self.targetcycle self.realityPub.publish(msg) time.sleep(self.st)
def coordcallback(msg): global rightpub, leftpub , V , K, targetAngle, buffer if(buffer%10==0): alpha = w(targetAngle) - w(msg.theta) alpha = w(alpha) Vr = V*(np.cos(alpha)+K*np.sin(alpha)) #rad/s Vl = V*(np.cos(alpha)-K*np.sin(alpha)) #rad/s msg = IntArr() msg.cycles = 1000 msg.ticks = Vr rightpub.publish(msg) msg.ticks = Vl leftpub.publish(msg) rospy.loginfo("|| "+str(Vr)+" | "+str(Vl)+" |") buffer+=1
def OnSendClicked(self, e): try: ticks = self.ctrl_target_ticks.GetValue() cycles = self.ctrl_target_cycles.GetValue() angle = self.ctrl_target_angle.GetValue() vitesse = self.ctrl_target_vitesse.GetValue() except ValueError: print("OnSendClicked => can't get values from inputs") print(ValueError) try: msg = IntArr() msg.ticks = int(ticks) msg.cycles = int(cycles) ctrlMsg = FloatArr() ctrlMsg.theta = float(angle) ctrlMsg.v = float(vitesse) except ValueError: print("OnSendClicked => can't create ros msgs") print(ValueError) try: rospy.loginfo("control => angle : " + str(angle) + " | vitesse : " + str(vitesse)) controlpub.publish(ctrlMsg) except ValueError: print("OnSendClicked => can't publish msg") print(ValueError) if pid_cote == 0: try: rospy.loginfo('gauche | ticks : ' + str(ticks) + " | cycles : " + str(cycles)) #N1targetpub.publish(msg) except ValueError: print("OnSendClicked => can't publish msg") print(ValueError) else: try: rospy.loginfo('droit | ticks : ' + str(ticks) + " | cycles : " + str(cycles)) #N2targetpub.publish(msg) except ValueError: print("OnSendClicked => can't publish msg") print(ValueError) e.Skip()
def setup(): running = True ultrapub = rospy.Publisher("/ultrasound", IntArr) rospy.init_node("dummy_ultra", anonymous=False) rospy.loginfo("> succesfully initialised") maxdist = 2000 distlist = [] for i in range(5): distlist.append(maxdist / 2) while running: for i in range(len(distlist)): distlist[i] += np.random.randint(maxdist * -1 / 5, maxdist / 5) distlist[i] = max(0, min(maxdist, distlist[i])) msg = IntArr() msg.ticks = int(distlist[i]) msg.cycles = i ultrapub.publish(msg) time.sleep(0.1)
def signal_handler(signal, frame): sys.exit(0) if __name__ == '__main__': signal.signal(signal.SIGINT, signal_handler) global rightpub, leftpub rospy.init_node("tickviewer", anonymous=False) breakpub = rospy.Publisher("/breakServo", Bool, queue_size=1) rightpub = rospy.Publisher("/N1/target", IntArr, queue_size=2) leftpub = rospy.Publisher("/N2/target", IntArr, queue_size=2) print("waiting for full init") time.sleep(5) msg = IntArr() msg.cycles = 30000 msg.ticks = 0 rightpub.publish(msg) msg.ticks = 0 leftpub.publish(msg) bmsg = Bool() bmsg.data = True breakpub.publish(bmsg) print("ON TEST BENCH") print("test for motor connectivity") print("when ready press enter:") raw_input("...") msg.ticks = 30 rightpub.publish(msg) time.sleep(0.2)
def rightcallback(msg): global rcount, rightpub, rtarget rcount += msg.ticks tmsg = IntArr() tmsg.ticks = rtarget tmsg.cycles = 32000
def leftcallback(msg): global lcount, leftpub, ltarget lcount += msg.ticks tmsg = IntArr() tmsg.ticks = ltarget tmsg.cycles = 32000