MoveBaseSBPL::MoveBaseSBPL()
: MoveBase(),
env_(0),
pMgr_(0),
goalCount_(0)
{
try {
// We're throwing int exit values if something goes wrong, and
// clean up any new instances in the catch clause. The sentry
// gets destructed when we go out of scope, so unlock() gets
// called no matter what.
sentry<MoveBaseSBPL> guard(this);
local_param("planStatsFile", planStatsFile_, string("/tmp/move_base_sbpl.log"));
local_param("plannerTimeLimit", plannerTimeLimit_, 10.0);
/*
if (0 > plannerTimeLimit_) {
int blah;
local_param("plannerTimeLimit", blah, -1); // parameters are picky about dots
if (0 > blah) {
ROS_ERROR("invalid or no %s/plannerTimeLimit specified: %g",
get_name().c_str(), plannerTimeLimit_);
throw int(7);
}
plannerTimeLimit_ = blah;
}
*/
string environmentType;
local_param("environmentType", environmentType, string("2D"));
if ("2D" == environmentType) {
// Initial Configuration is set with the threshold for
// obstacles set to the inscribed obstacle threshold. These,
// lethal obstacles, and cells with no information will thus
// be regarded as obstacles
env_ = new ompl::EnvironmentWrapper2D(getCostMap(), 0, 0, 0, 0,
CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
}
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
string obst_cost_thresh_str;
local_param(prefix + "obst_cost_thresh", obst_cost_thresh_str, string("lethal"));
unsigned char obst_cost_thresh(0);
if ("lethal" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::LETHAL_OBSTACLE;
else if ("inscribed" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::INSCRIBED_INFLATED_OBSTACLE;
else if ("circumscribed" == obst_cost_thresh_str)
obst_cost_thresh = costmap_2d::CostMap2D::CIRCUMSCRIBED_INFLATED_OBSTACLE;
else {
ROS_ERROR("invalid env3d/obst_cost_thresh \"%s\"\n"
" valid options: lethal, inscribed, or circumscribed",
obst_cost_thresh_str.c_str());
throw int(6);
}
double goaltol_x, goaltol_y, goaltol_theta,
nominalvel_mpersecs, timetoturn45degsinplace_secs;
local_param(prefix + "goaltol_x", goaltol_x, 0.3);
local_param(prefix + "goaltol_y", goaltol_y, 0.3);
local_param(prefix + "goaltol_theta", goaltol_theta, 30.0);
local_param(prefix + "nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
local_param(prefix + "timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
// Could also sanity check the other parameters...
env_ = new ompl::EnvironmentWrapper3DKIN(getCostMap(), obst_cost_thresh,
0, 0, 0, // start (x, y, th)
0, 0, 0, // goal (x, y, th)
goaltol_x, goaltol_y, goaltol_theta,
getFootprint(), nominalvel_mpersecs,
timetoturn45degsinplace_secs);
}
else {
ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D or 3DKIN",
environmentType.c_str());
throw int(2);
}
// This (weird?) order of inits is historical, could maybe be
// cleaned up...
bool const success(env_->InitializeMDPCfg(&mdpCfg_));
// try {
// // This one throws a string if something goes awry... and
// // always returns true. So no use cecking its
// // retval. Another candidate for cleanup.
// isMapDataOK();
// }
// catch (char const * ee) {
// ROS_ERROR("in MoveBaseSBPL ctor: isMapDataOK() said %s", ee);
// throw int(3);
// }
if ( ! success) {
ROS_ERROR("in MoveBaseSBPL ctor: env_->InitializeMDPCfg() failed");
throw int(4);
}
string plannerType;
local_param("plannerType", plannerType, string("ARAPlanner"));
pMgr_ = new ompl::SBPLPlannerManager(env_->getDSI(), false, &mdpCfg_);
if ( ! pMgr_->select(plannerType, false, 0)) {
ROS_ERROR("in MoveBaseSBPL ctor: pMgr_->select(%s) failed", plannerType.c_str());
throw int(5);
}
pStat_.pushBack(plannerType, environmentType);
}
catch (int ii) {
delete env_;
delete pMgr_;
exit(ii);
}
//Now initialize
initialize();
}
Pairing * PairGeneratorSector::run(HitCollection & hits, const GeometrySupplement & geomSupplement,
uint nThreads, const TripletConfigurations & layerTriplets, const Grid & grid)
{
std::vector<uint> oracleOffset;
uint totalMaxPairs = 0;
uint nLayerTriplets = layerTriplets.size();
for(uint e = 0; e < grid.config.nEvents; ++e){
for(uint p = 0; p < nLayerTriplets; ++p){
TripletConfiguration layerPair(layerTriplets, p);
LayerGrid layer1(grid, layerPair.layer1(),e);
LayerGrid layer2(grid, layerPair.layer2(),e);
uint nMaxPairs = layer1.size()*layer2.size();
nMaxPairs = 32 * std::ceil(nMaxPairs / 32.0); //round to next multiple of 32
oracleOffset.push_back(totalMaxPairs);
totalMaxPairs += nMaxPairs;
}
}
LOG << "Initializing oracle offsets for pair gen...";
clever::vector<uint, 1> m_oracleOffset(oracleOffset, ctx);
LOG << "done[" << m_oracleOffset.get_count() << "]" << std::endl;
LOG << "Initializing oracle for pair gen...";
clever::vector<uint, 1> m_oracle(0, std::ceil(totalMaxPairs / 32.0), ctx);
LOG << "done[" << m_oracle.get_count() << "]" << std::endl;
LOG << "Initializing prefix sum for pair gen...";
clever::vector<uint, 1> m_prefixSum(0, grid.config.nEvents*nLayerTriplets*nThreads+1, ctx);
LOG << "done[" << m_prefixSum.get_count() << "]" << std::endl;
//ctx.select_profile_event(KERNEL_COMPUTE_EVT());
LOG << "Running pair gen kernel...";
cl_event evt = pairCount.run(
//configuration
layerTriplets.transfer.buffer(Layer1()), layerTriplets.transfer.buffer(Layer2()), grid.config.nLayers,
grid.transfer.buffer(Boundary()),
grid.config.MIN_Z, grid.config.sectorSizeZ(), grid.config.nSectorsZ,
grid.config.MIN_PHI, grid.config.sectorSizePhi(), grid.config.nSectorsPhi,
layerTriplets.transfer.buffer(pairSpreadZ()), layerTriplets.transfer.buffer(pairSpreadPhi()),
// hit input
hits.transfer.buffer(GlobalX()), hits.transfer.buffer(GlobalY()), hits.transfer.buffer(GlobalZ()),
// intermeditate data: oracle for hit pairs, prefix sum for found pairs
m_oracle.get_mem(), m_oracleOffset.get_mem(), m_prefixSum.get_mem(),
//local
local_param(sizeof(cl_uint), (grid.config.nSectorsZ+1)*(grid.config.nSectorsPhi+1)),
//thread config
range(nThreads, nLayerTriplets, grid.config.nEvents),
range(nThreads, 1,1));
PairGeneratorSector::events.push_back(evt);
LOG << "done" << std::endl;
if(PROLIX){
PLOG << "Fetching prefix sum for pair gen...";
std::vector<uint> vPrefixSum(m_prefixSum.get_count());
transfer::download(m_prefixSum,vPrefixSum,ctx);
PLOG << "done" << std::endl;
PLOG << "Prefix sum: ";
for(auto i : vPrefixSum){
PLOG << i << " ; ";
}
PLOG << std::endl;
}
if(PROLIX){
PLOG << "Fetching oracle for pair gen...";
std::vector<uint> oracle(m_oracle.get_count());
transfer::download(m_oracle,oracle,ctx);
PLOG << "done" << std::endl;
PLOG << "Oracle: ";
for(auto i : oracle){
PLOG << i << " ; ";
}
PLOG << std::endl;
}
//Calculate prefix sum
PrefixSum prefixSum(ctx);
evt = prefixSum.run(m_prefixSum.get_mem(), m_prefixSum.get_count(), nThreads, PairGeneratorSector::events);
uint nFoundPairs;
transfer::downloadScalar(m_prefixSum, nFoundPairs, ctx, true, m_prefixSum.get_count()-1, 1, &evt);
if(PROLIX){
PLOG << "Fetching prefix sum for pair gen...";
std::vector<uint> vPrefixSum(m_prefixSum.get_count());
transfer::download(m_prefixSum,vPrefixSum,ctx);
PLOG << "done" << std::endl;
PLOG << "Prefix sum: ";
for(auto i : vPrefixSum){
PLOG << i << " ; ";
}
PLOG << std::endl;
}
LOG << "Initializing pairs...";
Pairing * hitPairs = new Pairing(ctx, nFoundPairs, grid.config.nEvents, layerTriplets.size());
LOG << "done[" << hitPairs->pairing.get_count() << "]" << std::endl;
LOG << "Running pair gen store kernel...";
evt = pairStore.run(
//configuration
layerTriplets.transfer.buffer(Layer1()), layerTriplets.transfer.buffer(Layer2()), grid.config.nLayers,
//grid
grid.transfer.buffer(Boundary()), grid.config.nSectorsZ, grid.config.nSectorsPhi,
// input for oracle and prefix sum
m_oracle.get_mem(), m_oracleOffset.get_mem(), m_prefixSum.get_mem(),
// output of pairs
hitPairs->pairing.get_mem(), hitPairs->pairingOffsets.get_mem(),
//thread config
range(nThreads, nLayerTriplets, grid.config.nEvents),
range(nThreads, 1,1));
PairGeneratorSector::events.push_back(evt);
LOG << "done" << std::endl;
if(PROLIX){
PLOG << "Fetching pairs...";
std::vector<uint2> pairs = hitPairs->getPairings();
PLOG <<"done[" << pairs.size() << "]" << std::endl;
PLOG << "Pairs:" << std::endl;
for(uint i = 0; i < nFoundPairs; ++i){
PLOG << "[" << i << "] " << pairs[i].x << "-" << pairs[i].y << std::endl;
}
PLOG << "Fetching pair offets...";
std::vector<uint> pairOffsets = hitPairs->getPairingOffsets();
PLOG <<"done[" << pairOffsets.size() << "]" << std::endl;
PLOG << "Pair Offsets:" << std::endl;
for(uint i = 0; i < pairOffsets.size(); ++i){
PLOG << "[" << i << "] " << pairOffsets[i] << std::endl;
}
}
return hitPairs;
}
