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