def publishEvent(self, event): trace('publishEvent %s', event.eventString) if self.publish: self.pub_team_event.publish(event) # also print to stdout so we can watch it on coach, as some sort of error log print time2str(event.timeStamp) + " - r" + str(event.robotId) + " - " + event.eventString time.sleep(0.01) # workaround for superfast consecutive publishing, prevent ROS dropping messages
def get_driving_displacement(self, uniqueID): ''' Fetch the total displacement ''' dx = 0.0 dy = 0.0 dtheta = 0.0 try: if not self._displacement.has_key(uniqueID): self._displacement[uniqueID] = [0.0, 0.0, 0.0] dx = copy.copy(self._displacement[uniqueID][0]) dy = copy.copy(self._displacement[uniqueID][1]) dtheta = copy.copy(self._displacement[uniqueID][2]) self._displacement[uniqueID][0] = 0.0 self._displacement[uniqueID][1] = 0.0 self._displacement[uniqueID][2] = 0.0 trace('get driving displacement = %6.2f, %6.2f, %6.2f', dx, dy, dtheta) except: traceError('Failed to get driving displacement') print traceback.format_exc() print sys.exc_info()[0] return dx, dy, dtheta
def __init__(self, compassObject): ''' Initialize motors class Steps: - Open serial ports for driving motors - Open serial ports for ball handling motors ''' try: self._motor_ports = [] self._robot_speed_x = 0.0 self._robot_speed_y = 0.0 self._robot_speed_theta = 0.0 self._displacement = {} self._vel_motor_left = 0.0 self._vel_motor_right = 0.0 self._vel_motor_rear = 0.0 self._ball_speed_left = 0.0 self._ball_speed_right = 0.0 self._timestamp = datetime.datetime.now() self._displacementTimestamp = datetime.datetime.now() self._compass = compassObject self.ball_possession_status = False self._bhAngle = 0 self._init_services() trace('Motors class initialized') except: traceError('failed to initialize cMotors')
def setKickPosition(self, request): response = s_peripheralsInterface_setKickPositionResponse() 'adjusts the shooter height by setting the given height to the stepper motor' self._shoot_height = float(request.kick_position) trace('shoot height: %8.4f' % (self._shoot_height)) return response
def feed(self, packet, timeStamp): trace("packet=%s", packet) self.prevPacket = self.currPacket self.prevTimeStamp = self.timeStamp self.currPacket = packet self.timeStamp = timeStamp if self.timeStampStart == None: self.timeStampStart = timeStamp
def analyzeDistanceDriven(self): # KPI distanceDriven: distance driven # do not accumulate fake distance while robot is standing still (noise) for irobot in ROBOT_IDS: if self.isDriving(irobot): dist = self.calculateDistance(self.currPacket.robots[irobot], self.prevPacket.robots[irobot]) self.distanceDriven[irobot] += dist trace("r%d currPos=(%6.2f,%6.2f) dist=%6.2f", irobot, self.currPacket.robots[irobot].x, self.currPacket.robots[irobot].y, dist)
def activate_ball_handling(self, speed_left=0, speed_right=0): 'set the speed of ball handling cMotors' try: self._ball_speed_left = speed_left self._ball_speed_right = speed_right trace('activated ball handling') except: traceError('Failed to set active ball handling')
def log(msg, level=1, eventType=0): trace("%s", msg) # note: msg may contain a percentage if level <= _verbose: timestamp = (str(datetime.datetime.now()).replace(' ', ',')) logmsg = timestamp + " : " + msg print logmsg # extra: relay level0 messages to coach, so the bag is selfcontaining if level == 0: generateEvent(msg, eventType)
def stop_robot(self): 'stops the robot by sending speed 0' try: self._vel_motor_left = 0.0 self._vel_motor_right = 0.0 self._vel_motor_rear = 0.0 trace('robot stopped') except: traceError('Cannot stop robot')
def sendTo(self, robotnum, command): """ Send a command to a dedicated robot. """ # prepend hostname to achieve claiming - robotControl listener will disregard # incoming messages from other hosts, if any message = "from " + self.hostname + " " + command trace("sendTo addr=%s port=%d message='%s'" % (self.address, getPort(robotnum), message)) burstSend(self.socket, self.address, getPort(robotnum), message, self.burstcount)
def __init__(self, *args): trace("constructing TestStress object") unittest.TestCase.__init__(self) self.N = 100 # list some topic (not necessarily all the same as defined in cDiagnosticsReceiver.cpp) self.topics = [] self.subscribers = [] self.topics.append((6, "g_diag_robot_control", t_diag_robot_control)) self.topics.append((7, "g_vision_diag", t_vision_diag)) self.callcount = defaultdict(lambda: 0) trace("construction complete")
def run(self): self.threads[0].start() sleep(3) rospy.init_node('captureToRosEKF') self.threads[1].start() rospy.spin() # shutdown trace('shutdown') global STOP STOP = True for t in self.threads: t.join()
def __init__(self, playbackfile): trace("initializing") # setup the threads self.threads = [] self.t0 = datetime.datetime.now() self.threads.append(Thread(target=startEKF)) self.threads.append( Thread(target=feedCaptureEKF, args=(playbackfile, ))) # subscribe to EKF output topic rospy.Subscriber("robot_pose_ekf/odom_combined", PoseWithCovarianceStamped, self.cbOdomCombined) trace("initialization done")
def verify(self): good = True for robotnum in range(1, 7): for tdef in self.topics: topic = ("/robot%d/" % (robotnum)) + tdef[1] trace("checking : topic " + topic + " published %d times" % (self.callcount[topic])) if self.callcount[topic] != self.N: good = False if not good: sys.exit( 1 ) # a bit blunt, but a mere 'assert' does not escalate to the MAIN thread ...
def robotFacesGoal(self, robotId): """ Determine if robot is facing opponent goal, for example attempting a shot. """ ourPos = self.getPosition(robotId) goalFcs1 = Position2D(-1, 9, 0) goalFcs2 = Position2D( 1, 9, 0) goalRcs1 = goalFcs1.transform_fcs2rcs(ourPos) goalRcs2 = goalFcs2.transform_fcs2rcs(ourPos) facingAngle1 = atan2(goalRcs1.x, goalRcs1.y) facingAngle2 = atan2(goalRcs2.x, goalRcs2.y) trace("r%d phi=%.2f facingAngle1=%.2f facingAngle2=%.2f", robotId, ourPos.phi, facingAngle1, facingAngle2) return 0.0 > facingAngle1 and 0.0 < facingAngle2
def run(self): ''' Spin, publish the data gathered via services. ''' rate = rospy.Rate(self._frequency) counter = 0 while not rospy.is_shutdown(): trace("iterate") # passive update via external service calls and topic subscriptions # so standard ROS stuff # active update by polling e.g. WorldModel # high-frequent: 10Hz self.update_hf() # low-frequent if counter % 30 == 0: trace("LF update start") try: # might fail in sim mode self.update_lf() trace("LF update OK") except: trace("LF update FAIL") pass self._topic_handle.publish(self._data) rate.sleep() counter += 1
def analyzeLocalizationGlitches(self): # cooldown to prevent reporting a single fast A-B-A glitch twice # check each active robot for irobot in ROBOT_IDS: if self.currPacket.active[irobot] and self.prevPacket.active[irobot]: deltaTime = self.timeStamp - self.prevTimeStamp if deltaTime < LOC_GLITCH_DT: deltaPos = self.getPosition(irobot) - self.getPosition(irobot, prev=True) glitchSize = max(abs(deltaPos.x), abs(deltaPos.y)) if glitchSize > LOC_GLITCH_TOL_XY: self.reportLocalizationGlitch(glitchSize, irobot) # ignore phi for now, not really a problem - glitches tend to correlate else: trace("unexpected large gap in data stream")
def _set_speed(self, motor_left_vel, motor_right_vel, motor_rear_vel): 'set the speed individually to cMotors left,right and rear' try: self._vel_motor_left = motor_left_vel self._vel_motor_right = motor_right_vel self._vel_motor_rear = motor_rear_vel trace('set speed = %6.2f, %6.2f, %6.2f', motor_left_vel, motor_right_vel, motor_rear_vel) trace('_set_speed finished') except: traceError('failed to set_speed')
def main(): 'main module to start the driver services of all hardware peripherals' try: trace('Initing node Peripherals Sim') rospy.init_node('Peripherals_SIM', anonymous=False) kicker = cKicker() compass = cCompass() motors = cMotors(compass) rospy.spin() except Exception, e: print str(e)
def setKickSpeed(self, request): 'adjusts the shoot height and triggers the shooter with the set speed' response = s_peripheralsInterface_setKickSpeedResponse() try: self._shoot_speed = float(request.kick_speed) trace('shoot speed: %8.4f' % (self._shoot_speed)) resp = self._sim_shoot_srv(self._shoot_speed * cKicker.BALL_SIM_FACTOR) except: traceError('failed to shoot with shoot speed: %8.4f' % (self._shoot_speed)) return response
def run(self): self.threads[0].start() sleep(3) rospy.init_node('captureToRosEKF2') self.threads[1].start() rospy.spin() # shutdown trace('shutdown') os.system("pkill roslaunch") global STOP STOP = True for t in self.threads: t.join() trace('joined') self.finalize()
def udp_talker(self, robotnum, count, sleeptime): trace((robotnum, count, sleeptime)) multicast = True sleep(1) # give some time for topic listeners to come online for tdef in self.topics: # TODO brigde C++/python port = 10000 + 1000 * robotnum + tdef[0] u = UDPsender("224.16.32.74", port, multicast) # serialize message b = StringIO() msg = tdef[2]().serialize(b) # send repeatedly for i in range(count): u.burstSend(b.getvalue()) sleep(sleeptime)
def cbOdomCombined(self, data): trace(str(data)) elapsed = datetime.datetime.now() - self.t0 t = elapsed.total_seconds() if args.preskip: t += args.tmin x = data.pose.pose.position.x y = data.pose.pose.position.y # ?? magic pi factor phi = data.pose.pose.orientation.z * math.pi print "%10.6f kalman %10.6f %10.6f %10.6f" % (t, x, y, phi) if (t > self.args.tmin) and (t < self.args.tmax): self.kalman_t.append(t) self.kalman_x.append(x) self.kalman_y.append(y) self.kalman_phi.append(phi)
def __init__(self, args): trace("initializing") # setup the threads self.threads = [] self.t0 = datetime.datetime.now() self.args = args self.threads.append(Thread(target=startEKF)) self.threads.append(Thread(target=feedCaptureEKF, args=(args, ))) # subscribe to EKF output topic rospy.Subscriber("odometry/filtered", Odometry, self.cbOdomCombined) # data so we can calculate KPI's self.kalman_t = [] self.kalman_x = [] self.kalman_y = [] self.kalman_phi = [] trace("initialization done")
def checkRefbox(self): #trace('checkRefbox') if self.lastRefboxCmd != None: elapsed = time.time() - self.lastRefboxCmd[1] trace('elapsed = %6.2f' % (elapsed)) if elapsed > REFBOX_CHECK_TIME: ok = True for robotnum in self.getRobots(t_ana_online.INGAME): if self.robots[robotnum].lastRefboxCmd != self.lastRefboxCmd[0]: ok = False eventString = 'refbox inconsistency (%s)' % (self.robots[robotnum].lastRefboxCmd) self.eventWrapper(eventString, t_event.TYPE_WARNING, robotnum) trace('ok=' + str(ok)) if ok: # no need to check again until next refbox signal occurs self.lastRefboxCmd = None
def get_service(self, *args): """ Return a service client connection, create if not yet existing. """ assert (isinstance(args, collections.Hashable)) key = args if self._service_cache.has_key(key): # cache hit trace('cache hit') service = self._service_cache[key] else: # cache miss - wait, construct client and store in cache trace('waiting for service', args[0]) rospy.wait_for_service(args[0]) service = rospy.ServiceProxy(*args, persistent=True) self._service_cache[key] = service return service
def get_driving_speed(self): ''' Fetch robot speed in meters per second ''' vx = 0.0 vy = 0.0 vtheta = 0.0 try: vx = self._robot_speed_x vy = self._robot_speed_y vtheta = self._robot_speed_theta trace('get driving speed=%6.2f, %6.2f, %6.2f', vx, vy, vtheta) except: traceError('Failed to get driving speed') return vx, vy, vtheta
def handleGlitch(self, allowedTime, glitch): trace("detected possible glitch: " + str(glitch)) # check if glitch is new (then just store) or existing (might need to generate event) if self.glitchStore.has_key(glitch): # send if the glitch was already stored a while ago if time.time() - self.glitchStore[glitch] > allowedTime: # it must really be a glitch, so generate an event trace("publishing event: " + str(glitch)) self.eventWrapper(*glitch) # cleanup to reduce spam del self.glitchStore[glitch] else: # store glitch with its timestamp self.glitchStore[glitch] = time.time() # cleanup outdated items in glitch store for g in self.glitchStore.keys(): if self.glitchStore[g] < time.time() - 2: self.resetGlitch(g)
def checkRoleAssignment(self): #trace('checkRoleAssignment') roles = {} # check if every role in array is unique onlineRobots = self.getRobots(t_ana_online.INGAME) for robotnumA in onlineRobots: robotA = self.robots[robotnumA] if robotA.role != None and robotA.role != "": if robotA.role != 'R_robotStop': # ignore, all robots get this at the same time # check if any of the other robots have this role for robotnumB in onlineRobots: if robotnumB > robotnumA: robotB = self.robots[robotnumB] trace("compare roles " + robotA.role + " " + robotB.role) if robotA.role == robotB.role: # log error eventString = "RoleAssignment inconsistency: robot%d and robot%d have role %s " % (robotnumA, robotnumB, robotA.role) self.eventWrapper(eventString, t_event.TYPE_WARNING)
def cbRefbox(self, msg): # NOTE: we assume this callback is triggered only on event! # so if we would start repeating the refbox on the on-coach topic, here the goal administration would get messed up.. trace("cbRefbox: " + msg.refboxCmd) self.eventWrapper("refbox command: " + msg.refboxCmd, t_event.TYPE_INFO) self.lastRefboxCmd = (msg.refboxCmd, time.time()) # update match state administration if "KICKOFF" in msg.refboxCmd: if self.matchState.state == "": self.matchState.state = "firstHalf" if self.matchState.state == "halfTime": self.matchState.state = "secondHalf" if "GOAL_OWN" == msg.refboxCmd: self.matchState.goalsOwn += 1 if "GOAL_OPP" == msg.refboxCmd: self.matchState.goalsOpponent += 1 if "HALF_TIME" == msg.refboxCmd: self.matchState.state = "halfTime" if "END_GAME" == msg.refboxCmd: self.matchState.state = "afterMatch"