size_t Mapper2d::
RemoveOneObstacle(index_t source_index, draw_callback * cb)
{
index_buffer_t remove;
remove.insert(source_index);
return UpdateObstacles(0, false, &remove, cb);
}
//------------------------------------------------------------------------------------------
void cUpdateLayout::UpdateMoveCosts(ecs::cEntityWithData lvl, const glm::ivec2& pos)
{
rl::cLayout& lay = lvl->second.Component<cmp::cLevelData>()->mLayout;
auto cost_act = lay.mActors.Cells().View().IsClear(pos.x, pos.y) ? 0.0f : lay.mActors.Cells()(pos)->second.Component<ecs::cmp::cMoveCost>()->mMoveCost;
if (lay.mMoveCosts(pos) != cost_act)
{
lay.mMoveCosts(pos) = cost_act;
UpdateObstacles(lvl, pos);
evt::cStaticMoveCostUpdated::Sig().emit(lvl);
}
}
/**
\todo Removals should be done "en vrac", not one by
one. Multiscanner::raw_scan_collection_t is bad because it uses
the robot's pose, not each sensor's at the time of capture.
*/
size_t Mapper2d::
SwipedUpdate(const Frame & pose,
const Multiscanner::raw_scan_collection_t & scans,
double max_remove_distance,
draw_callback * cb)
{
m_freespace_buffer.clear();
m_obstacle_buffer.clear();
m_swipe_check_buffer.clear();
swipe_cb swipe(m_swipe_check_buffer, m_freespace_buffer, *m_travmap);
ssize_t const bbx0(m_travmap->grid.xbegin());
ssize_t const bbx1(m_travmap->grid.xend());
ssize_t const bby0(m_travmap->grid.ybegin());
ssize_t const bby1(m_travmap->grid.yend());
// compute sets of swiped and obstacle cells
for (size_t is(0); is < scans.size(); ++is) {
double const spx(scans[is]->scanner_pose.X());
double const spy(scans[is]->scanner_pose.Y());
double const spt(scans[is]->scanner_pose.Theta());
Scan::array_t const & scan_data(scans[is]->data);
const index_t i0(gridframe.GlobalIndex(spx, spy));
for (size_t ir(0); ir < scan_data.size(); ++ir) {
index_t const ihit(gridframe.GlobalIndex(scan_data[ir].globx, scan_data[ir].globy));
index_t iswipe;
if (scan_data[ir].rho <= max_remove_distance) {
// swipe up to the laser point
iswipe = ihit;
}
else {
// swipe along ray, but stop after max_remove_distance
double const theta(spt + scan_data[ir].phi);
iswipe = gridframe.GlobalIndex(spx + max_remove_distance * cos(theta),
spy + max_remove_distance * sin(theta));
}
gridframe.DrawDDALine(i0.v0, i0.v1, iswipe.v0, iswipe.v1,
bbx0, bbx1, bby0, bby1,
swipe);
if (scan_data[ir].in_range) {
// always insert them into m_obstacle_buffer, do not check
// m_travmap->IsWObst(ihit.v0, ihit.v1), because otherwise
// neighboring rays can erase known W-obstacles
PDEBUG("in range W-obst: global %g %g index %zd %zd\n",
scan_data[ir].globx, scan_data[ir].globy,
ihit.v0, ihit.v1);
m_obstacle_buffer.insert(ihit);
}
}
}
// remove new obstacles from swiped set (due to grid effects, it
// would otherwise be possible for a later ray to erase an
// obstacle seen by an earlier ray)
for (index_buffer_t::const_iterator io(m_obstacle_buffer.begin());
io != m_obstacle_buffer.end(); ++io)
m_freespace_buffer.erase(*io);
return UpdateObstacles(&m_obstacle_buffer, false, &m_freespace_buffer, cb);
}
