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"
