Commit b3e99200 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[Device] Copy robotState in state at initialization

  Formerly the Stack of Tasks started from a prespecified configuration.
  Starting from another configuration caused the robot to quickly move
  to the prespecified configuration. This commit fixes this undesired
  behavior.
parent 4097adb3
Pipeline #19294 passed with stage
in 4 minutes and 13 seconds
......@@ -265,7 +265,13 @@ void SoTTalosDevice::setSensors(map<string, dgsot::SensorValues>& SensorsIn) {
sotDEBUGOUT(25);
}
void SoTTalosDevice::setupSetSensors(map<string, dgsot::SensorValues>& SensorsIn) { setSensors(SensorsIn); }
void SoTTalosDevice::setupSetSensors(map<string, dgsot::SensorValues>& SensorsIn)
{
// The first time we read the sensors, we need to copy the state of the
// robot into the signal device.state.
setSensors(SensorsIn);
setState(robotState_(0));
}
void SoTTalosDevice::nominalSetSensors(map<string, dgsot::SensorValues>& SensorsIn) { setSensors(SensorsIn); }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment