$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <srs_env_model/but_server/plugins/collision_grid_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 00031 #include <pcl_ros/transforms.h> 00032 00033 00034 srs_env_model::CCollisionGridPlugin::CCollisionGridPlugin(const std::string & name) 00035 : srs_env_model::CServerPluginBase(name) 00036 , m_publishGrid(true) 00037 , m_gridPublisherName(COLLISIONGRID_PUBLISHER_NAME) 00038 , m_latchedTopics(false) 00039 , m_minSizeX(0.0) 00040 , m_minSizeY(0.0) 00041 { 00042 assert( m_data != 0 ); 00043 } 00044 00045 00046 00047 srs_env_model::CCollisionGridPlugin::~CCollisionGridPlugin() 00048 { 00049 } 00050 00051 00052 00053 bool srs_env_model::CCollisionGridPlugin::shouldPublish() 00054 { 00055 return( m_publishGrid && m_gridPublisher.getNumSubscribers() > 0 ); 00056 } 00057 00058 00059 00060 void srs_env_model::CCollisionGridPlugin::init(ros::NodeHandle & node_handle) 00061 { 00062 node_handle.param("map2d_publisher", m_gridPublisherName, COLLISIONGRID_PUBLISHER_NAME ); 00063 int depth(m_crawlDepth); 00064 node_handle.param("map2d_tree_depth", depth, depth ); 00065 m_crawlDepth = (depth < 0)?0:depth; 00066 node_handle.param("map2d_min_x_size", m_minSizeX, m_minSizeX); 00067 node_handle.param("map2d_min_y_size", m_minSizeY, m_minSizeY); 00068 00069 // Create publisher 00070 m_gridPublisher = node_handle.advertise<nav_msgs::OccupancyGrid> (m_gridPublisherName, 5, m_latchedTopics); 00071 } 00072 00073 00074 00075 void srs_env_model::CCollisionGridPlugin::publishInternal(const ros::Time & timestamp) 00076 { 00077 boost::mutex::scoped_lock lock(m_lockData); 00078 00079 if( shouldPublish() ) 00080 m_gridPublisher.publish(*m_data); 00081 } 00082 00083 00084 00085 void srs_env_model::CCollisionGridPlugin::newMapDataCB(SMapWithParameters & par) 00086 { 00087 // init projected 2D map: 00088 m_data->header.frame_id = par.frameId; 00089 m_data->header.stamp = par.currentTime; 00090 m_crawlDepth = par.treeDepth; 00091 bool sizeChanged(false); 00092 00093 // TODO: move most of this stuff into c'tor and init map only once (adjust if size changes) 00094 double minX, minY, minZ, maxX, maxY, maxZ; 00095 par.map->getTree().getMetricMin(minX, minY, minZ); 00096 par.map->getTree().getMetricMax(maxX, maxY, maxZ); 00097 00098 octomap::point3d minPt(minX, minY, minZ); 00099 octomap::point3d maxPt(maxX, maxY, maxZ); 00100 octomap::OcTreeKey minKey, maxKey, curKey; 00101 if (!par.map->getTree().genKey(minPt, minKey)){ 00102 ROS_ERROR("Could not create min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); 00103 return; 00104 } 00105 00106 if (!par.map->getTree().genKey(maxPt, maxKey)){ 00107 ROS_ERROR("Could not create max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); 00108 return; 00109 } 00110 par.map->getTree().genKeyAtDepth(minKey, par.treeDepth, minKey); 00111 par.map->getTree().genKeyAtDepth(maxKey, par.treeDepth, maxKey); 00112 00113 ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]); 00114 00115 // add padding if requested (= new min/maxPts in x&y): 00116 double halfPaddedX = 0.5*m_minSizeX; 00117 double halfPaddedY = 0.5*m_minSizeY; 00118 minX = std::min(minX, -halfPaddedX); 00119 maxX = std::max(maxX, halfPaddedX); 00120 minY = std::min(minY, -halfPaddedY); 00121 maxY = std::max(maxY, halfPaddedY); 00122 minPt = octomap::point3d(minX, minY, minZ); 00123 maxPt = octomap::point3d(maxX, maxY, maxZ); 00124 00125 octomap::OcTreeKey paddedMaxKey; 00126 if (!par.map->getTree().genKey(minPt, m_paddedMinKey)){ 00127 ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); 00128 return; 00129 } 00130 if (!par.map->getTree().genKey(maxPt, paddedMaxKey)){ 00131 ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); 00132 return; 00133 } 00134 par.map->getTree().genKeyAtDepth(m_paddedMinKey, par.treeDepth, m_paddedMinKey); 00135 par.map->getTree().genKeyAtDepth(paddedMaxKey, par.treeDepth, paddedMaxKey); 00136 00137 ROS_DEBUG("Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]); 00138 assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]); 00139 00140 m_multires2DScale = 1 << (par.treeDepth - m_crawlDepth); 00141 unsigned int newWidth = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1; 00142 unsigned int newHeight = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1; 00143 00144 if (newWidth != m_data->info.width || newHeight != m_data->info.height) 00145 sizeChanged = true; 00146 00147 m_data->info.width = newWidth; 00148 m_data->info.height = newHeight; 00149 int mapOriginX = minKey[0] - m_paddedMinKey[0]; 00150 int mapOriginY = minKey[1] - m_paddedMinKey[1]; 00151 assert(mapOriginX >= 0 && mapOriginY >= 0); 00152 00153 // might not exactly be min / max of octree: 00154 octomap::point3d origin; 00155 par.map->getTree().genCoords(m_paddedMinKey, m_crawlDepth, origin); 00156 double gridRes = par.map->getTree().getNodeSize(par.treeDepth); 00157 m_data->info.resolution = gridRes; 00158 m_data->info.origin.position.x = origin.x() - gridRes*0.5; 00159 m_data->info.origin.position.y = origin.y() - gridRes*0.5; 00160 00161 // std::cerr << "Origin: " << origin << ", grid resolution: " << gridRes << ", computed origin: "<< mapOriginX << ".." << mapOriginY << std::endl; 00162 00163 if (par.treeDepth != m_crawlDepth){ 00164 m_data->info.origin.position.x -= par.resolution/2.0; 00165 m_data->info.origin.position.y -= par.resolution/2.0; 00166 } 00167 00168 if (sizeChanged){ 00169 ROS_INFO("2D grid map size changed to %d x %d", m_data->info.width, m_data->info.height); 00170 m_data->data.clear(); 00171 // init to unknown: 00172 m_data->data.resize(m_data->info.width * m_data->info.height, -1); 00173 } 00174 00175 tButServerOcTree & tree( par.map->getTree() ); 00176 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00177 00178 // Crawl through nodes 00179 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00180 { 00181 // Node is occupied? 00182 if (tree.isNodeOccupied(*it)) 00183 { 00184 handleOccupiedNode(it, par); 00185 }// Node is occupied? 00186 else 00187 { 00188 handleFreeNode( it, par ); 00189 } 00190 00191 } // Iterate through octree 00192 00193 m_DataTimeStamp = par.currentTime; 00194 00195 invalidate(); 00196 } 00197 00201 void srs_env_model::CCollisionGridPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) 00202 { 00203 if (it.getDepth() == m_crawlDepth) 00204 { 00205 00206 octomap::OcTreeKey nKey = it.getKey(); // TODO: remove intermedate obj (1.4) 00207 int i = (nKey[0] - m_paddedMinKey[0])/m_multires2DScale;; 00208 int j = (nKey[1] - m_paddedMinKey[1])/m_multires2DScale;; 00209 m_data->data[m_data->info.width * j + i] = 100; 00210 00211 } else { 00212 00213 int intSize = 1 << (m_crawlDepth - it.getDepth()); 00214 octomap::OcTreeKey minKey = it.getIndexKey(); 00215 for (int dx = 0; dx < intSize; dx++) { 00216 int i = (minKey[0] + dx - m_paddedMinKey[0])/m_multires2DScale;; 00217 for (int dy = 0; dy < intSize; dy++) { 00218 int j = (minKey[1] + dy - m_paddedMinKey[1])/m_multires2DScale;; 00219 m_data->data[m_data->info.width * j + i] = 100; 00220 } 00221 } 00222 } 00223 } 00224 00228 void srs_env_model::CCollisionGridPlugin::handleFreeNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp ) 00229 { 00230 if (it.getDepth() == m_crawlDepth) { 00231 octomap::OcTreeKey nKey = it.getKey(); //TODO: remove intermedate obj (1.4) 00232 int i = (nKey[0] - m_paddedMinKey[0])/m_multires2DScale;; 00233 int j = (nKey[1] - m_paddedMinKey[1])/m_multires2DScale; 00234 if (m_data->data[m_data->info.width * j + i] == -1) { 00235 m_data->data[m_data->info.width * j + i] = 0; 00236 } 00237 } else { 00238 int intSize = 1 << (m_crawlDepth - it.getDepth()); 00239 octomap::OcTreeKey minKey = it.getIndexKey(); 00240 for (int dx = 0; dx < intSize; dx++) { 00241 int i = (minKey[0] + dx - m_paddedMinKey[0])/m_multires2DScale; 00242 for (int dy = 0; dy < intSize; dy++) { 00243 int j = (minKey[1] + dy - m_paddedMinKey[1])/m_multires2DScale; 00244 if (m_data->data[m_data->info.width * j + i] == -1) { 00245 m_data->data[m_data->info.width * j + i] = 0; 00246 } 00247 } 00248 } 00249 } 00250 } 00251 00255 void srs_env_model::CCollisionGridPlugin::pause( bool bPause, ros::NodeHandle & node_handle ) 00256 { 00257 if( bPause ) 00258 { 00259 m_gridPublisher.shutdown(); 00260 } 00261 else 00262 { 00263 // Create publisher 00264 m_gridPublisher = node_handle.advertise<nav_msgs::OccupancyGrid> (m_gridPublisherName, 5, m_latchedTopics); 00265 } 00266 }