diff --git a/crosbot_explore/src/nodes/astar.cpp b/crosbot_explore/src/nodes/astar.cpp index 4eb44b85913ce6d1d89331307984f4a157d45752..c4cf41bd29566e51e7b9e4760a5e3f7e2633dcff 100644 --- a/crosbot_explore/src/nodes/astar.cpp +++ b/crosbot_explore/src/nodes/astar.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -75,8 +76,10 @@ public: } }} - if (voronoi == NULL) + if (voronoi == NULL) { + ERROR("AstarNode::GetPath(): No voronoi map available.\n"); return false; + } ImagePtr image; if (imagePub.getNumSubscribers() > 0) { @@ -94,7 +97,7 @@ public: // return the plan res.path.header.stamp = ros::Time::now(); - std::string frame_id = latestMap->header.frame_id; + std::string frame_id = voronoi->frame; res.path.header.frame_id = frame_id; res.path.poses.resize(path.size()); for (size_t i = 0; i < path.size(); ++i) { @@ -102,7 +105,7 @@ public: pose.header.frame_id = frame_id; pose.pose = path[i].toROS(); } - + LOG("AstarNode::GetPath(): Returning a path with %d poses.\n", path.size()); return true; } @@ -110,8 +113,16 @@ public: // read configuration/parameters ros::NodeHandle paramNH("~"); // Because ROS's search order is buggered paramNH.param("base_frame", baseFrame, DEFAULT_BASEFRAME); - + ConfigElementPtr cfg = new ROSConfigElement(paramNH); + // TODO: read voronoi constraints + ConfigElementPtr voronoiCfg = cfg->getChild("voronoi"); + if (voronoiCfg != NULL) { + voronoiConstraints.restricted = voronoiCfg->getParamAsDouble("restrict", voronoiConstraints.restricted); + voronoiConstraints.partial = voronoiCfg->getParamAsDouble("partial", voronoiConstraints.partial); + voronoiConstraints.expand = voronoiCfg->getParamAsDouble("expand", voronoiConstraints.expand); + voronoiConstraints.orphanThreshold = voronoiCfg->getParamAsInt("orphan", voronoiConstraints.orphanThreshold); + } gridSub = nh.subscribe("map", 1, &AstarNode::callbackOccGrid, this); historySub = nh.subscribe("history", 1, &AstarNode::callbackHistory, this); diff --git a/crosbot_explore/src/nodes/explorer.cpp b/crosbot_explore/src/nodes/explorer.cpp index 82c1268759aa6e4c7c0472488dcb14e78acc227b..2a6e72a5f10aaf6e3e16ddf49e14672f3559a2c7 100644 --- a/crosbot_explore/src/nodes/explorer.cpp +++ b/crosbot_explore/src/nodes/explorer.cpp @@ -65,10 +65,11 @@ public: tf::StampedTransform transform; try { - tfListener.waitForTransform(voronoi->frame, req.path.header.frame_id, ros::Time::now(), ros::Duration(DEFAULT_MAXWAIT4TRANSFORM)); - tfListener.lookupTransform(voronoi->frame, req.path.header.frame_id, ros::Time::now(), transform); + ros::Time now = ros::Time::now(); + tfListener.waitForTransform(voronoi->frame, req.path.header.frame_id, now, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM)); + tfListener.lookupTransform(voronoi->frame, req.path.header.frame_id, now, transform); } catch (tf::TransformException& ex) { - ERROR("localmap: Error getting transform. (%s)\n", ex.what()); + ERROR("explorer: Error getting transform. (%s)\n", ex.what()); return false; } diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp index c08acf97c510a3d2fa1774123ff32a49ede30728..b120d3ac1c7fa748d719e2fc37abd2f09f92a1c2 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp @@ -113,8 +113,11 @@ public: if (gridPub.getNumSubscribers() > 0) { nav_msgs::OccupancyGridPtr grid = asOccupancyGrid(newMean); - if (grid != NULL) + if (grid != NULL) { + grid->header.frame_id = mapFrame; + grid->header.stamp = ros::Time::now(); gridPub.publish(grid); + } } // TODO: publish history