def __init__(self, client=None, platform=None, message="", **kargs): super(EuropaOdometry, self).__init__(**kargs) self.client = client self.platform = platform self.message = message self.publisher = CEuropaOdometry("CEuropaOdometry", self.client.client, self.message) self.publisher.reparentTo(self) self.origin = Node("Origin", parent=self) self.origin.clearTransform(self.platform)
class EuropaOdometry(Output): def __init__(self, client=None, platform=None, message="", **kargs): super(EuropaOdometry, self).__init__(**kargs) self.client = client self.platform = platform self.message = message self.publisher = CEuropaOdometry("CEuropaOdometry", self.client.client, self.message) self.publisher.reparentTo(self) self.origin = Node("Origin", parent=self) self.origin.clearTransform(self.platform) # ------------------------------------------------------------------------------- def outputData(self, time): position = self.platform.getPosition(self.origin) orientation = self.platform.getOrientation(self.origin) pose = panda.Vec3(position[0], position[1], orientation[0]) velocity = panda.Vec2(*self.platform.chassis.state) self.publisher.publish(time, framework.scheduler.frameTime, pose, velocity)