/*
* build and publish a grid map from stored localized point clouds
*/
void GridConstructionNode::buildGrid (const ros::WallTimerEvent& scan)
{
if(clouds_.size()==0) return;
ROS_DEBUG( "Building grid map with %d scans",clouds_.size());
// Set up map dimensions
nm::MapMetaData info;
info.origin.position.x = -local_grid_size_/2; //odom_pose.pose.position.x-local_grid_size_/2;
info.origin.position.y = -local_grid_size_/2; //odom_pose.pose.position.y-local_grid_size_/2;
info.origin.orientation = tf::createQuaternionMsgFromYaw(0);
info.resolution = resolution_;
info.width = local_grid_size_/resolution_;
info.height = local_grid_size_/resolution_;
nm::OccupancyGrid fake_grid;
fake_grid.info = info;
gu::OverlayClouds overlay = gu::createCloudOverlay(fake_grid, fixed_frame_, 0.3, 10, 2);
vector<CloudConstPtr> clouds(clouds_.begin(), clouds_.end());
BOOST_FOREACH (CloudConstPtr cloud, clouds_)
gu::addCloud(&overlay, cloud);
nm::OccupancyGrid::ConstPtr grid = gu::getGrid(overlay);
grid_pub_.publish(grid);
}
/*
* callback for localized point clouds
*/
void GridConstructionNode::scanCallback (const gu::LocalizedCloud::Ptr& loc_cloud)
{
Lock lock(mutex_);
last_cloud_=loc_cloud;
if (last_cloud_)
{
if(last_cloud_->sensor_pose.position.z == -1.0)
{
ROS_INFO("Resetting map");
clouds_.clear();
}
else
clouds_.push_back(last_cloud_);
last_cloud_.reset();
}
}
void GridConstructionNode::buildGrid (const ros::WallTimerEvent& scan)
{
if (last_cloud_) {
{
Lock lock(mutex_);
clouds_.push_back(last_cloud_);
last_cloud_.reset();
}
ROS_DEBUG_NAMED ("build_grid", "Building grid with %zu scans", clouds_.size());
// Figure out current position
gm::PoseStamped identity, odom_pose;
identity.pose.orientation = tf::createQuaternionMsgFromYaw(0);
identity.header.frame_id = sensor_frame_;
identity.header.stamp = ros::Time();
tf_.transformPose(fixed_frame_, identity, odom_pose);
// Set up map dimensions
nm::MapMetaData info;
info.origin.position.x = odom_pose.pose.position.x-local_grid_size_/2;
info.origin.position.y = odom_pose.pose.position.y-local_grid_size_/2;
info.origin.orientation = tf::createQuaternionMsgFromYaw(0);
info.resolution = resolution_;
info.width = local_grid_size_/resolution_;
info.height = local_grid_size_/resolution_;
nm::OccupancyGrid fake_grid;
fake_grid.info = info;
gu::OverlayClouds overlay = gu::createCloudOverlay(fake_grid, fixed_frame_, 0.1, 10, 2);
vector<CloudConstPtr> clouds(clouds_.begin(), clouds_.end());
BOOST_FOREACH (CloudConstPtr cloud, clouds_)
gu::addCloud(&overlay, cloud);
nm::OccupancyGrid::ConstPtr grid = gu::getGrid(overlay);
ROS_DEBUG_NAMED ("build_grid", "Done building grid");
grid_pub_.publish(grid);
}
}
