SelfIL(SoarAgent agent) {
super(agent, IOConstants.SELF, agent.getSoarAgent().GetInputLink());
this.output = agent.getRobotOutput();
properties = agent.getProperties();
areaId = IntWme.newInstance(getRoot(), IOConstants.AREA);
headlight = StringWme.newInstance(getRoot(), IOConstants.HEADLIGHT);
battery = FloatWme.newInstance(getRoot(), IOConstants.BATTERY);
Identifier pose = getRoot().CreateIdWME(IOConstants.POSE);
xyz.add(DistanceWme.newInstance(pose, IOConstants.X, properties));
xyz.add(DistanceWme.newInstance(pose, IOConstants.Y, properties));
xyz.add(DistanceWme.newInstance(pose, IOConstants.Z, properties));
yaw = YawWme.newInstance(pose, IOConstants.YAW, properties);
xyzVelocity.add(LinSpeedWme.newInstance(pose, IOConstants.X_VELOCITY, properties));
xyzVelocity.add(LinSpeedWme.newInstance(pose, IOConstants.Y_VELOCITY, properties));
xyzVelocity.add(LinSpeedWme.newInstance(pose, IOConstants.Z_VELOCITY, properties));
yawVelocity = AngSpeedWme.newInstance(pose, IOConstants.YAW_VELOCITY, properties);
update();
}
@Override
public void update() {
if (output.getCarriedObject() == null) {
if (carryId != null) {
carryId.destroy();
carryId = null;
}
} else {
if (carryId == null) carryId = IntWme.newInstance(getRoot(), IOConstants.CARRY);
carryId.update(output.getCarriedObject().getId());
logger.trace(carryId);
}
AreaDescription a = output.getAreaDescription();
areaId.update((a != null) ? a.getId() : -1);
headlight.update(output.isHeadlightOn() ? IOConstants.ON : IOConstants.OFF);
battery.update(output.getBatteryLife() * 100);
logger.trace(areaId);
Pose p = output.getPose();
p.translate(properties.get(AgentProperties.TRANSLATION));
for (int i = 0; i < xyz.size(); ++i) {
xyz.get(i).update(p.getPos().get(i));
logger.trace(xyz.get(i));
xyzVelocity.get(i).update(p.getVel().get(i));
logger.trace(xyzVelocity.get(i));
}
yaw.update(p.getYaw());
logger.trace(yaw);
yawVelocity.update(p.getRotationRate().get(2));
logger.trace(yawVelocity);
}
