$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_object_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 00031 #include <pcl_ros/transforms.h> 00032 00033 00034 srs_env_model::CCollisionObjectPlugin::CCollisionObjectPlugin(const std::string & name) 00035 : srs_env_model::CServerPluginBase(name) 00036 , m_publishCollisionObject(true) 00037 , m_coPublisherName(COLLISION_OBJECT_PUBLISHER_NAME) 00038 , m_latchedTopics(false) 00039 , m_coFrameId(COLLISION_OBJECT_FRAME_ID) 00040 , m_bConvert( false ) 00041 { 00042 assert( m_data != 0 ); 00043 } 00044 00045 00046 00047 srs_env_model::CCollisionObjectPlugin::~CCollisionObjectPlugin() 00048 { 00049 } 00050 00051 00052 00053 bool srs_env_model::CCollisionObjectPlugin::shouldPublish() 00054 { 00055 return( m_publishCollisionObject && m_coPublisher.getNumSubscribers() > 0 ); 00056 } 00057 00058 00059 00060 void srs_env_model::CCollisionObjectPlugin::init(ros::NodeHandle & node_handle) 00061 { 00062 node_handle.param("collision_object_publisher", m_coPublisherName, COLLISION_OBJECT_PUBLISHER_NAME ); 00063 node_handle.param("collision_object_frame_id", m_coFrameId, COLLISION_OBJECT_FRAME_ID ); 00064 00065 // Create publisher 00066 m_coPublisher = node_handle.advertise<arm_navigation_msgs::CollisionObject> (m_coPublisherName, 5, m_latchedTopics); 00067 } 00068 00069 00070 00071 void srs_env_model::CCollisionObjectPlugin::publishInternal(const ros::Time & timestamp) 00072 { 00073 if( m_coPublisher.getNumSubscribers() > 0 ) 00074 m_coPublisher.publish(*m_data); 00075 } 00076 00077 00078 00079 void srs_env_model::CCollisionObjectPlugin::newMapDataCB( SMapWithParameters & par ) 00080 { 00081 m_data->header.frame_id = m_coFrameId; 00082 m_data->header.stamp = par.currentTime; 00083 m_data->id = "map"; 00084 00085 m_ocFrameId = par.frameId; 00086 00087 tf::StampedTransform ocToCoTf; 00088 00089 m_bConvert = m_ocFrameId != m_coFrameId; 00090 00092 if( ! m_bConvert ) 00093 return; 00094 00095 // Get transform 00096 try { 00097 // Transformation - to, from, time, waiting time 00098 m_tfListener.waitForTransform(m_coFrameId, m_ocFrameId, 00099 par.currentTime, ros::Duration(5)); 00100 00101 m_tfListener.lookupTransform(m_coFrameId, m_ocFrameId, 00102 par.currentTime, ocToCoTf); 00103 00104 } catch (tf::TransformException& ex) { 00105 ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); 00106 PERROR("Transform error."); 00107 return; 00108 } 00109 00110 Eigen::Matrix4f ocToPcTM; 00111 00112 // Get transformation matrix 00113 pcl_ros::transformAsMatrix(ocToCoTf, ocToPcTM); 00114 00115 // Disassemble translation and rotation 00116 m_ocToCoRot = ocToPcTM.block<3, 3> (0, 0); 00117 m_ocToCoTrans = ocToPcTM.block<3, 1> (0, 3); 00118 00119 // Initialize leaf iterators 00120 tButServerOcTree & tree( par.map->getTree() ); 00121 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00122 00123 // Crawl through nodes 00124 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00125 { 00126 // Node is occupied? 00127 if (tree.isNodeOccupied(*it)) 00128 { 00129 handleOccupiedNode(it, par); 00130 }// Node is occupied? 00131 00132 } // Iterate through octree 00133 00134 m_DataTimeStamp = par.currentTime; 00135 00136 invalidate(); 00137 } 00138 00139 00140 00141 void srs_env_model::CCollisionObjectPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) 00142 { 00143 // Transform input point 00144 Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() ); 00145 00146 // Convert point if needed... 00147 if( m_bConvert ) 00148 point = m_ocToCoRot * point + m_ocToCoTrans; 00149 00150 // Add shape 00151 arm_navigation_msgs::Shape shape; 00152 shape.type = arm_navigation_msgs::Shape::BOX; 00153 shape.dimensions.resize(3); 00154 shape.dimensions[0] = shape.dimensions[1] = shape.dimensions[2] = it.getSize(); 00155 m_data->shapes.push_back(shape); 00156 00157 // Add pose 00158 geometry_msgs::Pose pose; 00159 pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 00160 pose.position.x = point.x(); 00161 pose.position.y = point.y(); 00162 pose.position.z = point.z(); 00163 m_data->poses.push_back(pose); 00164 } 00165 00166 00168 void srs_env_model::CCollisionObjectPlugin::pause( bool bPause, ros::NodeHandle & node_handle) 00169 { 00170 if( bPause ) 00171 m_coPublisher.shutdown(); 00172 else 00173 m_coPublisher = node_handle.advertise<arm_navigation_msgs::CollisionObject> (m_coPublisherName, 5, m_latchedTopics); 00174 }