コード例 #1
0
/*
 * 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();
	}


}
コード例 #2
0
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);
    }
}