void MVisionSensors::update() {
ADD_PROTO_TIMESTAMP;
this->clear_vision_body_angles();
vector<float> bodyAngles = sensors->getVisionBodyAngles();
for (vector<float>::iterator i = bodyAngles.begin(); i != bodyAngles.end(); i++) {
this->add_vision_body_angles(*i);
}
FootBumper leftFootBumper = sensors->getLeftFootBumper();
PSensors::PFootBumper* lfb = this->mutable_left_foot_bumper();
lfb->set_left(leftFootBumper.left);
lfb->set_right(leftFootBumper.right);
FootBumper rightFootBumper = sensors->getRightFootBumper();
PSensors::PFootBumper* rfb = this->mutable_right_foot_bumper();
rfb->set_left(rightFootBumper.left);
rfb->set_right(rightFootBumper.right);
this->set_ultra_sound_distance_left(sensors->getUltraSoundLeft());
this->set_ultra_sound_distance_right(sensors->getUltraSoundRight());
this->set_battery_charge(sensors->getBatteryCharge());
this->set_battery_current(sensors->getBatteryCurrent());
//std::cout << this->DebugString() << std::endl;
}
void Sensors::updateVisionSensorsInMemory(man::memory::MVisionSensors::ptr vs) const {
using namespace man::memory::proto;
vs->get()->clear_vision_body_angles();
vector<float> bodyAngles = this->getVisionBodyAngles();
for (vector<float>::iterator i = bodyAngles.begin(); i != bodyAngles.end();
i++) {
vs->get()->add_vision_body_angles(*i);
}
FootBumper leftFootBumper = this->getLeftFootBumper();
PSensors::PFootBumper* lfb = vs->get()->mutable_left_foot_bumper();
lfb->set_left(leftFootBumper.left);
lfb->set_right(leftFootBumper.right);
FootBumper rightFootBumper = this->getRightFootBumper();
PSensors::PFootBumper* rfb = vs->get()->mutable_right_foot_bumper();
rfb->set_left(rightFootBumper.left);
rfb->set_right(rightFootBumper.right);
vs->get()->set_ultra_sound_distance_left(this->getUltraSoundLeft());
vs->get()->set_ultra_sound_distance_right(this->getUltraSoundRight());
vs->get()->set_battery_charge(this->getBatteryCharge());
vs->get()->set_battery_current(this->getBatteryCurrent());
}
