$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/marker_array_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 00031 #include <pcl_ros/transforms.h> 00032 00033 00034 srs_env_model::CMarkerArrayPlugin::CMarkerArrayPlugin(const std::string & name) 00035 : srs_env_model::CServerPluginBase(name) 00036 , m_publishMarkerArray(true) 00037 , m_markerArrayPublisherName(MARKERARRAY_PUBLISHER_NAME) 00038 , m_latchedTopics(false) 00039 , m_markerArrayFrameId(MARKERARRAY_FRAME_ID) 00040 , m_bHeightMap( true ) 00041 , m_bTransform( false ) 00042 , m_colorFactor(0.8) 00043 { 00044 assert( m_data != 0 ); 00045 } 00046 00047 00048 00049 srs_env_model::CMarkerArrayPlugin::~CMarkerArrayPlugin() 00050 { 00051 } 00052 00053 00054 00055 bool srs_env_model::CMarkerArrayPlugin::shouldPublish() 00056 { 00057 return( m_publishMarkerArray && m_markerArrayPublisher.getNumSubscribers() > 0 ); 00058 } 00059 00060 00061 00062 void srs_env_model::CMarkerArrayPlugin::init(ros::NodeHandle & node_handle) 00063 { 00064 node_handle.param("marker_array_publisher", m_markerArrayPublisherName, MARKERARRAY_PUBLISHER_NAME ); 00065 node_handle.param("marker_array_frame_id", m_markerArrayFrameId, MARKERARRAY_FRAME_ID ); 00066 00067 double r, g, b, a; 00068 node_handle.param("color/r", r, 0.0); 00069 node_handle.param("color/g", g, 0.0); 00070 node_handle.param("color/b", b, 1.0); 00071 node_handle.param("color/a", a, 1.0); 00072 00073 m_color.r = r; 00074 m_color.g = g; 00075 m_color.b = b; 00076 m_color.a = a; 00077 00078 // Create publisher 00079 m_markerArrayPublisher = node_handle.advertise<visualization_msgs::MarkerArray> (m_markerArrayPublisherName, 5, m_latchedTopics); 00080 } 00081 00082 00083 00084 void srs_env_model::CMarkerArrayPlugin::publishInternal(const ros::Time & timestamp) 00085 { 00086 if( shouldPublish() ) 00087 m_markerArrayPublisher.publish(*m_data); 00088 } 00089 00090 00091 00092 void srs_env_model::CMarkerArrayPlugin::newMapDataCB(SMapWithParameters & par) 00093 { 00094 // each array stores all cubes of a different size, one for each depth level: 00095 m_data->markers.resize(par.treeDepth + 1); 00096 00097 m_ocFrameId = par.frameId; 00098 00099 // Get octomap parameters 00100 par.map->getTree().getMetricMin(m_minX, m_minY, m_minZ); 00101 par.map->getTree().getMetricMax(m_maxX, m_maxY, m_maxZ); 00102 00103 m_bTransform = m_ocFrameId != m_markerArrayFrameId; 00104 00105 // Is transform needed? 00106 if( ! m_bTransform ) 00107 return; 00108 00109 ros::Time timestamp( par.currentTime ); 00110 tf::StampedTransform ocToMarkerArrayTf; 00111 00112 00113 // Get transform 00114 try { 00115 // Transformation - to, from, time, waiting time 00116 m_tfListener.waitForTransform(m_markerArrayFrameId, m_ocFrameId, 00117 timestamp, ros::Duration(5)); 00118 00119 m_tfListener.lookupTransform(m_markerArrayFrameId, m_ocFrameId, 00120 timestamp, ocToMarkerArrayTf); 00121 00122 } catch (tf::TransformException& ex) { 00123 ROS_ERROR_STREAM("MarkerArrayPlugin: Transform error - " << ex.what() << ", quitting callback"); 00124 PERROR( "Transform error."); 00125 return; 00126 } 00127 00128 00129 Eigen::Matrix4f ocToMarkerArrayTM; 00130 00131 // Get transformation matrix 00132 pcl_ros::transformAsMatrix(ocToMarkerArrayTf, ocToMarkerArrayTM); 00133 00134 // Disassemble translation and rotation 00135 m_ocToMarkerArrayRot = ocToMarkerArrayTM.block<3, 3> (0, 0); 00136 m_ocToMarkerArrayTrans = ocToMarkerArrayTM.block<3, 1> (0, 3); 00137 00138 00139 tButServerOcTree & tree( par.map->getTree() ); 00140 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00141 00142 // Crawl through nodes 00143 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00144 { 00145 handleNode(it, par); 00146 } // Iterate through octree 00147 00148 handlePostNodeTraversal( par ); 00149 00150 m_DataTimeStamp = par.currentTime; 00151 00152 } 00153 00154 void srs_env_model::CMarkerArrayPlugin::handleNode(const srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) 00155 { 00156 unsigned idx = it.getDepth(); 00157 assert(idx < m_data->markers.size()); 00158 00159 geometry_msgs::Point cubeCenter; 00160 00161 00162 00163 if( m_bTransform ) 00164 { 00165 // Transform input point 00166 Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() ); 00167 point = m_ocToMarkerArrayRot * point + m_ocToMarkerArrayTrans; 00168 cubeCenter.x = it.getX(); 00169 cubeCenter.y = it.getY(); 00170 cubeCenter.z = it.getZ(); 00171 00172 }else{ 00173 cubeCenter.x = it.getX(); 00174 cubeCenter.y = it.getY(); 00175 cubeCenter.z = it.getZ(); 00176 } 00177 00178 m_data->markers[idx].points.push_back(cubeCenter); 00179 00180 if (m_bHeightMap){ 00181 double h = (1.0 - std::min(std::max((cubeCenter.z-m_minZ)/ (m_maxZ - m_minZ), 0.0), 1.0)) *m_colorFactor; 00182 m_data->markers[idx].colors.push_back(heightMapColor(h)); 00183 } 00184 } 00185 00186 00187 00188 void srs_env_model::CMarkerArrayPlugin::handlePostNodeTraversal(const SMapWithParameters & mp) 00189 { 00190 for (unsigned i= 0; i < m_data->markers.size(); ++i){ 00191 double size = mp.map->getTree().getNodeSize(i); 00192 00193 m_data->markers[i].header.frame_id = m_markerArrayFrameId; 00194 m_data->markers[i].header.stamp = mp.currentTime; 00195 m_data->markers[i].ns = "map"; 00196 m_data->markers[i].id = i; 00197 m_data->markers[i].type = visualization_msgs::Marker::CUBE_LIST; 00198 m_data->markers[i].scale.x = size; 00199 m_data->markers[i].scale.y = size; 00200 m_data->markers[i].scale.z = size; 00201 m_data->markers[i].color = m_color; 00202 00203 00204 if (m_data->markers[i].points.size() > 0) 00205 m_data->markers[i].action = visualization_msgs::Marker::ADD; 00206 else 00207 m_data->markers[i].action = visualization_msgs::Marker::DELETE; 00208 } 00209 00210 invalidate(); 00211 } 00212 00213 std_msgs::ColorRGBA srs_env_model::CMarkerArrayPlugin::heightMapColor(double h) const { 00214 00215 std_msgs::ColorRGBA color; 00216 color.a = 1.0; 00217 // blend over HSV-values (more colors) 00218 00219 double s = 1.0; 00220 double v = 1.0; 00221 00222 h -= floor(h); 00223 h *= 6; 00224 int i; 00225 double m, n, f; 00226 00227 i = floor(h); 00228 f = h - i; 00229 if (!(i & 1)) 00230 f = 1 - f; // if i is even 00231 m = v * (1 - s); 00232 n = v * (1 - s * f); 00233 00234 switch (i) { 00235 case 6: 00236 case 0: 00237 color.r = v; color.g = n; color.b = m; 00238 break; 00239 case 1: 00240 color.r = n; color.g = v; color.b = m; 00241 break; 00242 case 2: 00243 color.r = m; color.g = v; color.b = n; 00244 break; 00245 case 3: 00246 color.r = m; color.g = n; color.b = v; 00247 break; 00248 case 4: 00249 color.r = n; color.g = m; color.b = v; 00250 break; 00251 case 5: 00252 color.r = v; color.g = m; color.b = n; 00253 break; 00254 default: 00255 color.r = 1; color.g = 0.5; color.b = 0.5; 00256 break; 00257 } 00258 00259 return color; 00260 } 00261 00265 void srs_env_model::CMarkerArrayPlugin::pause( bool bPause, ros::NodeHandle & node_handle ) 00266 { 00267 if( bPause ) 00268 m_markerArrayPublisher.shutdown(); 00269 else 00270 m_markerArrayPublisher = node_handle.advertise<visualization_msgs::MarkerArray> (m_markerArrayPublisherName, 5, m_latchedTopics); 00271 }