/* * 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); }
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); } }