stdr_server.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015 
00016    Authors :
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com
00020 ******************************************************************************/
00021 
00022 #include <stdr_server/stdr_server.h>
00023 
00024 namespace stdr_server {
00025 
00030   Server::Server(int argc, char** argv)
00031     :_spawnRobotServer(_nh, "stdr_server/spawn_robot",
00032       boost::bind(&Server::spawnRobotCallback, this, _1), false)
00033 
00034     ,_registerRobotServer(_nh, "stdr_server/register_robot",
00035       boost::bind(&Server::registerRobotCallback, this, _1), false)
00036 
00037     ,_deleteRobotServer(_nh, "stdr_server/delete_robot",
00038       boost::bind(&Server::deleteRobotCallback, this, _1), false)
00039 
00040     ,_id(0)
00041   {
00042   //~ _spawnRobotServer.registerGoalCallback(
00043     //~ boost::bind(&Server::spawnRobotCallback, this) );
00044 
00045     if (argc > 2) {
00046       ROS_ERROR("%s", USAGE);
00047       exit(-1);
00048     }
00049 
00050     if (argc == 2) {
00051       std::string fname(argv[1]);
00052       _mapServer.reset(new MapServer(fname));
00053 
00055       activateActionServers();
00056     }
00057 
00058     _loadMapService = _nh.advertiseService(
00059       "/stdr_server/load_static_map", &Server::loadMapCallback, this);
00060 
00061     _loadExternalMapService = _nh.advertiseService(
00062       "/stdr_server/load_static_map_external",
00063         &Server::loadExternalMapCallback, this);
00064 
00065     while (!ros::service::waitForService("robot_manager/load_nodelet",
00066         ros::Duration(.1)) && ros::ok())
00067     {
00068       ROS_WARN("Trying to register to robot_manager/load_nodelet...");
00069     }
00070 
00071     _spawnRobotClient = _nh.serviceClient<nodelet::NodeletLoad>
00072       ("robot_manager/load_nodelet", true);
00073 
00074     while (!ros::service::waitForService("robot_manager/unload_nodelet",
00075       ros::Duration(.1)) && ros::ok())
00076     {
00077       ROS_WARN("Trying to register to robot_manager/unload_nodelet...");
00078     }
00079 
00080     _unloadRobotClient =
00081       _nh.serviceClient<nodelet::NodeletUnload>("robot_manager/unload_nodelet");
00082 
00083     _robotsPublisher =
00084       _nh.advertise<stdr_msgs::RobotIndexedVectorMsg>
00085         ("stdr_server/active_robots", 10, true);
00086 
00088     _sourceVectorPublisherRviz = _nh.advertise<visualization_msgs::MarkerArray>(
00089       "stdr_server/sources_visualization_markers", 1, true);
00090 
00092 
00093     _addRfidTagServiceServer = _nh.advertiseService("stdr_server/add_rfid_tag",
00094       &Server::addRfidTagCallback, this);
00095 
00096     _deleteRfidTagServiceServer =
00097       _nh.advertiseService("stdr_server/delete_rfid_tag",
00098       &Server::deleteRfidTagCallback, this);
00099 
00100     _rfidTagVectorPublisher = _nh.advertise<stdr_msgs::RfidTagVector>(
00101       "stdr_server/rfid_list", 1, true);
00102 
00104 
00105     _addCO2SourceServiceServer = _nh.advertiseService(
00106       "stdr_server/add_co2_source",
00107       &Server::addCO2SourceCallback, this);
00108 
00109     _deleteCO2SourceServiceServer =
00110       _nh.advertiseService("stdr_server/delete_co2_source",
00111       &Server::deleteCO2SourceCallback, this);
00112 
00113     _CO2SourceVectorPublisher = _nh.advertise<stdr_msgs::CO2SourceVector>(
00114       "stdr_server/co2_sources_list", 1, true);
00115 
00117 
00118     _addThermalSourceServiceServer = _nh.advertiseService(
00119       "stdr_server/add_thermal_source",
00120       &Server::addThermalSourceCallback, this);
00121 
00122     _deleteThermalSourceServiceServer =
00123       _nh.advertiseService("stdr_server/delete_thermal_source",
00124       &Server::deleteThermalSourceCallback, this);
00125 
00126     _thermalSourceVectorPublisher =
00127       _nh.advertise<stdr_msgs::ThermalSourceVector>(
00128       "stdr_server/thermal_sources_list", 1, true);
00129 
00131 
00132     _addSoundSourceServiceServer = _nh.advertiseService(
00133       "stdr_server/add_sound_source",
00134       &Server::addSoundSourceCallback, this);
00135 
00136     _deleteSoundSourceServiceServer =
00137       _nh.advertiseService("stdr_server/delete_sound_source",
00138       &Server::deleteSoundSourceCallback, this);
00139 
00140     _soundSourceVectorPublisher = _nh.advertise<stdr_msgs::SoundSourceVector>(
00141       "stdr_server/sound_sources_list", 1, true);
00142   }
00143 
00147   bool Server::addRfidTagCallback(
00148     stdr_msgs::AddRfidTag::Request &req,
00149     stdr_msgs::AddRfidTag::Response &res)
00150   {
00152     stdr_msgs::RfidTag new_rfid = req.newTag;
00153     if(_rfidTagMap.find(new_rfid.tag_id) != _rfidTagMap.end())
00154     { 
00155       res.success = false;
00156       res.message = "Duplicate rfid_id";
00157       return false;
00158     }
00159 
00161     _rfidTagMap.insert(std::pair<std::string, stdr_msgs::RfidTag>(
00162       new_rfid.tag_id, new_rfid));
00163 
00165     stdr_msgs::RfidTagVector rfidTagList;
00166     visualization_msgs::MarkerArray RFIDMarkerArray;
00167 
00168     for(RfidTagMapIt it = _rfidTagMap.begin() ; it != _rfidTagMap.end() ; it++)
00169     {
00170       rfidTagList.rfid_tags.push_back(it->second);
00171       RFIDMarkerArray.markers.push_back(toMarker(it->second, true));
00172     }
00173     _rfidTagVectorPublisher.publish(rfidTagList);
00174 
00175     _sourceVectorPublisherRviz.publish(RFIDMarkerArray);
00176 
00178     republishSources();
00179 
00181     res.success = true;
00182     return true;
00183   }
00184 
00188   bool Server::addCO2SourceCallback(
00189     stdr_msgs::AddCO2Source::Request &req,
00190     stdr_msgs::AddCO2Source::Response &res)
00191   {
00193     stdr_msgs::CO2Source new_source = req.newSource;
00194     if(_CO2SourceMap.find(new_source.id) != _CO2SourceMap.end())
00195     { 
00196       res.success = false;
00197       res.message = "Duplicate CO2 id";
00198       return false;
00199     }
00200 
00202     _CO2SourceMap.insert(std::pair<std::string, stdr_msgs::CO2Source>(
00203       new_source.id, new_source));
00204 
00206     stdr_msgs::CO2SourceVector CO2SourceList;
00207     visualization_msgs::MarkerArray C02MarkerArray;
00208 
00209     for(CO2SourceMapIt it = _CO2SourceMap.begin()
00210       ; it != _CO2SourceMap.end() ; it++)
00211     {
00212       CO2SourceList.co2_sources.push_back(it->second);
00213       C02MarkerArray.markers.push_back(toMarker(it->second, true));
00214     }
00215     _CO2SourceVectorPublisher.publish(CO2SourceList);
00216     _sourceVectorPublisherRviz.publish(C02MarkerArray);
00217 
00218 
00220     republishSources();
00221 
00223     res.success = true;
00224     return true;
00225   }
00226 
00230   bool Server::addThermalSourceCallback(
00231     stdr_msgs::AddThermalSource::Request &req,
00232     stdr_msgs::AddThermalSource::Response &res)
00233   {
00235     stdr_msgs::ThermalSource new_source = req.newSource;
00236     if(_thermalSourceMap.find(new_source.id) != _thermalSourceMap.end())
00237     { 
00238       res.success = false;
00239       res.message = "Duplicate thermal source is";
00240       return false;
00241     }
00242 
00244     _thermalSourceMap.insert(std::pair<std::string, stdr_msgs::ThermalSource>(
00245       new_source.id, new_source));
00246 
00248     stdr_msgs::ThermalSourceVector thermalSourceList;
00249     visualization_msgs::MarkerArray thermalMarkerArray;
00250 
00251     for(ThermalSourceMapIt it = _thermalSourceMap.begin()
00252       ; it != _thermalSourceMap.end() ; it++)
00253     {
00254       thermalSourceList.thermal_sources.push_back(it->second);
00255       thermalMarkerArray.markers.push_back(toMarker(it->second, true));
00256     }
00257     _thermalSourceVectorPublisher.publish(thermalSourceList);
00258     _sourceVectorPublisherRviz.publish(thermalMarkerArray);
00259 
00260 
00262     republishSources();
00263 
00265     res.success = true;
00266     return true;
00267   }
00268 
00272   bool Server::addSoundSourceCallback(
00273     stdr_msgs::AddSoundSource::Request &req,
00274     stdr_msgs::AddSoundSource::Response &res)
00275   {
00277     stdr_msgs::SoundSource new_source = req.newSource;
00278     if(_soundSourceMap.find(new_source.id) != _soundSourceMap.end())
00279     { 
00280       res.success = false;
00281       res.message = "Duplicate sound source is";
00282       return false;
00283     }
00284 
00286     _soundSourceMap.insert(std::pair<std::string, stdr_msgs::SoundSource>(
00287       new_source.id, new_source));
00288 
00290     stdr_msgs::SoundSourceVector soundSourceList;
00291     visualization_msgs::MarkerArray soundMarkerArray;
00292 
00293     for(SoundSourceMapIt it = _soundSourceMap.begin()
00294       ; it != _soundSourceMap.end() ; it++)
00295     {
00296       soundSourceList.sound_sources.push_back(it->second);
00297       soundMarkerArray.markers.push_back(toMarker(it->second, true));
00298     }
00299     _soundSourceVectorPublisher.publish(soundSourceList);
00300     _sourceVectorPublisherRviz.publish(soundMarkerArray);
00301 
00302 
00304     republishSources();
00305 
00307     res.success = true;
00308     return true;
00309   }
00310 
00314   bool Server::deleteRfidTagCallback(
00315     stdr_msgs::DeleteRfidTag::Request &req,
00316     stdr_msgs::DeleteRfidTag::Response &res)
00317   {
00318 
00319     std::string name = req.name;
00320     if(_rfidTagMap.find(name) != _rfidTagMap.end())
00321     {
00322       // Publish deletion to RVIZ
00323       visualization_msgs::MarkerArray rfidMarkerArray;
00324       rfidMarkerArray.markers.push_back(toMarker(_rfidTagMap[name], false));
00325       _sourceVectorPublisherRviz.publish(rfidMarkerArray);
00326 
00327       _rfidTagMap.erase(name);
00328 
00330       stdr_msgs::RfidTagVector rfidTagList;
00331 
00332       for(RfidTagMapIt it = _rfidTagMap.begin() ;
00333         it != _rfidTagMap.end() ; it++)
00334       {
00335         rfidTagList.rfid_tags.push_back(it->second);
00336       }
00337       _rfidTagVectorPublisher.publish(rfidTagList);
00339       republishSources();
00340     }
00341     else  
00342     {
00343       return false;
00344     }
00345     return true;
00346   }
00347 
00351   bool Server::deleteCO2SourceCallback(
00352     stdr_msgs::DeleteCO2Source::Request &req,
00353     stdr_msgs::DeleteCO2Source::Response &res)
00354   {
00355     std::string name = req.name;
00356     if(_CO2SourceMap.find(name) != _CO2SourceMap.end())
00357     {
00358       // Publish deletion to RVIZ
00359       visualization_msgs::MarkerArray CO2MarkerArray;
00360       CO2MarkerArray.markers.push_back(toMarker(_CO2SourceMap[name], false));
00361       _sourceVectorPublisherRviz.publish(CO2MarkerArray);
00362 
00363       _CO2SourceMap.erase(name);
00364 
00366       stdr_msgs::CO2SourceVector CO2SourceList;
00367 
00368       for(CO2SourceMapIt it = _CO2SourceMap.begin() ;
00369         it != _CO2SourceMap.end() ; it++)
00370       {
00371         CO2SourceList.co2_sources.push_back(it->second);
00372       }
00373       _CO2SourceVectorPublisher.publish(CO2SourceList);
00375       republishSources();
00376     }
00377 
00378     else  
00379     {
00380       return false;
00381     }
00382     return true;
00383   }
00384 
00388   bool Server::deleteThermalSourceCallback(
00389     stdr_msgs::DeleteThermalSource::Request &req,
00390     stdr_msgs::DeleteThermalSource::Response &res)
00391   {
00392 
00393     std::string name = req.name;
00394     if(_thermalSourceMap.find(name) != _thermalSourceMap.end())
00395     {
00396       // Publish deletion to RVIZ
00397       visualization_msgs::MarkerArray thermalMarkerArray;
00398       thermalMarkerArray.markers.push_back(toMarker(_thermalSourceMap[name], false));
00399       _sourceVectorPublisherRviz.publish(thermalMarkerArray);
00400 
00401       _thermalSourceMap.erase(name);
00402 
00404       stdr_msgs::ThermalSourceVector thermalSourceList;
00405 
00406       for(ThermalSourceMapIt it = _thermalSourceMap.begin() ;
00407         it != _thermalSourceMap.end() ; it++)
00408       {
00409         thermalSourceList.thermal_sources.push_back(it->second);
00410       }
00411       _thermalSourceVectorPublisher.publish(thermalSourceList);
00413       republishSources();
00414     }
00415     else  
00416     {
00417       return false;
00418     }
00419     return true;
00420   }
00421 
00425   bool Server::deleteSoundSourceCallback(
00426     stdr_msgs::DeleteSoundSource::Request &req,
00427     stdr_msgs::DeleteSoundSource::Response &res)
00428   {
00429 
00430     std::string name = req.name;
00431     if(_soundSourceMap.find(name) != _soundSourceMap.end())
00432     {
00433       // Publish deletion to RVIZ
00434       visualization_msgs::MarkerArray soundMarkerArray;
00435       soundMarkerArray.markers.push_back(toMarker(_soundSourceMap[name], false));
00436       _sourceVectorPublisherRviz.publish(soundMarkerArray);
00437 
00438       _soundSourceMap.erase(name);
00439 
00441       stdr_msgs::SoundSourceVector soundSourceList;
00442 
00443 
00444       for(SoundSourceMapIt it = _soundSourceMap.begin() ;
00445         it != _soundSourceMap.end() ; it++)
00446       {
00447         soundSourceList.sound_sources.push_back(it->second);
00448 
00449       }
00450       _soundSourceVectorPublisher.publish(soundSourceList);
00452       republishSources();
00453     }
00454     else  
00455     {
00456       return false;
00457     }
00458     return true;
00459   }
00460 
00467   bool Server::loadMapCallback(
00468     stdr_msgs::LoadMap::Request& req,
00469     stdr_msgs::LoadMap::Response& res)
00470   {
00471     if (_mapServer) {
00472       ROS_WARN("Map already loaded!");
00473       return false;
00474     }
00475     _mapServer.reset(new MapServer(req.mapFile));
00477     activateActionServers();
00478 
00479     return true;
00480   }
00481 
00488   bool Server::loadExternalMapCallback(
00489     stdr_msgs::LoadExternalMap::Request& req,
00490     stdr_msgs::LoadExternalMap::Response& res)
00491   {
00492     if (_mapServer) {
00493       ROS_WARN("Map already loaded!");
00494       return false;
00495     }
00496     _mapServer.reset(new MapServer(req.map));
00497 
00499     activateActionServers();
00500 
00501     return true;
00502   }
00503 
00509   void Server::spawnRobotCallback(
00510     const stdr_msgs::SpawnRobotGoalConstPtr& goal)
00511   {
00512     stdr_msgs::SpawnRobotResult result;
00513 
00514     std::string f_id;
00515     if(hasDublicateFrameIds(goal->description, f_id))
00516     {
00517       result.message = std::string("Double frame_id:") + f_id;
00518       _spawnRobotServer.setAborted(result);
00519       return;
00520     }
00521 
00522     if (addNewRobot(goal->description, &result)) {
00523       _spawnRobotServer.setSucceeded(result);
00524 
00526       stdr_msgs::RobotIndexedVectorMsg msg;
00527       for (RobotMap::iterator it = _robotMap.begin();
00528         it != _robotMap.end(); ++it)
00529       {
00530         msg.robots.push_back( it->second );
00531       }
00532 
00533       _robotsPublisher.publish(msg);
00534       return;
00535     }
00536 
00537     _spawnRobotServer.setAborted(result);
00538   }
00539 
00545   void Server::deleteRobotCallback(
00546     const stdr_msgs::DeleteRobotGoalConstPtr&  goal)
00547   {
00548 
00549     stdr_msgs::DeleteRobotResult result;
00550 
00551     if (deleteRobot(goal->name, &result)) {
00552 
00553       // publish to active_robots topic
00554       stdr_msgs::RobotIndexedVectorMsg msg;
00555       for (RobotMap::iterator it = _robotMap.begin();
00556         it != _robotMap.end(); ++it)
00557       {
00558         msg.robots.push_back( it->second );
00559       }
00560       _robotsPublisher.publish(msg);
00561       _deleteRobotServer.setSucceeded(result);
00562       return;
00563     }
00564 
00565     _deleteRobotServer.setAborted(result);
00566   }
00567 
00573   void Server::registerRobotCallback(
00574     const stdr_msgs::RegisterRobotGoalConstPtr& goal)
00575   {
00576 
00577     boost::unique_lock<boost::mutex> lock(_mut);
00578     stdr_msgs::RegisterRobotResult result;
00579 
00580     // look for the robot name in _robotMap
00581     RobotMap::iterator search = _robotMap.find(goal->name);
00582 
00583     if (search != _robotMap.end())
00584     {
00585       result.description = _robotMap[goal->name].robot;
00586       _registerRobotServer.setSucceeded(result);
00587 
00589       _cond.notify_one();
00590     }
00591     else
00592     {
00593       _registerRobotServer.setAborted(result);
00594     }
00595   }
00596 
00601   void Server::activateActionServers(void)
00602   {
00603     _spawnRobotServer.start();
00604     _registerRobotServer.start();
00605     _deleteRobotServer.start();
00606   }
00607 
00614   bool Server::addNewRobot(
00615     stdr_msgs::RobotMsg description,
00616     stdr_msgs::SpawnRobotResult* result)
00617   {
00618 
00619     stdr_msgs::RobotIndexedMsg namedRobot;
00620 
00621     if(description.kinematicModel.type == "")
00622       description.kinematicModel.type = "ideal";
00623 
00624     namedRobot.robot = description;
00625 
00626     namedRobot.name = "robot" + boost::lexical_cast<std::string>(_id++);
00627 
00628     _robotMap.insert( std::make_pair(namedRobot.name, namedRobot) );
00629 
00630     nodelet::NodeletLoad srv;
00631     srv.request.name = namedRobot.name;
00632     srv.request.type = "stdr_robot/Robot";
00633 
00634     boost::unique_lock<boost::mutex> lock(_mut);
00635 
00636     if (_spawnRobotClient.call(srv)) {
00638       if( _cond.timed_wait(lock, ros::Duration(5.0).toBoost()) )
00639       {
00640         result->indexedDescription = namedRobot;
00641 
00642         lock.unlock();
00643         return true;
00644       }
00645     }
00646 
00647     result->message =
00648       "Robot didn't respond within time or registered with incorrect name.";
00649     lock.unlock();
00650     return false;
00651   }
00652 
00659   bool Server::deleteRobot(
00660     std::string name,
00661     stdr_msgs::DeleteRobotResult* result)
00662   {
00663 
00664     RobotMap::iterator it = _robotMap.find(name);
00665 
00666     if (it != _robotMap.end()) {
00667 
00668       nodelet::NodeletUnload srv;
00669       srv.request.name =  name;
00670 
00671       if (_unloadRobotClient.call(srv)) {
00672 
00673         if (srv.response.success) {
00674           _robotMap.erase(it);
00675         }
00676 
00677         result->success = srv.response.success;
00678         return srv.response.success;
00679       }
00680 
00681       result->success = false;
00682       return false;
00683     }
00684 
00685     ROS_WARN("Requested to delete robot, with name %s does not exist.",
00686       name.c_str());
00687 
00688     result->success = false;
00689     return false;
00690   }
00691 
00692   bool Server::hasDublicateFrameIds(const stdr_msgs::RobotMsg& robot,
00693     std::string &f_id)
00694   {
00695     std::set<std::string> f_ids;
00696     for(unsigned int i = 0 ; i < robot.laserSensors.size() ; i++)
00697     {
00698       if(f_ids.find(robot.laserSensors[i].frame_id) == f_ids.end())
00699       {
00700         f_ids.insert(robot.laserSensors[i].frame_id);
00701       }
00702       else
00703       {
00704         f_id = robot.laserSensors[i].frame_id;
00705         return true;
00706       }
00707     }
00708     for(unsigned int i = 0 ; i < robot.sonarSensors.size() ; i++)
00709     {
00710       if(f_ids.find(robot.sonarSensors[i].frame_id) == f_ids.end())
00711       {
00712         f_ids.insert(robot.sonarSensors[i].frame_id);
00713       }
00714       else
00715       {
00716         f_id = robot.sonarSensors[i].frame_id;
00717         return true;
00718       }
00719     }
00720     for(unsigned int i = 0 ; i < robot.rfidSensors.size() ; i++)
00721     {
00722       if(f_ids.find(robot.rfidSensors[i].frame_id) == f_ids.end())
00723       {
00724         f_ids.insert(robot.rfidSensors[i].frame_id);
00725       }
00726       else
00727       {
00728         f_id = robot.rfidSensors[i].frame_id;
00729         return true;
00730       }
00731     }
00732     for(unsigned int i = 0 ; i < robot.co2Sensors.size() ; i++)
00733     {
00734       if(f_ids.find(robot.co2Sensors[i].frame_id) == f_ids.end())
00735       {
00736         f_ids.insert(robot.co2Sensors[i].frame_id);
00737       }
00738       else
00739       {
00740         f_id = robot.co2Sensors[i].frame_id;
00741         return true;
00742       }
00743     }
00744     for(unsigned int i = 0 ; i < robot.soundSensors.size() ; i++)
00745     {
00746       if(f_ids.find(robot.soundSensors[i].frame_id) == f_ids.end())
00747       {
00748         f_ids.insert(robot.soundSensors[i].frame_id);
00749       }
00750       else
00751       {
00752         f_id = robot.soundSensors[i].frame_id;
00753         return true;
00754       }
00755     }
00756     for(unsigned int i = 0 ; i < robot.thermalSensors.size() ; i++)
00757     {
00758       if(f_ids.find(robot.thermalSensors[i].frame_id) == f_ids.end())
00759       {
00760         f_ids.insert(robot.thermalSensors[i].frame_id);
00761       }
00762       else
00763       {
00764         f_id = robot.thermalSensors[i].frame_id;
00765         return true;
00766       }
00767     }
00768     return false;
00769   }
00770 
00774   visualization_msgs::Marker Server::toMarker(const stdr_msgs::CO2Source& msg, bool added)
00775   {
00776     visualization_msgs::Marker marker = createMarker(msg, added);
00777 
00778     // Set the namespace and id for this marker.  This serves to create a unique ID
00779     // Any marker sent with the same namespace and id will overwrite the old one
00780     marker.ns = "co2";
00781     marker.id = atoi(msg.id.c_str());
00782 
00783     // Set the color specific to the source type -- be sure to set alpha to something non-zero!
00784     marker.color.r = 0.0f;
00785     marker.color.g = 1.0f;
00786     marker.color.b = 0.0f;
00787     marker.color.a = 1.0;
00788 
00789     return marker;
00790   }
00791 
00795   visualization_msgs::Marker Server::toMarker(const stdr_msgs::ThermalSource& msg, bool added)
00796   {
00797     visualization_msgs::Marker marker = createMarker(msg, added);
00798 
00799     // Set the namespace and id for this marker.  This serves to create a unique ID
00800     // Any marker sent with the same namespace and id will overwrite the old one
00801     marker.ns = "thermal";
00802     marker.id = atoi(msg.id.c_str());
00803 
00804     // Set the color specific to the source type -- be sure to set alpha to something non-zero!
00805     marker.color.r = 1.0f;
00806     marker.color.g = 0.0f;
00807     marker.color.b = 0.0f;
00808     marker.color.a = 1.0;
00809 
00810     return marker;
00811   }
00812 
00816   visualization_msgs::Marker Server::toMarker(const stdr_msgs::SoundSource& msg, bool added)
00817   {
00818     visualization_msgs::Marker marker = createMarker(msg, added);
00819 
00820     // Set the namespace and id for this marker.  This serves to create a unique ID
00821     // Any marker sent with the same namespace and id will overwrite the old one
00822     marker.ns = "sound";
00823     marker.id = atoi(msg.id.c_str());
00824 
00825     // Set the color specific to the source type -- be sure to set alpha to something non-zero!
00826     marker.color.r = 0.0f;
00827     marker.color.g = 0.0f;
00828     marker.color.b = 1.0f;
00829     marker.color.a = 1.0;
00830 
00831     return marker;
00832   }
00833 
00837   visualization_msgs::Marker Server::toMarker(const stdr_msgs::RfidTag& msg, bool added)
00838   {
00839     visualization_msgs::Marker marker = createMarker(msg, added);
00840 
00841     // Set the namespace and id for this marker.  This serves to create a unique ID
00842     // Any marker sent with the same namespace and id will overwrite the old one
00843     marker.ns = "rfid";
00844     marker.id = atoi(msg.tag_id.c_str());
00845 
00846     // Set the color specific to the source type -- be sure to set alpha to something non-zero!
00847     marker.color.r = 0.0f;
00848     marker.color.g = 0.0f;
00849     marker.color.b = 0.0f;
00850     marker.color.a = 1.0;
00851 
00852     return marker;
00853   }
00854 
00855   void Server::republishSources()
00856   {
00857     visualization_msgs::MarkerArray ma;
00858     for(SoundSourceMapIt it = _soundSourceMap.begin()
00859       ; it != _soundSourceMap.end() ; it++)
00860     {
00861         ma.markers.push_back(toMarker(it->second, true));
00862     }
00863     for(CO2SourceMapIt it = _CO2SourceMap.begin()
00864       ; it != _CO2SourceMap.end() ; it++)
00865     {
00866       ma.markers.push_back(toMarker(it->second, true));
00867     }
00868     for(ThermalSourceMapIt it = _thermalSourceMap.begin()
00869       ; it != _thermalSourceMap.end() ; it++)
00870     {
00871       ma.markers.push_back(toMarker(it->second, true));
00872     }
00873     for(RfidTagMapIt it = _rfidTagMap.begin() ; it != _rfidTagMap.end() ; it++)
00874     {
00875       ma.markers.push_back(toMarker(it->second, true));
00876     }
00877     _sourceVectorPublisherRviz.publish(ma);
00878   }
00879 
00884   template <class SourceMsg>
00885   visualization_msgs::Marker Server::createMarker(const SourceMsg& msg, bool added)
00886   {
00887     visualization_msgs::Marker marker;
00888 
00889     marker.header.frame_id = "/map_static";
00890     marker.header.stamp = ros::Time();
00891 
00892     uint32_t shape = visualization_msgs::Marker::SPHERE;
00893     marker.type = shape;
00894 
00895     // Set the marker action.
00896     if(added) {
00897       marker.action = visualization_msgs::Marker::ADD;
00898     }
00899     else {
00900       marker.action = visualization_msgs::Marker::DELETE;
00901     }
00902 
00903     marker.pose.position.x = msg.pose.x;
00904     marker.pose.position.y = msg.pose.y;
00905     marker.pose.position.z = 0;
00906     marker.pose.orientation.x = 0.0;
00907     marker.pose.orientation.y = 0.0;
00908     marker.pose.orientation.z = 0.0;
00909     marker.pose.orientation.w = 1.0;
00910 
00911     marker.scale.x = 0.5;
00912     marker.scale.y = 0.5;
00913     marker.scale.z = 0.5;
00914 
00915     marker.lifetime = ros::Duration();
00916     return marker;
00917   }
00918 
00919 } // end of namespace stdr_robot


stdr_server
Author(s): Chris Zalidis
autogenerated on Thu Jun 6 2019 18:57:23