Changeset 7

Show
Ignore:
Timestamp:
03/23/07 01:30:43 (2 years ago)
Author:
vash
Message:

logging works

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved

  • trunk/src/logging.py

    r1 r7  
    11import os, sys, time 
    2 from frontend import * 
    32from network import * 
    43from robot import Robot 
    5 from world import World 
    64 
    75#================================================================================= 
    …… 
    1816         
    1917        # Initialize the world and all of its robots 
    20         self.world = World(cfg.world) 
     18        self.robots = [] 
    2119        for item in cfg.world.robot.items(): 
    2220            robot = LogRobot(item[1], cfg.icehost, item[0]) 
    23             self.world.robots.append(robot) 
     21            self.robots.append(robot) 
    2422             
    2523    def run(self): 
    …… 
    3230    def __init__(self, robotCfg, host, name): 
    3331        Robot.__init__(self, robotCfg) 
    34         self.cmdLog = open(name + ".command.log", "w") 
    3532        self.poseLog = open(name + ".pose.log", "w") 
    36          
    37         self.cmdSub = CommandSubscriber(host, name + ":command", self.logCommand) 
    3833        self.poseSub = PoseSubscriber(host, name + ":pose", self.logPose) 
    3934         
    …… 
    4540            elif sensorCfg.type[:3] == "imu": 
    4641                self.sensors[sid] = LogImu(sensorCfg, host, topic, name) 
    47             elif sensorCfg.type[:5] == "sonar": 
    48                 self.sensors[sid] = LogSonarArray(sensorCfg, host, topic, name) 
    4942 
    50     def logCommand(self, ts, v, w): 
    51         msg = str(ts) + " " + str(v) + " " + str(w) + "\n" 
    52         self.cmdLog.writelines(msg) 
    53          
    54     def logPose(self, ts, x, y, theta): 
    55         msg = str(ts) + " " + str(x) + " " + str(y) + str(theta) + "\n" 
     43    def logPose(self, ts, x, y, z, roll, pitch, yaw): 
     44        msg = str(ts) + " " + str(x) + " " + str(y) + " " + str(z) + " " + \ 
     45              str(roll) + " " + str(pitch) + " " + str(yaw) + "\n" 
    5646        self.poseLog.writelines(msg) 
    5747 
    …… 
    7464        self.imuSub = ImuSubscriber(host, topic, self.logImu) 
    7565     
    76     def logImu(self, ts, phi, theta, psi, p, q, r): 
     66    def logImu(self, ts, phi, theta, psi, p, q, r, fx, fy, fz): 
    7767        msg = str(ts) + " " + str(phi) + " " + str(theta) + " " + str(psi) + " " \ 
    78               + str(p) + " " + str(q) + " " + str(r) + "\n" 
     68              + str(p) + " " + str(q) + " " + str(r) + " " + str(fx) + " " \ 
     69              + str(fy) + " " + str(fz) + "\n" 
    7970        self.imuLog.writelines(msg) 
    80  
    81 #================================================================================= 
    82 # LOGSONARARRAY --  
    83 class LogSonarArray: 
    84     def __init__(self, imuCfg, host, topic, name): 
    85         self.sonarLog = open(name + ".sonar.log", "w") 
    86         self.sonarSub = SonarArraySubscriber(host, topic, self.logSonarArray) 
    87      
    88     def logSonarArray(self, ts, ranges): 
    89         msg = str(ts) 
    90         for r in ranges: 
    91             msg += " " + str(r) 
    92         msg += "\n" 
    93         self.sonarLog.writelines(msg) 
    9471 
    9572if __name__ == "__main__": 

  • trunk/src/robot.py

    r1 r7  
    11from numpy import mat, float32 
    2 from geometry import * 
    32from math import degrees, radians 
    4 from sonars import SonarArray 
    53 
    6 class UndefinedChassisType(Exception): pass 
    74class UndefinedBodyType(Exception): pass 
    85 
    …… 
    1411        self.x = robotCfg.x 
    1512        self.y = robotCfg.y 
    16         self.theta = radians(robotCfg.theta) 
    17          
    18         # Configure the chassis 
    19         chassisCfg = robotCfg.chassis 
    20         if chassisCfg.type[:4] == "diff": 
    21             self.chassis = DifferentialChassis(chassisCfg) 
    22         else: 
    23             raise UndefinedChassisType() 
    24  
    25         # Configure the body 
    26         bodyCfg = robotCfg.body 
    27         if bodyCfg.type[:3] == "box": 
    28             self.body = Box(Point(0., 0.), bodyCfg.length, bodyCfg.width) 
    29         elif bodyCfg.type[:4] == "circ": 
    30             self.body = Circle(Point(0., 0.), bodyCfg.radius) 
    31         else: 
    32             raise UndefinedBodyType() 
     13        self.z = robotCfg.z 
     14        self.roll = radians(robotCfg.roll) 
     15        self.pitch = radians(robotCfg.pitch) 
     16        self.yaw = radians(robotCfg.yaw) 
    3317 
    3418        # Define the sensor data structure 
    3519        self.sensors = {} 
    36  
    37     def draw(self): 
    38         self.drawBody(self.x, self.y, self.theta) 
    39         self.drawSensors(self.x, self.y, self.theta) 
    40  
    41     def drawBody(self, x, y, theta): 
    42         glPushMatrix() 
    43         glTranslatef(x, y, 0) 
    44         glRotatef(degrees(theta), 0, 0, 1) 
    45         glTranslatef(self.chassis.mountX, 0, 0) 
    46         self.body.draw() 
    47         glPopMatrix() 
    48  
    49     def drawSensors(self, x, y, theta): 
    50         glPushMatrix() 
    51         glTranslatef(x, y, 0) 
    52         glRotatef(degrees(theta), 0, 0, 1) 
    53         for sensor in self.sensors.values(): 
    54             sensor.draw() 
    55         glPopMatrix() 
    56  
    57 #=============================================================================== 
    58 # DIFFERENTIALCHASSIS -- Models the robot as having two motors and a caster 
    59 class DifferentialChassis: 
    60     def __init__(self, chassisCfg): 
    61         self.mountX = chassisCfg.mountX 
    62         self.mountY = 0.5 * chassisCfg.axleLength 
    63         self.wheelRadius = 0.5 * chassisCfg.wheelDiameter 
    64         self.encTicsPerRev = chassisCfg.encTicsPerRev 
    65