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()); }