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 #ifndef STDR_SERVER_H
00023 #define STDR_SERVER_H
00024
00025 #define USAGE "\nUSAGE: stdr_server <map.yaml>\n" \
00026 " map.yaml: map description file\n"
00027
00028 #include <ros/ros.h>
00029 #include <actionlib/server/simple_action_server.h>
00030 #include <stdr_server/map_server.h>
00031 #include <stdr_msgs/LoadMap.h>
00032 #include <stdr_msgs/LoadExternalMap.h>
00033 #include <stdr_msgs/RegisterGui.h>
00034 #include <stdr_msgs/RegisterRobotAction.h>
00035 #include <stdr_msgs/SpawnRobotAction.h>
00036 #include <stdr_msgs/DeleteRobotAction.h>
00037 #include <stdr_msgs/RobotIndexedMsg.h>
00038 #include <stdr_msgs/RobotIndexedVectorMsg.h>
00039
00040 #include <visualization_msgs/Marker.h>
00041 #include <visualization_msgs/MarkerArray.h>
00042
00043 #include <stdr_msgs/RfidTagVector.h>
00044 #include <stdr_msgs/AddRfidTag.h>
00045 #include <stdr_msgs/DeleteRfidTag.h>
00046
00047 #include <stdr_msgs/CO2SourceVector.h>
00048 #include <stdr_msgs/AddCO2Source.h>
00049 #include <stdr_msgs/DeleteCO2Source.h>
00050
00051 #include <stdr_msgs/ThermalSourceVector.h>
00052 #include <stdr_msgs/AddThermalSource.h>
00053 #include <stdr_msgs/DeleteThermalSource.h>
00054
00055 #include <stdr_msgs/SoundSourceVector.h>
00056 #include <stdr_msgs/AddSoundSource.h>
00057 #include <stdr_msgs/DeleteSoundSource.h>
00058
00059 #include <nodelet/NodeletLoad.h>
00060 #include <nodelet/NodeletUnload.h>
00061
00066 namespace stdr_server {
00067
00068 typedef boost::shared_ptr<MapServer> MapServerPtr;
00069
00070 typedef actionlib::SimpleActionServer<stdr_msgs::SpawnRobotAction>
00071 SpawnRobotServer;
00072
00073 typedef actionlib::SimpleActionServer<stdr_msgs::RegisterRobotAction>
00074 RegisterRobotServer;
00075
00076 typedef actionlib::SimpleActionServer<stdr_msgs::DeleteRobotAction>
00077 DeleteRobotServer;
00078
00079 typedef std::map<std::string, stdr_msgs::RobotIndexedMsg> RobotMap;
00080
00081 typedef std::map<std::string, stdr_msgs::RfidTag> RfidTagMap;
00082 typedef std::map<std::string, stdr_msgs::RfidTag>::iterator RfidTagMapIt;
00083
00084 typedef std::map<std::string, stdr_msgs::CO2Source> CO2SourceMap;
00085 typedef std::map<std::string, stdr_msgs::CO2Source>::iterator CO2SourceMapIt;
00086
00087 typedef std::map<std::string, stdr_msgs::ThermalSource> ThermalSourceMap;
00088 typedef std::map<std::string, stdr_msgs::ThermalSource>::iterator
00089 ThermalSourceMapIt;
00090
00091 typedef std::map<std::string, stdr_msgs::SoundSource> SoundSourceMap;
00092 typedef std::map<std::string, stdr_msgs::SoundSource>::iterator
00093 SoundSourceMapIt;
00094
00099 class Server
00100 {
00101
00102 public:
00103
00110 Server(int argc, char** argv);
00111
00113
00120 bool loadMapCallback(stdr_msgs::LoadMap::Request& req,
00121 stdr_msgs::LoadMap::Response& res);
00122
00129 bool loadExternalMapCallback(stdr_msgs::LoadExternalMap::Request& req,
00130 stdr_msgs::LoadExternalMap::Response& res);
00131
00133
00139 void spawnRobotCallback(const stdr_msgs::SpawnRobotGoalConstPtr& goal);
00140
00146 void deleteRobotCallback(
00147 const stdr_msgs::DeleteRobotGoalConstPtr& goal);
00148
00154 void registerRobotCallback(
00155 const stdr_msgs::RegisterRobotGoalConstPtr& goal);
00156
00157 private:
00158
00163 void activateActionServers(void);
00164
00171 bool addNewRobot(stdr_msgs::RobotMsg description,
00172 stdr_msgs::SpawnRobotResult* result);
00173
00180 bool deleteRobot(std::string name, stdr_msgs::DeleteRobotResult* result);
00181
00188 bool addRfidTagCallback(
00189 stdr_msgs::AddRfidTag::Request &req,
00190 stdr_msgs::AddRfidTag::Response &res);
00191
00198 bool deleteRfidTagCallback(
00199 stdr_msgs::DeleteRfidTag::Request &req,
00200 stdr_msgs::DeleteRfidTag::Response &res);
00201
00208 bool addCO2SourceCallback(
00209 stdr_msgs::AddCO2Source::Request &req,
00210 stdr_msgs::AddCO2Source::Response &res);
00211
00218 bool deleteCO2SourceCallback(
00219 stdr_msgs::DeleteCO2Source::Request &req,
00220 stdr_msgs::DeleteCO2Source::Response &res);
00221
00228 bool addThermalSourceCallback(
00229 stdr_msgs::AddThermalSource::Request &req,
00230 stdr_msgs::AddThermalSource::Response &res);
00231
00238 bool deleteThermalSourceCallback(
00239 stdr_msgs::DeleteThermalSource::Request &req,
00240 stdr_msgs::DeleteThermalSource::Response &res);
00241
00248 bool addSoundSourceCallback(
00249 stdr_msgs::AddSoundSource::Request &req,
00250 stdr_msgs::AddSoundSource::Response &res);
00251
00258 bool deleteSoundSourceCallback(
00259 stdr_msgs::DeleteSoundSource::Request &req,
00260 stdr_msgs::DeleteSoundSource::Response &res);
00261
00262 bool hasDublicateFrameIds(const stdr_msgs::RobotMsg& robot,
00263 std::string &f_id);
00264
00271 visualization_msgs::Marker toMarker(const stdr_msgs::CO2Source& msg,bool added);
00272
00279 visualization_msgs::Marker toMarker(const stdr_msgs::ThermalSource& msg,bool added);
00280
00287 visualization_msgs::Marker toMarker(const stdr_msgs::SoundSource& msg,bool added);
00288
00295 visualization_msgs::Marker toMarker(const stdr_msgs::RfidTag& msg,bool added);
00296
00301 void republishSources();
00302
00310 template <class SourceMsg>
00311 visualization_msgs::Marker createMarker(const SourceMsg& msg,bool added);
00312
00314 ros::NodeHandle _nh;
00316 MapServerPtr _mapServer;
00317
00319 ros::Publisher _robotsPublisher;
00320
00322 ros::ServiceClient _spawnRobotClient;
00324 ros::ServiceClient _unloadRobotClient;
00326 ros::ServiceServer _loadMapService;
00328 ros::ServiceServer _loadExternalMapService;
00330 ros::ServiceServer _moveRobotService;
00331
00333 RegisterRobotServer _registerRobotServer;
00335 SpawnRobotServer _spawnRobotServer;
00337 DeleteRobotServer _deleteRobotServer;
00338
00340 RobotMap _robotMap;
00342 int _id;
00343
00345 RfidTagMap _rfidTagMap;
00347 CO2SourceMap _CO2SourceMap;
00349 ThermalSourceMap _thermalSourceMap;
00351 SoundSourceMap _soundSourceMap;
00352
00354 boost::mutex _mut;
00356 boost::condition_variable _cond;
00357
00358
00360 ros::Publisher _sourceVectorPublisherRviz;
00361
00363 ros::ServiceServer _addRfidTagServiceServer;
00365 ros::ServiceServer _deleteRfidTagServiceServer;
00367 ros::Publisher _rfidTagVectorPublisher;
00368
00369
00371 ros::ServiceServer _addCO2SourceServiceServer;
00373 ros::ServiceServer _deleteCO2SourceServiceServer;
00375 ros::Publisher _CO2SourceVectorPublisher;
00376
00378 ros::ServiceServer _addThermalSourceServiceServer;
00380 ros::ServiceServer _deleteThermalSourceServiceServer;
00382 ros::Publisher _thermalSourceVectorPublisher;
00383
00385 ros::ServiceServer _addSoundSourceServiceServer;
00387 ros::ServiceServer _deleteSoundSourceServiceServer;
00389 ros::Publisher _soundSourceVectorPublisher;
00390 };
00391 }
00392
00393
00394 #endif