$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/imarkers_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 #include <srs_interaction_primitives/billboard.h> 00031 00032 #include <pcl_ros/transforms.h> 00033 00034 00035 srs_env_model::CIMarkersPlugin::CIMarkersPlugin(const std::string & name) 00036 : srs_env_model::CServerPluginBase(name) 00037 , m_IMarkersFrameId(IM_SERVER_FRAME_ID) 00038 , m_serverTopicName( IM_SERVER_TOPIC_NAME ) 00039 , m_uniqueNameCounter(0) 00040 , m_bUseExternalServer( false ) 00041 { 00042 } 00043 00044 srs_env_model::CIMarkersPlugin::~CIMarkersPlugin() 00045 { 00046 if( ! m_bUseExternalServer ) 00047 { 00048 // Remove all markers 00049 m_imServer->clear(); 00050 } 00051 } 00052 00053 void srs_env_model::CIMarkersPlugin::init(ros::NodeHandle & node_handle) 00054 { 00055 // Get interactive markers server topic name 00056 node_handle.param("im_server_topic_name", m_serverTopicName, m_serverTopicName ); 00057 00058 // Get default frame id 00059 node_handle.param("im_server_frame_id", m_IMarkersFrameId, m_IMarkersFrameId ); 00060 00061 // Should external server be used 00062 node_handle.param("im_server_use_external_server", m_bUseExternalServer, false ); 00063 00064 // Use external server or start one? 00065 if( m_bUseExternalServer ) 00066 { 00067 // Connect to the services 00068 m_removeInteractiveMarkerService = node_handle.serviceClient<srs_interaction_primitives::RemovePrimitive> (srs_interaction_primitives::RemovePrimitive_SRV); 00069 m_addInteractivePlaneService = node_handle.serviceClient<srs_interaction_primitives::AddPlane> (srs_interaction_primitives::AddPlane_SRV); 00070 } 00071 else 00072 { 00073 // Initialize interactive markers server 00074 m_imServer.reset(new interactive_markers::InteractiveMarkerServer(m_serverTopicName, "", false)); 00075 } 00076 00077 // Advertise services 00078 m_serviceInsertPlanes = node_handle.advertiseService("insert_plane", &srs_env_model::CIMarkersPlugin::insertPlaneCallback, this); 00079 00080 00081 // Interactive marker server test 00082 /* 00083 { 00084 srs_env_model_msgs::PlaneDesc planedesc; 00085 00086 00087 // Positioning 00088 geometry_msgs::Pose p; 00089 p.position.x = p.position.y = p.position.z = 1.0; 00090 p.orientation.x = 0.0; p.orientation.y = p.orientation.z = p.orientation.w = 0.6; 00091 00092 // Scaling 00093 srs_interaction_primitives::Scale s; 00094 s.x = s.y = s.z = 10.0; 00095 00096 // Test plane insertion 00097 m_planesFrameId = m_IMarkersFrameId; 00098 planedesc.pose = p; 00099 planedesc.scale = s; 00100 planedesc.flags = srs_env_model_msgs::PlaneDesc::INSERT; 00101 planedesc.id = 0; 00102 00103 operatePlane( planedesc ); 00104 00105 // Test plane modification 00106 p.position.x = p.position.y = p.position.z = 1.0; 00107 p.orientation.x = p.orientation.y = p.orientation.z = p.orientation.w = 0.3; 00108 planedesc.pose = p; 00109 planedesc.flags = srs_env_model_msgs::PlaneDesc::MODIFY; 00110 00111 operatePlane( planedesc ); 00112 00113 PERROR( "Inserting plane"); 00114 } 00115 00116 m_imServer->applyChanges(); 00117 00118 //*/ 00119 } 00120 00121 00123 00129 bool srs_env_model::CIMarkersPlugin::insertPlaneCallback(srs_env_model::AddPlanes::Request & req, 00130 srs_env_model::AddPlanes::Response & res) { 00131 std::cerr << "Inset plane called" << std::endl; 00132 // Get plane array 00133 srs_env_model_msgs::PlaneArray & planea(req.plane_array); 00134 m_planesFrameId = planea.header.frame_id; 00135 std::vector<srs_env_model_msgs::PlaneDesc> & planes(planea.planes); 00136 std::vector<srs_env_model_msgs::PlaneDesc>::iterator i; 00137 00138 for (i = planes.begin(); i != planes.end(); ++i) { 00139 operatePlane(*i); 00140 } 00141 00142 return true; 00143 } 00144 00146 00152 void srs_env_model::CIMarkersPlugin::operatePlane(const srs_env_model_msgs::PlaneDesc & plane) { 00153 00154 std::string name; 00155 00156 switch (plane.flags) { 00157 case srs_env_model_msgs::PlaneDesc::INSERT: 00158 name = getUniqueName(); 00159 m_dataPlanes[plane.id] = tNamedPlane(name, plane); 00160 00161 addPlaneSrvCall(plane, name); 00162 // PERROR( "Added plane: " << "Name: " << name << std::endl << plane ); 00163 00164 break; 00165 00166 case srs_env_model_msgs::PlaneDesc::MODIFY: 00167 m_dataPlanes[plane.id].second = plane; 00168 name = m_dataPlanes[plane.id].first; 00169 00170 // call remove plane, add plane 00171 removePlaneSrvCall(plane, name); 00172 addPlaneSrvCall(plane, name); 00173 // PERROR( "Modified plane: " << "Name: " << name << std::endl << plane ); 00174 00175 break; 00176 00177 case srs_env_model_msgs::PlaneDesc::REMOVE: 00178 name = m_dataPlanes[plane.id].first; 00179 00180 m_dataPlanes.erase(plane.id); 00181 // call remove plane 00182 removePlaneSrvCall(plane, name); 00183 00184 // PERROR( "Removed plane: " << "Name: " << name << std::endl << plane ); 00185 00186 break; 00187 00188 default: 00189 break; 00190 } 00191 00192 // Apply changes 00193 if(!m_bUseExternalServer) 00194 m_imServer->applyChanges(); 00195 00196 } 00197 00199 00205 void srs_env_model::CIMarkersPlugin::addPlaneSrvCall(const srs_env_model_msgs::PlaneDesc & plane, 00206 const std::string & name) 00207 { 00208 if( m_bUseExternalServer ) 00209 { 00210 // Create service 00211 srs_interaction_primitives::AddPlane addPlaneSrv; 00212 00213 // Modify service 00214 addPlaneSrv.request.name = name; 00215 addPlaneSrv.request.frame_id = m_planesFrameId; 00216 addPlaneSrv.request.pose = plane.pose; 00217 addPlaneSrv.request.scale = plane.scale; 00218 addPlaneSrv.request.color.r = 1.0; 00219 addPlaneSrv.request.color.g = 0.0; 00220 addPlaneSrv.request.color.b = 0.0; 00221 addPlaneSrv.request.color.a = 0.8; 00222 00223 m_addInteractivePlaneService.call(addPlaneSrv); 00224 } 00225 else 00226 { 00227 // Creating Plane object with name "plane1" 00228 srs_interaction_primitives::Plane *p = new srs_interaction_primitives::Plane(m_imServer, m_IMarkersFrameId, name ); 00229 00230 p->setPose( plane.pose ); 00231 p->setScale( plane.scale ); 00232 00233 // Color 00234 std_msgs::ColorRGBA c; 00235 c.r = 1.0; 00236 c.g = c.b = 0.0; 00237 c.a = 1.0; 00238 p->setColor( c ); 00239 00240 p->insert(); 00241 00242 } 00243 00244 } 00245 00247 00253 void srs_env_model::CIMarkersPlugin::removePlaneSrvCall(const srs_env_model_msgs::PlaneDesc & plane, 00254 const std::string & name) 00255 { 00256 if( m_bUseExternalServer ) 00257 { 00258 // Create service 00259 srs_interaction_primitives::RemovePrimitive removeObjectSrv; 00260 00261 // Modify service 00262 removeObjectSrv.request.name = name; 00263 00264 m_removeInteractiveMarkerService.call(removeObjectSrv); 00265 } 00266 else 00267 { 00268 m_imServer->erase( name ); 00269 } 00270 } 00271 00273 00277 std::string srs_env_model::CIMarkersPlugin::getUniqueName() { 00278 std::stringstream ss; 00279 ss << "imn" << ++m_uniqueNameCounter; 00280 return ss.str(); 00281 }