void plResponderModifier::read(hsStream* S, plResManager* mgr) {
plSingleModifier::read(S, mgr);
clearStates();
fStates.setSizeNull(S->readByte());
for (size_t i=0; i<fStates.getSize(); i++) {
fStates[i] = new plResponderState();
fStates[i]->fNumCallbacks = S->readByte();
fStates[i]->fSwitchToState = S->readByte();
fStates[i]->fCmds.setSizeNull(S->readByte());
for (size_t j=0; j<fStates[i]->fCmds.getSize(); j++) {
plMessage* msg = plMessage::Convert(mgr->ReadCreatable(S));
int8_t waitOn = S->readByte();
fStates[i]->fCmds[j] = new plResponderCmd(msg, waitOn);
if (msg == NULL)
throw hsNotImplementedException(__FILE__, __LINE__, "Responder Message");
}
size_t count = S->readByte();
for (size_t j=0; j<count; j++) {
int8_t wait = S->readByte();
fStates[i]->fWaitToCmd[wait] = S->readByte();
}
}
int8_t state = S->readByte();
if (state >= 0 && (size_t)state < fStates.getSize()) {
fCurState = state;
} else {
plDebug::Warning("Invalid state %d found, will default to 0", state);
fCurState = 0;
}
fEnabled = S->readBool();
fFlags = S->readByte();
}
void CLegGrid::makeStates()
{
/*
* measurement = recieved leg detector data
* control command = leg velocity (-robot velocity)
* last state = last estimated state
*/
clearStates();
ROS_INFO_COND(DEBUG,"size of new leg msgs: %lu", grid_->polar_array.current.size());
if(diff_time_.toSec() < 1.0)
{
meas_.assign(grid_->polar_array.current.begin(), grid_->polar_array.current.end());
if(!grid_->polar_array.current.empty())
grid_->polar_array.current.clear();
}
match_meas_.resize(meas_.size(), false);
addLastStates();
addMeasurements();
filterStates();
ROS_ASSERT(cmeas_.size() == cstate_.size());
}
//============================================================================
// Perform the fit
//============================================================================
void TbKalmanTrack::fit() {
// remove existing states
clearStates();
// do the fit
if (!m_nodes.empty()) {
// initialize the seed: very simple for now. later we may want
// to run some iterations and then this becomes more
// complicated.
LHCb::TbState seedstate(firstState());
Gaudi::SymMatrix4x4 seedcov;
seedcov(0, 0) = seedcov(1, 1) = 1e4;
seedcov(2, 2) = seedcov(3, 3) = 1;
seedstate.covariance() = seedcov;
m_nodes.front()->setSeed(seedstate);
m_nodes.back()->setSeed(seedstate);
// everything happens on demand, I hope. so all we need to do is copy the
// smoothed states back.
LHCb::ChiSquare chi2(0, -4);
for (LHCb::TbKalmanNode* node : m_nodes) {
// get the smoothed state
addToStates(node->state());
// add to the chi2
chi2 += node->deltaChi2(0);
}
setNdof(chi2.nDoF());
if (chi2.nDoF() > 0) {
setChi2PerNdof(chi2.chi2() / chi2.nDoF());
} else {
setChi2PerNdof(0);
}
}
}
void plResponderModifier::IPrcParse(const pfPrcTag* tag, plResManager* mgr) {
if (tag->getName() == "ResponderModParams") {
fCurState = tag->getParam("CurState", "0").toInt();
fEnabled = tag->getParam("Enabled", "false").toBool();
fFlags = tag->getParam("Flags", "0").toUint();
} else if (tag->getName() == "States") {
clearStates();
fStates.setSizeNull(tag->countChildren());
const pfPrcTag* state = tag->getFirstChild();
for (size_t i=0; i<fStates.getSize(); i++) {
if (state->getName() != "plResponderState")
throw pfPrcTagException(__FILE__, __LINE__, state->getName());
fStates[i] = new plResponderState();
fStates[i]->fNumCallbacks = state->getParam("NumCallbacks", "0").toInt();
fStates[i]->fSwitchToState = state->getParam("SwitchToState", "-1").toInt();
const pfPrcTag* child = state->getFirstChild();
while (child != NULL) {
if (child->getName() == "Commands") {
fStates[i]->fCmds.setSize(child->countChildren());
const pfPrcTag* cmdChild = child->getFirstChild();
for (size_t j=0; j<fStates[i]->fCmds.getSize(); j++) {
if (cmdChild->getName() != "Command")
throw pfPrcTagException(__FILE__, __LINE__, cmdChild->getName());
plMessage* msg = NULL;
int8_t waitOn = -1;
const pfPrcTag* subChild = cmdChild->getFirstChild();
while (subChild != NULL) {
if (subChild->getName() == "WaitOn") {
waitOn = subChild->getParam("value", "-1").toInt();
} else {
msg = plMessage::Convert(mgr->prcParseCreatable(subChild));
}
subChild = subChild->getNextSibling();
}
fStates[i]->fCmds[j] = new plResponderCmd(msg, waitOn);
cmdChild = cmdChild->getNextSibling();
}
} else if (child->getName() == "WaitToCmdTable") {
size_t nWaits = child->countChildren();
const pfPrcTag* waitChild = child->getFirstChild();
for (size_t j=0; j<nWaits; j++) {
if (waitChild->getName() != "Item")
throw pfPrcTagException(__FILE__, __LINE__, waitChild->getName());
int8_t wait = waitChild->getParam("Wait", "0").toInt();
fStates[i]->fWaitToCmd[wait] = waitChild->getParam("Cmd", "0").toInt();
waitChild = waitChild->getNextSibling();
}
} else {
throw pfPrcTagException(__FILE__, __LINE__, child->getName());
}
child = child->getNextSibling();
}
state = state->getNextSibling();
}
} else {
plSingleModifier::IPrcParse(tag, mgr);
}
}
StateStorage & StateStorage::operator=( const StateStorage & other )
{
if (this == &other)
return *this;
clearStates();
states = other.states;
initialStates = other.initialStates;
finalStates = other.finalStates;
stateInfos.clear();
for (std::map<State,ClientInfoRefPtr>::const_iterator other_cis = other.stateInfos.begin() ;
other_cis != other.stateInfos.end(); ++other_cis)
{
stateInfos[other_cis->first] = other_cis->second->cloneRp();
}
return *this;
}
void lightDisplay(void) {
int i;
clearStates();
displaySpeed();
displayDirection();
_delay_ms(1000);
for (i = 0; i < 8; i++) {
dir[i] = ON;
if (i < 6) {
spd[i] = ON;
if (i < 4) {
ctrl[i] = ON;
}
}
displaySpeed();
displayDirection();
_delay_ms(1000);
}
}
void CWebUser::logout(bool destroySession)
{
if (beforeLogout()) {
CWebApplication * app = dynamic_cast<CWebApplication*>(Cws::app());
if (allowAutoLogin) {
CCookieCollection cookies = app->getRequest()->getCookies();
CHttpResponse * response = app->getResponse();
response->removeCookie(getStateKeyPrefix());
if (identityCookie.empty()) {
CHttpCookie cookie = createIdentityCookie(getStateKeyPrefix());
response->removeCookie(cookie);
}
}
if (destroySession) {
CHttpSession * session = dynamic_cast<CHttpSession*>(app->getComponent("session"));
session->destroy();
} else {
clearStates();
}
_access = TWebUserAccessMap();
afterLogout();
}
}
void HMMSoftAutomaton::hmmTrain() {
Init();
clearStates();
std::vector<WaveFeatureOP> & datas = *templates;
// states.push_back(DummyState(NULL));
int idx, idy, idz;
states.push_back(new DummyState(NULL));
// 初始化 几个states
for(idx = 1;idx <= stateNum;idx ++) {
states.push_back(new SoftState(templates));
}
// 统计每个状态初始有多少个节点
std::vector<int> nodeCnt(stateNum + 1);
for(idx = 0; idx <= stateNum; idx++)
nodeCnt[idx] = 0;
for(idx = 0; idx < datas.size();idx ++)
for(idy = 1; idy <=stateNum; idy ++)
for(idz = 0; idz < datas[idx].size(); idz ++)
getState(idy)->probabilities[idx][idz] = 0.0;
// 初始化， 仍然平均分段
for(idx = 0; idx < datas.size(); idx++) {
int templateSiz = datas[idx].size();
int featurePerState = templateSiz / stateNum;
for(idy = 1, idz = 0; idy <= stateNum; idy ++) {
if(idy == stateNum) featurePerState = templateSiz - idz;
int itr;
for(itr = 0; itr < featurePerState; itr ++) {
getState(idy)->probabilities[idx][idz+itr] = 1.0;
nodeCnt[idy] ++;
}
idz += itr;
}
}
for(idx = 1;idx <= stateNum;idx ++) {
getState(idx)->gaussianTrain(gaussNum);
// printf("%d\n", idx);
// getState(idx)->dump();
}
// init transferCost
for(idx = 0; idx <= stateNum; idx++)
for(idy = 0; idy <= stateNum; idy++)
transferCost[idx][idy] = Feature::IllegalDist;
for(idx = 1; idx <= stateNum; idx++) {
int nxtCnt = std::min(stateNum - idx + 1, DTW_MAX_FORWARD);
for(idy = 0; idy < nxtCnt; idy++)
transferCost[idx][idx+idy] = p2cost(1.0/nxtCnt);
}
/*
for(idx = 1; idx < stateNum; idx ++) {
transferCost[idx][idx] = p2cost(1.0 * (nodeCnt[idx] - datas.size()) / nodeCnt[idx]);
transferCost[idx][idx + 1] = p2cost(1.0 * datas.size() / nodeCnt[idx]);
}
transferCost[stateNum][stateNum] = 0.0;
*/
/*
for(idx = 0; idx <= stateNum; idx++) {
for(idy = 0; idy <= stateNum; idy++)
printf("%lf ", transferCost[idx][idy]);
puts("");
}
*/
// Dummy 只会走到1
if(stateNum)
transferCost[0][1] = 0.0;
for(idx = 0; idx < trainTimes; idx++) {
// printf("%d\n", idx);
if(! iterateTrain()) break;
}
/*
for(idx = 0; idx <= stateNum; idx++) {
for(idy = 0; idy <= stateNum; idy++)
printf("%lf ", transferCost[idx][idy]);
puts("");
}
*/
clearTrainBuffer();
}
void GameEngine::run()
{
// The engine can only run once at a time and with at least one game state
if(m_running || m_states.empty())
return;
m_running = true;
// Variables for timing
unsigned int skippedFrame;
float interpolation;
sf::Time nextUpdateTime = sf::Time::Zero;
sf::Clock clock;
sf::Event ev;
sf::Clock FPSClock;
// Main loop
while(m_running)
{
clearDroppedStates();
m_currentState = m_states.top();
skippedFrame = 0;
while(clock.getElapsedTime() >= nextUpdateTime && skippedFrame <= m_maxFrameSkip)
{
while(m_window.pollEvent(ev))
m_currentState->handleEvents(ev);
m_currentState->update();
if(m_states.empty())
{
m_running = false;
break;
}
else if(m_states.top() != m_currentState)
{
m_currentState = m_states.top();
break;
}
nextUpdateTime += m_updateDuration;
skippedFrame++;
}
if(m_running)
{
if(m_nbUpdatesPerSecond > 0)
interpolation = (float)(clock.getElapsedTime().asMicroseconds()
+ m_updateDuration.asMicroseconds()
- nextUpdateTime.asMicroseconds())
/ (float)m_updateDuration.asMicroseconds();
else
interpolation = 0;
m_window.clear();
m_currentState->render(m_window, interpolation);
m_window.display();
m_FPS = 1000000 / FPSClock.getElapsedTime().asMicroseconds();
FPSClock.restart();
}
}
m_currentState = NULL;
clearStates();
clearDroppedStates();
}
GameEngine::~GameEngine()
{
clearStates();
clearDroppedStates();
}
void DefaultCore::teardown() {
// Ensure everything is torndown before we start deleting stuff!
clearStates();
}
