map_t * requestCSpaceMap(const char *srv_name, const int free_threshold,
const int occupied_threshold) {
ros::NodeHandle nh;
// Taken from PAMCL
map_t* map = map_alloc();
ROS_ASSERT(map);
// get map via RPC
nav_msgs::GetMap::Request req;
nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...");
while(!ros::service::call(srv_name, req, resp)) {
ROS_WARN("Request for map '%s' failed; trying again...",
ros::names::resolve(string(srv_name)).c_str());
ros::Duration d(4.0);
d.sleep();
if (!nh.ok()) {
return NULL;
}
}
ROS_INFO_ONCE("Received a %d X %d map @ %.3f m/pix\n",
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution);
convertMap(resp.map, map, free_threshold, occupied_threshold);
return map;
}
void PrologEngine::call(QString proc,
const QVector<QVariant> &args,
std::function<void (QMap<QString, QVariant>)> callBack)
{
wam.Init();
QMap<QString, shared_ptr<Term::Term> > bindings;
for(int i=args.count()-1; i>=0; --i)
{
if(args[i].type() == QVariant::String && args[i].toString().startsWith("_"))
{
QString var = args[i].toString().mid(1);
shared_ptr<Term::Var> v = wam.newVar();
wam.operandStack.push(v);
bindings[var] = v;
}
else
{
wam.operandStack.push(termFromVariant(args[i]));
}
}
wam.Run(proc, bindings);
errors << wam.errors;
if(!errors.empty())
return;
for(auto i=wam.solutions.begin(); i != wam.solutions.end();++i)
{
QMap<QString,QVariant> m = convertMap(*i);
callBack(m);
}
}
void OccupancyMap::setMap(const nav_msgs::OccupancyGrid &grid) {
if (map_ != NULL) {
map_free(map_);
}
map_ = map_alloc();
ROS_ASSERT(map_);
convertMap(grid, map_, max_free_threshold_, min_occupied_threshold_);
}
