void MMotionSensors::update() {
ADD_PROTO_TIMESTAMP;
this->clear_body_angles();
vector<float> bodyAngles = sensors->getBodyAngles();
for (vector<float>::iterator i = bodyAngles.begin(); i != bodyAngles.end(); i++) {
this->add_body_angles(*i);
}
this->clear_body_temperatures();
vector<float> bodyTemperatures = sensors->getBodyTemperatures();
for (vector<float>::iterator i = bodyTemperatures.begin(); i != bodyTemperatures.end(); i++) {
this->add_body_temperatures(*i);
}
const Inertial _inertial = sensors->getInertial();
PSensors::PInertial* inertial = this->mutable_inertial();
inertial->set_acc_x(_inertial.accX);
inertial->set_acc_y(_inertial.accY);
inertial->set_acc_z(_inertial.accZ);
inertial->set_gyr_x(_inertial.accX);
inertial->set_gyr_y(_inertial.accY);
inertial->set_angle_x(_inertial.angleX);
inertial->set_angle_y(_inertial.angleY);
const FSR _lfsr = sensors->getLeftFootFSR();
PSensors::PFSR* lfsr = this->mutable_left_foot_fsr();
lfsr->set_front_left(_lfsr.frontLeft);
lfsr->set_front_right(_lfsr.frontRight);
lfsr->set_rear_left(_lfsr.rearLeft);
lfsr->set_rear_right(_lfsr.rearRight);
const FSR _rfsr = sensors->getRightFootFSR();
PSensors::PFSR* rfsr = this->mutable_right_foot_fsr();
rfsr->set_front_left(_rfsr.frontLeft);
rfsr->set_front_right(_rfsr.frontRight);
rfsr->set_rear_left(_rfsr.rearLeft);
rfsr->set_rear_right(_rfsr.rearRight);
this->set_support_foot(sensors->getSupportFoot());
}
void Sensors::updateMotionSensorsInMemory(man::memory::MMotionSensors::ptr ms) const {
using namespace man::memory::proto;
ms->get()->clear_body_angles();
vector<float> bodyAngles = this->getBodyAngles();
for (vector<float>::iterator i = bodyAngles.begin(); i != bodyAngles.end(); i++) {
ms->get()->add_body_angles(*i);
}
ms->get()->clear_body_temperatures();
vector<float> bodyTemperatures = this->getBodyTemperatures();
for (vector<float>::iterator i = bodyTemperatures.begin(); i != bodyTemperatures.end(); i++) {
ms->get()->add_body_temperatures(*i);
}
const Inertial _inertial = this->getInertial();
PSensors::PInertial* inertial = ms->get()->mutable_inertial();
inertial->set_acc_x(_inertial.accX);
inertial->set_acc_y(_inertial.accY);
inertial->set_acc_z(_inertial.accZ);
inertial->set_gyr_x(_inertial.accX);
inertial->set_gyr_y(_inertial.accY);
inertial->set_angle_x(_inertial.angleX);
inertial->set_angle_y(_inertial.angleY);
const FSR _lfsr = this->getLeftFootFSR();
PSensors::PFSR* lfsr = ms->get()->mutable_left_foot_fsr();
lfsr->set_front_left(_lfsr.frontLeft);
lfsr->set_front_right(_lfsr.frontRight);
lfsr->set_rear_left(_lfsr.rearLeft);
lfsr->set_rear_right(_lfsr.rearRight);
const FSR _rfsr = this->getRightFootFSR();
PSensors::PFSR* rfsr = ms->get()->mutable_right_foot_fsr();
rfsr->set_front_left(_rfsr.frontLeft);
rfsr->set_front_right(_rfsr.frontRight);
rfsr->set_rear_left(_rfsr.rearLeft);
rfsr->set_rear_right(_rfsr.rearRight);
ms->get()->set_support_foot(this->getSupportFoot());
}
