bool RangeSensorWallFollow::regressionsGerade(double _alpha, double _beta, double delta, double& a, double& b) const { // correction angle Vector2d alpha(cos(-delta), sin(-delta)); // build vector of egocentric sensor readings std::vector<Vector2d> scan; scan.reserve(scan_.size()); EgoMap::const_iterator first, last = egoMap_.upper_bound(_beta); for (first = egoMap_.lower_bound(_alpha); first != last; ++first) { scan.push_back(first->second * alpha); // cout << "x =" << first->real() << "\t y=" << first->imag() << endl; } if (scan.size() < 5) { // too litle data cout << "too little data" << endl; return false; } // Geradengleichung y = m*x + b Vector2d sum = std::accumulate(scan.begin(), scan.end(), Vector2d()); // cout << "sum: " << sum.real() << ", " << sum.imag() << endl; double n = scan.size(); double xmean = sum.real() / n; double ymean = sum.imag() / n; double sxy = 0.; double ssqx = 0.; std::vector<Vector2d>::const_iterator i, iEnd = scan.end(); for (i = scan.begin(); i != iEnd; ++i) { ssqx += i->real() * i->real(); sxy += i->real() * i->imag(); } double d = ssqx - n * xmean * xmean; // cout << "x*x Sum=" << ssqx << " \tpSum=" << sxy << "\td=" << d << endl; if (fabs(d) < 0.0001) return false; a = (sxy - n * xmean * ymean) / d; b = ymean - a * xmean; return true; }
int ConstraintArbiter::handle_timeout(const ACE_Time_Value &, const void*) { std::cout << "ConstraintArbiter-handle_timeout before Guard" << std::endl; Miro::Guard guard(mutex_); std::cout << "ConstraintArbiter-handle_timeout after Guard" << std::endl; timing_.start(); ConstraintArbiterParameters * params = const_cast<ConstraintArbiterParameters *> ( dynamic_cast<ConstraintArbiterParameters const *>(params_)); MIRO_ASSERT(params != NULL); Vector2d velocity; // std::FILE *logFile1; typedef std::vector<Behaviour *> BehaviourVector; // preinitialize the velocity space params->velocitySpace.clearAllEvals(); params->velocitySpace.clearCurvatureConstraints(); // let each behaviour calculate its velocity space MessageVector::const_iterator first, last = message_.end(); for(first = message_.begin(); first != last; ++first) { if ((*first)->active){ std::cout << "ConstraintArbiter before addEval" << std::endl; (*first)->id->addEvaluation(¶ms->velocitySpace); std::cout << "ConstraintArbiter after addEval" << std::endl; } } // calculate new velocity using the content of the velocity space std::cout << "ConstraintArbiter before applyFunction" << std::endl; velocity = params->velocitySpace.applyObjectiveFunctionToEval(); std::cout << "ConstraintArbiter after applyFunction" << std::endl; // cout << "LEFT: " << velocity.real() << " ::: " << velocity.imag() << endl; timing_.stop(); // set steering commands pMotion_->setLRVelocity(velocity.real(), velocity.imag()); if (Miro::Log::level() >= Miro::Log::LL_PRATTLE) { TimeStats stats; timing_.eval(stats); MIRO_DBG_OSTR(MIRO, LL_PRATTLE, "ConstraintArbiter eval time: " << std::endl << stats); } // logFile1 = std::fopen("velocityspace.log","a"); // for(int r_index = 0; r_index <= 2*(velocitySpace_.maxVelocity_/velocitySpace_.spaceResolution_)+1; r_index++) { // for(int l_index = 0; l_index <= 2*(velocitySpace_.maxVelocity_/velocitySpace_.spaceResolution_)+1; l_index++) { // fprintf(logFile1, "%d\n", velocitySpace_.velocitySpace_[l_index][r_index]); // } // } // fclose(logFile1); if (pSupplier_ && pSupplier_->subscribed(offerIndex_) && !skipDebug_) { unsigned int const headerSize = 12; unsigned int dataSize = params->velocitySpace.dynamicWindowSize(); unsigned char * buffer = new unsigned char[headerSize + dataSize]; short * h = reinterpret_cast<short *>(buffer); h[0] = 0; // protocol id h[1] = params->velocitySpace.size(); params->velocitySpace.windowBounds(h[2], h[3], h[4], h[5]); unsigned char * p = buffer + headerSize; params->velocitySpace.dumpDynamicWindow(p); Miro::VelocitySpaceIDL * dynamicWindow = new Miro::VelocitySpaceIDL(headerSize + dataSize, headerSize + dataSize, buffer, 1); notifyEvent_.remainder_of_body <<= dynamicWindow; pSupplier_->sendEvent(notifyEvent_); } skipDebug_++; skipDebug_ %= skipMax_; return 0; }