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>();
}
size_t Mapper2d::
AddOneObstacle(index_t source_index, bool force, draw_callback * cb)
{
ssize_t const ix0(source_index.v0);
ssize_t const iy0(source_index.v1);
// We can skip this one if it is already a W-space obstacle (and
// we are allowed to).
if (( ! force) && (m_travmap->IsWObst(ix0, iy0))) {
PVDEBUG("source %zd %zd is already W-obst\n", ix0, iy0);
return 0;
}
// maybe grow the grid...
ssize_t const bbx0(m_addmask_x0 + ix0);
ssize_t const bby0(m_addmask_y0 + iy0);
ssize_t const bbx1(m_addmask_x1 + ix0);
ssize_t const bby1(m_addmask_y1 + iy0);
(*m_grow_strategy)(*m_travmap, bbx0, bby0);
(*m_grow_strategy)(*m_travmap, bbx0, bby1);
(*m_grow_strategy)(*m_travmap, bbx1, bby0);
(*m_grow_strategy)(*m_travmap, bbx1, bby1);
// Ready to insert W-space obstacle and perform C-space expansion
m_travmap->SetWObst(ix0, iy0, cb);
size_t count(1);
PVDEBUG("added W-obst at source %zd %zd\n", ix0, iy0);
// Perform C-space extension with buffer zone.
for (addmask_t::const_iterator is(m_addmask.begin()); is != m_addmask.end(); ++is) {
ssize_t const ix(ix0 + is->first.v0);
ssize_t const iy(iy0 + is->first.v1);
if ((ix0 == ix) && (iy0 == iy))
continue; // we've already flagged the center
int old_value;
if (m_travmap->GetValue(ix, iy, old_value) // bound check in here
&& (is->second > old_value)) {
m_travmap->SetValue(ix, iy, is->second, cb);
++count;
PVDEBUG("added C-obst at target %zd %zd, cost %d (old cost %d)\n",
ix, iy, is->second, old_cost);
}
}
PVDEBUG("changed %zu cells\n", count);
return count;
}
void dump_grid_range_highlight(const Grid & grid,
ssize_t ix0, ssize_t iy0,
ssize_t ix1, ssize_t iy1,
ssize_t ixhigh, ssize_t iyhigh,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu %zu %zu\n",
ix0, iy0, ix1, iy1, ixhigh, iyhigh);
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
ssize_t iy(iy1);
for(/**/; iy != iy0; --iy){
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
}
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
linesep(grid, stream, ix0, ix1, 0, " ");
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
}
void Simulator::
UpdateRobots()
{
PVDEBUG("updating robots...\n");
UpdateAllSensors();
for(robot_t::iterator ir(m_robot.begin()); ir != m_robot.end(); ++ir){
ir->runnable = ir->client->PrepareAction(m_timestep);
if(( ! ir->runnable) && m_continuous){
m_continuous = false;
m_step = true;
}
}
for(robot_t::iterator ir(m_robot.begin()); ir != m_robot.end(); ++ir)
if(ir->runnable)
ir->server->SimulateAction(m_timestep);
for(robot_t::iterator ir(m_robot.begin()); ir != m_robot.end(); ++ir){
if( ! ir->runnable)
continue;
if( ! ir->client->GoalReached())
continue;
if(ir->client->m_goals.size() <= 1)
continue;
ir->goalidx = (ir->goalidx + 1) % ir->client->m_goals.size();
ir->client->SetGoal(m_timestep, ir->client->m_goals[ir->goalidx]);
}
}
void Esbot::
SetGoal(double timestep, const Goal & goal)
{
PVDEBUG("%g %g %g %g %g\n", goal.X(), goal.Y(), goal.Theta(),
goal.Dr(), 180 * goal.Dtheta() / M_PI);
m_goal->Set(goal);
m_replan_request = true;
}
void Queue::
Requeue(vertex_t vertex,
flag_map_t & flag_map,
const value_map_t & value_map,
const rhs_map_t & rhs_map)
{
const double value(get(value_map, vertex));
const double rhs(get(rhs_map, vertex));
const flag_t flag(get(flag_map, vertex));
if(absval(value - rhs) < epsilon){
PVDEBUG("CONSISTENT f: %s i: %lu v: %g rhs: %g\n",
flag_name(flag), vertex, value, rhs);
if(flag & OPEN){
DoDequeue(vertex, m_queue.begin());
m_map.erase(vertex);
put(flag_map, vertex, static_cast<flag_t>(flag ^ OPEN));
}
return;
}
const double key(minval(value, rhs));
if( ! (flag & OPEN)){
m_queue.insert(make_pair(key, vertex));
m_map.insert(make_pair(vertex, key));
put(flag_map, vertex, static_cast<flag_t>(flag | OPEN));
PVDEBUG("ENQUEUE f: %s i: %lu v: %g rhs: %g\n",
flag_name(flag), vertex, value, rhs);
return;
}
queue_map_t::iterator im(m_map.find(vertex));
BOOST_ASSERT( im != m_map.end() );
if(absval(im->second - key) < epsilon){
PVDEBUG("KEEP f: %s i: %lu v: %g rhs: %g (absval(%g) < %g)\n",
flag_name(flag), vertex, value, rhs, im->second - key, epsilon);
return;
}
PVDEBUG("REQUEUE f: %s i: %lu v: %g rhs: %g\n",
flag_name(flag), vertex, value, rhs);
DoDequeue(vertex, m_queue.find(im->second));
m_queue.insert(make_pair(key, vertex));
im->second = key;
}
virtual void operator () (ssize_t ix, ssize_t iy)
{
sfl::Mapper2d::index_t const ii(ix, iy);
swipe_buffer.insert(ii);
if (travmap.IsWObst(ix, iy)) {
PVDEBUG("swipe_cb on W-obst %zd %zd\n", ix, iy);
freespace_buffer.insert(ii);
}
}
vertex_t Queue::
Pop(flag_map_t & flag_map)
{
BOOST_ASSERT( ! m_queue.empty() );
queue_it iq(m_queue.begin());
const vertex_t vertex(iq->second);
m_queue.erase(iq);
m_map.erase(vertex);
put(flag_map, vertex, static_cast<flag_t>(get(flag_map, vertex) ^ OPEN));
PVDEBUG("f: %s i: %lu\n", flag_name(get(flag_map, vertex)), vertex);
return vertex;
}
void dump_grid_range(const Grid & grid,
ssize_t ix0, ssize_t iy0, ssize_t ix1, ssize_t iy1,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu\n", ix0, iy0, ix1, iy1);
const char * even("");
const char * oddsep;
const char * oddpre;
if (grid.GetNeighborhood() == Grid::SIX) {
oddsep = "+-----";
oddpre = " ";
}
else {
oddsep = even;
oddpre = even;
}
ssize_t iy(iy1);
const char * prefix;
for(/**/; iy != iy0; --iy){
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
}
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
if(iy % 2)
linesep(grid, stream, ix0, ix1, even, 0);
else
linesep(grid, stream, ix0, ix1, oddsep, 0);
}
bool Simulator::
Idle()
{
PVDEBUG("Hello!\n");
bool retval(false);
if(m_step || m_continuous){
if(m_step)
m_step = false;
UpdateRobots();
retval = true;
}
if(m_printscreen)
for (size_t ii(0); ii < m_appwin.size(); ++ii)
m_appwin[ii]->PrintScreen();
return retval;
}
void Queue::
DoDequeue(vertex_t vertex, queue_t::iterator iq)
{
for(/**/; iq != m_queue.end(); ++iq){
if(iq->second == vertex)
break;
}
if(iq == m_queue.end()){
PVDEBUG("WARNING search again!\n");
for(iq = m_queue.begin(); iq != m_queue.end(); ++iq){
if(iq->second == vertex)
break;
}
}
BOOST_ASSERT( iq != m_queue.end() );
m_queue.erase(iq);
}
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;
}
ValueColorScheme(double _vmax): vmax(_vmax) {
PVDEBUG("vmax = %g\n", vmax);
}
