boost::shared_ptr<carrot_trace> Esbot::
ComputeFullCarrot() const
{
// can only compute full carrot if the partial one exists...
if((!m_carrot_trace) || m_carrot_trace->empty()){
PVDEBUG("m_carrot_trace.empty()\n");
return shared_ptr<carrot_trace>();
}
shared_ptr<const GridFrame> gframe(m_pnf->GetGridFrame());
double robx(m_pose->X());
double roby(m_pose->Y());
gframe->From(robx, roby);
const double distance(sqrt(sqr(m_goal->X() - m_pose->X())
+ sqr(m_goal->Y() - m_pose->Y()))
- 0.5 * m_goal->Dr());
const double stepsize(0.2); // magic numbers stink
const size_t maxnsteps(static_cast<size_t>(ceil(2 * distance / stepsize)));
shared_ptr<carrot_trace> trace(new carrot_trace);
const int result(m_pnf->GetFlow()->GetPNF().
TraceCarrot(robx, roby,
distance, stepsize, maxnsteps, *trace, 0));
PVDEBUG("d: %g s: %g n: %lu result: %d\n",
distance, stepsize, maxnsteps, result);
if(0 <= result)
return trace;
return shared_ptr<carrot_trace>();
}
bool Esbot::
PrepareAction(double timestep)
{
m_pose->Set(*m_odometry->Get());
m_front->Update();
m_rear->Update();
if(m_replan_request){
m_replan_request = false;
m_pnf.reset(new PNF(m_pose->X(), m_pose->Y(), m_radius, m_speed,
m_goal->X(), m_goal->Y(), m_goal->Dr(),
m_grid_width, m_grid_wdim, m_enable_thread));
PVDEBUG("static lines...\n");
m_cheat->UpdateLines();
bool ok(false);
for(size_t il(0); il < m_cheat->line.size(); ++il)
if(m_pnf->AddStaticLine(m_cheat->line[il].x0, m_cheat->line[il].y0,
m_cheat->line[il].x1, m_cheat->line[il].y1))
ok = true;
if( ! ok){
cerr << __func__ << "(): oops AddStaticLine [not able to do without]\n";
exit(EXIT_FAILURE);
}
m_cheat->UpdateDynobjs();
PVDEBUG("%zu dynamic objects...\n", m_cheat->dynobj.size());
ok = false;
for(size_t ir(0); ir < m_cheat->dynobj.size(); ++ir)
if(m_pnf->SetDynamicObject(ir, m_cheat->dynobj[ir].x,
m_cheat->dynobj[ir].y,
m_cheat->dynobj[ir].r, m_speed)){
PVDEBUG("OK: [%zu] %g %g %g\n", ir, m_cheat->dynobj[ir].x,
m_cheat->dynobj[ir].y, m_cheat->dynobj[ir].r);
ok = true;
}
else
PVDEBUG("fail: [%zu] %g %g %g\n", ir, m_cheat->dynobj[ir].x,
m_cheat->dynobj[ir].y, m_cheat->dynobj[ir].r);
if( ! ok){
cerr << __func__ << "(): oops SetDynamicObject [cannot do without]\n";
exit(EXIT_FAILURE);
}
m_pnf->StartPlanning();
m_dynamicWindow->GoSlow();
PVDEBUG("DONE\n");
}
static PNF::step_t prevstep(static_cast<PNF::step_t>(PNF::DONE + 1));
const PNF::step_t step(m_pnf->GetStep(true));
if(step != prevstep)
cerr << "**************************************************\n"
<< __func__ << "(): step " << prevstep << " ==> " << step << "\n";
prevstep = step;
m_carrot_trace->clear();
// default: go straight for goal
double carx(m_goal->X() - m_pose->X());
double cary(m_goal->Y() - m_pose->Y());
if(PNF::DONE == step){
PVDEBUG("(PNF::DONE == step)\n");
shared_ptr<const GridFrame> gframe(m_pnf->GetGridFrame());
double robx(m_pose->X());
double roby(m_pose->Y());
PVDEBUG("global robot : %g %g\n", robx, roby);
gframe->From(robx, roby);
PVDEBUG("grid local robot : %g %g\n", robx, roby);
const double carrot_distance(1.2); // XXX to do: magic numbers...
const double carrot_stepsize(0.3);
const size_t carrot_maxnsteps(30);
const int result(m_pnf->GetFlow()->GetPNF().
TraceCarrot(robx, roby,
carrot_distance, carrot_stepsize,
carrot_maxnsteps, *m_carrot_trace, 0));
if(0 <= result){
if(1 == result)
PVDEBUG("WARNING: carrot didn't reach distance %g\n", carrot_distance);
PVDEBUG("carrot.value = %g\n", m_carrot_trace->back().value);
if(m_carrot_trace->back().value <= 3 * carrot_stepsize){
// could just as well simply keep (carx, cary) untouched, but
// we want to see this adaption in the CarrotDrawing... his is
// really "dumb" because we undo the same trasformations right
// afterwards, but well.
PVDEBUG("carrot on goal border, appending goal point to carrot");
double foox(m_goal->X());
double fooy(m_goal->Y());
gframe->From(foox, fooy);
m_carrot_trace->push_back(carrot_item(foox, fooy, 0, 0, 0, true));
}
carx = m_carrot_trace->back().cx;
cary = m_carrot_trace->back().cy;
PVDEBUG("grid local carrot : %g %g\n", carx, cary);
gframe->To(carx, cary);
PVDEBUG("global carrot : %g %g\n", carx, cary);
carx -= m_pose->X();
cary -= m_pose->Y();
PVDEBUG("global delta carrot : %g %g\n", carx, cary);
}
else
PVDEBUG("FAILED compute_carrot()\n");
}
else
PVDEBUG("no local goal can be determined\n");
m_pose->RotateFrom(carx, cary);
PVDEBUG("robot local delta carrot: %g %g\n", carx, cary);
// pure rotation hysteresis
static const double start_aiming(10 * M_PI / 180); // to do: magic numbers
static const double start_homing(2 * M_PI / 180); // to do: magic numbers
const double dhead(atan2(cary, carx));
if(sfl::absval(dhead) < start_homing)
m_dynamicWindow->GoFast();
else if(sfl::absval(dhead) > start_aiming)
m_dynamicWindow->GoSlow();
shared_ptr<const Scan> scan(m_multiscanner->CollectScans());
double qdl, qdr;
m_motionController->GetCurrentAct(qdl, qdr);
m_dynamicWindow->Update(qdl, qdr, timestep, carx, cary, scan);
if( ! m_dynamicWindow->OptimalActuators(qdl, qdr)){
qdl = 0;
qdr = 0;
}
m_motionController->ProposeActuators(qdl, qdr);
m_motionController->Update(timestep);
m_odometry->Update();
return true;
}
void code()
{
int x1,y1,cl;
int x=28,y=4;
int p=28,q=4;
int r=28,s=4;
int i=28;
int j=0;
char c;
textcolor(0);
do
{
if(y+5>17&&x==i)
{ gover();
getch();
exit(1);
}
if(q+5>17&&p==i)
{ gover();
getch();
exit(1);
}
if(s+5>17&&r==i)
{ gover();
getch();
exit(1);
}
clrscr();
gframe();
if (j>7)
{
if(q==17)
{ q=4;
p=28+(7*(j%3));
}
textcolor(15);
object(p,q);
q++;
}
if (j>15)
{
if(s==17)
{ s=4;
r=28+(7*((j+1)%3));
}
textcolor(15);
object(r,s);
s++;
}
if(y==17)
{ y=4;
x=28+(7*((j+2)%3));
}
textcolor(15);
// object(x,y);
y++;
textcolor(1);
gotoxy(i,18);
cprintf(" ^ ^ ");
gotoxy(i,19);
cprintf("||@*@||");
gotoxy(i,20);
cprintf("||***||");
gotoxy(i,21);
cprintf(" *** ");
gotoxy(i,22);
cprintf(" *** ");
gotoxy(i,23);
cprintf("||***||");
gotoxy(i,24);
cprintf("||***||");
mouseposi(&x,&y,&cl);
if(cl==1&&i>34)
i=i-7;
if(cl==2&&i<36)
i=i+7;
j++;
delay(300) ;
}while(!kbhit());
}