Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <srs_env_model/but_server/plugins/old_imarkers_plugin.h>
00029 #include <srs_env_model/topics_list.h>
00030
00031 #include <pcl_ros/transforms.h>
00032 #include <srs_interaction_primitives/billboard.h>
00033
00034
00035 srs_env_model::COldIMarkersPlugin::COldIMarkersPlugin(const std::string & name)
00036 : srs_env_model::CServerPluginBase(name)
00037 , m_IMarkersFrameId(IM_SERVER_FRAME_ID)
00038 , m_serverTopicName(OLD_IM_SERVER_TOPIC_NAME)
00039 , m_uniqueNameCounter(0)
00040 , m_bUseExternalServer( false )
00041 {
00042 }
00043
00044 srs_env_model::COldIMarkersPlugin::~COldIMarkersPlugin()
00045 {
00046 if( ! m_bUseExternalServer )
00047 {
00048
00049 m_imServer->clear();
00050 }
00051 }
00052
00053 void srs_env_model::COldIMarkersPlugin::init(ros::NodeHandle & node_handle)
00054 {
00055
00056 node_handle.param("im_server_topic_name", m_serverTopicName, m_serverTopicName );
00057
00058
00059 node_handle.param("im_server_frame_id", m_IMarkersFrameId, m_IMarkersFrameId );
00060
00061
00062 node_handle.param("im_server_use_external_server", m_bUseExternalServer, false );
00063
00064
00065 if( m_bUseExternalServer )
00066 {
00067
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
00074 m_imServer.reset(new interactive_markers::InteractiveMarkerServer(m_serverTopicName, "", false));
00075 }
00076
00077
00078 m_serviceInsertPlanes = node_handle.advertiseService("insert_plane", &srs_env_model::COldIMarkersPlugin::insertPlaneCallback, this);
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119 }
00120
00121
00123
00129 bool srs_env_model::COldIMarkersPlugin::insertPlaneCallback(srs_env_model::AddPlanes::Request & req,
00130 srs_env_model::AddPlanes::Response & res) {
00131 std::cerr << "Inset plane called" << std::endl;
00132
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::COldIMarkersPlugin::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
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
00171 removePlaneSrvCall(plane, name);
00172 addPlaneSrvCall(plane, name);
00173
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
00182 removePlaneSrvCall(plane, name);
00183
00184
00185
00186 break;
00187
00188 default:
00189 break;
00190 }
00191
00192
00193 if(!m_bUseExternalServer)
00194 m_imServer->applyChanges();
00195
00196 }
00197
00199
00205 void srs_env_model::COldIMarkersPlugin::addPlaneSrvCall(const srs_env_model_msgs::PlaneDesc & plane,
00206 const std::string & name)
00207 {
00208 if( m_bUseExternalServer )
00209 {
00210
00211 srs_interaction_primitives::AddPlane addPlaneSrv;
00212
00213
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
00224 m_addInteractivePlaneService.call(addPlaneSrv);
00225 }
00226 else
00227 {
00228
00229 srs_interaction_primitives::Plane *p = new srs_interaction_primitives::Plane(m_imServer, m_IMarkersFrameId, name );
00230
00231 p->setPose( plane.pose );
00232 p->setScale( plane.scale );
00233
00234
00235 std_msgs::ColorRGBA c;
00236 c.r = 1.0;
00237 c.g = c.b = 0.0;
00238 c.a = 1.0;
00239 p->setColor( c );
00240
00241 p->insert();
00242
00243 }
00244
00245 }
00246
00248
00254 void srs_env_model::COldIMarkersPlugin::removePlaneSrvCall(const srs_env_model_msgs::PlaneDesc & plane,
00255 const std::string & name)
00256 {
00257 if( m_bUseExternalServer )
00258 {
00259
00260 srs_interaction_primitives::RemovePrimitive removeObjectSrv;
00261
00262
00263 removeObjectSrv.request.name = name;
00264
00265 m_removeInteractiveMarkerService.call(removeObjectSrv);
00266 }
00267 else
00268 {
00269 m_imServer->erase( name );
00270 }
00271 }
00272
00274
00278 std::string srs_env_model::COldIMarkersPlugin::getUniqueName() {
00279 std::stringstream ss;
00280 ss << "imn" << ++m_uniqueNameCounter;
00281 return ss.str();
00282 }
srs_env_model
Author(s): Vit Stancl (stancl@fit.vutbr.cz), Tomas Lokaj, Jan Gorig, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:50:49