stdr_server.cpp

Go to the documentation of this file.

/******************************************************************************
   STDR Simulator - Simple Two DImensional Robot Simulator
   Copyright (C) 2013 STDR Simulator
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 3 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software Foundation,
   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
   
   Authors : 
   * Manos Tsardoulias, etsardou@gmail.com
   * Aris Thallas, aris.thallas@gmail.com
   * Chris Zalidis, zalidis@gmail.com 
******************************************************************************/

#include <stdr_server/stdr_server.h>

namespace stdr_server {
  
  Server::Server(int argc, char** argv) 
    :_spawnRobotServer(_nh, "stdr_server/spawn_robot", 
      boost::bind(&Server::spawnRobotCallback, this, _1), false)
      
    ,_registerRobotServer(_nh, "stdr_server/register_robot", 
      boost::bind(&Server::registerRobotCallback, this, _1), false)
    
    ,_deleteRobotServer(_nh, "stdr_server/delete_robot", 
      boost::bind(&Server::deleteRobotCallback, this, _1), false)
    
    ,_id(0)
  {
  //~ _spawnRobotServer.registerGoalCallback( 
    //~ boost::bind(&Server::spawnRobotCallback, this) );
    
    if (argc > 2) {
      ROS_ERROR("%s", USAGE);
      exit(-1);
    }
    
    if (argc == 2) {
      std::string fname(argv[1]);
      _mapServer.reset(new MapServer(fname));
      
      activateActionServers();
    }
      
    _loadMapService = _nh.advertiseService(
      "/stdr_server/load_static_map", &Server::loadMapCallback, this);
    
    _loadExternalMapService = _nh.advertiseService(
      "/stdr_server/load_static_map_external", 
        &Server::loadExternalMapCallback, this);
    
    while (!ros::service::waitForService("robot_manager/load_nodelet", 
        ros::Duration(.1)) && ros::ok()) 
    {
      ROS_WARN("Trying to register to robot_manager/load_nodelet...");
    }
    
    _spawnRobotClient = _nh.serviceClient<nodelet::NodeletLoad>
      ("robot_manager/load_nodelet", true);
    
    while (!ros::service::waitForService("robot_manager/unload_nodelet", 
      ros::Duration(.1)) && ros::ok()) 
    {
      ROS_WARN("Trying to register to robot_manager/unload_nodelet...");
    }
    
    _unloadRobotClient = 
      _nh.serviceClient<nodelet::NodeletUnload>("robot_manager/unload_nodelet");
    
    _robotsPublisher = 
      _nh.advertise<stdr_msgs::RobotIndexedVectorMsg>
        ("stdr_server/active_robots", 10, true);
        
        
    _addRfidTagServiceServer = _nh.advertiseService("stdr_server/add_rfid_tag",
      &Server::addRfidTagCallback, this);
      
    _deleteRfidTagServiceServer = 
      _nh.advertiseService("stdr_server/delete_rfid_tag",
      &Server::deleteRfidTagCallback, this);
      
    _rfidTagVectorPublisher = _nh.advertise<stdr_msgs::RfidTagVector>(
      "stdr_server/rfid_list", 1, true);
      
        
    _addCO2SourceServiceServer = _nh.advertiseService(
      "stdr_server/add_co2_source",
      &Server::addCO2SourceCallback, this);
      
    _deleteCO2SourceServiceServer = 
      _nh.advertiseService("stdr_server/delete_co2_source",
      &Server::deleteCO2SourceCallback, this);
      
    _CO2SourceVectorPublisher = _nh.advertise<stdr_msgs::CO2SourceVector>(
      "stdr_server/co2_sources_list", 1, true);
      
        
    _addThermalSourceServiceServer = _nh.advertiseService(
      "stdr_server/add_thermal_source",
      &Server::addThermalSourceCallback, this);
      
    _deleteThermalSourceServiceServer = 
      _nh.advertiseService("stdr_server/delete_thermal_source",
      &Server::deleteThermalSourceCallback, this);
      
    _thermalSourceVectorPublisher = 
      _nh.advertise<stdr_msgs::ThermalSourceVector>(
      "stdr_server/thermal_sources_list", 1, true);
    
        
    _addSoundSourceServiceServer = _nh.advertiseService(
      "stdr_server/add_sound_source",
      &Server::addSoundSourceCallback, this);
      
    _deleteSoundSourceServiceServer = 
      _nh.advertiseService("stdr_server/delete_sound_source",
      &Server::deleteSoundSourceCallback, this);
      
    _soundSourceVectorPublisher = _nh.advertise<stdr_msgs::SoundSourceVector>(
      "stdr_server/sound_sources_list", 1, true);
  }
  
  bool Server::addRfidTagCallback(
    stdr_msgs::AddRfidTag::Request &req, 
    stdr_msgs::AddRfidTag::Response &res)
  {
    stdr_msgs::RfidTag new_rfid = req.newTag;
    if(_rfidTagMap.find(new_rfid.tag_id) != _rfidTagMap.end())
    { 
      res.success = false;
      res.message = "Duplicate rfid_id";
      return false;
    }
    
    _rfidTagMap.insert(std::pair<std::string, stdr_msgs::RfidTag>(
      new_rfid.tag_id, new_rfid));
    
    stdr_msgs::RfidTagVector rfidTagList;
    for(RfidTagMapIt it = _rfidTagMap.begin() ; it != _rfidTagMap.end() ; it++)
    {
      rfidTagList.rfid_tags.push_back(it->second);
    }
    _rfidTagVectorPublisher.publish(rfidTagList);

    res.success = true;
    return true;
  }
  
  bool Server::addCO2SourceCallback(
    stdr_msgs::AddCO2Source::Request &req, 
    stdr_msgs::AddCO2Source::Response &res)
  {
    stdr_msgs::CO2Source new_source = req.newSource;
    if(_CO2SourceMap.find(new_source.id) != _CO2SourceMap.end())
    { 
      res.success = false;
      res.message = "Duplicate CO2 is";
      return false;
    }
    
    _CO2SourceMap.insert(std::pair<std::string, stdr_msgs::CO2Source>(
      new_source.id, new_source));
    
    stdr_msgs::CO2SourceVector CO2SourceList;
    for(CO2SourceMapIt it = _CO2SourceMap.begin() 
      ; it != _CO2SourceMap.end() ; it++)
    {
      CO2SourceList.co2_sources.push_back(it->second);
    }
    _CO2SourceVectorPublisher.publish(CO2SourceList);
    
    res.success = true;
    return true;
  }
  
  bool Server::addThermalSourceCallback(
    stdr_msgs::AddThermalSource::Request &req, 
    stdr_msgs::AddThermalSource::Response &res)
  {
    stdr_msgs::ThermalSource new_source = req.newSource;
    if(_thermalSourceMap.find(new_source.id) != _thermalSourceMap.end())
    { 
      res.success = false;
      res.message = "Duplicate thermal source is";
      return false;
    }
    
    _thermalSourceMap.insert(std::pair<std::string, stdr_msgs::ThermalSource>(
      new_source.id, new_source));
    
    stdr_msgs::ThermalSourceVector thermalSourceList;
    for(ThermalSourceMapIt it = _thermalSourceMap.begin() 
      ; it != _thermalSourceMap.end() ; it++)
    {
      thermalSourceList.thermal_sources.push_back(it->second);
    }
    _thermalSourceVectorPublisher.publish(thermalSourceList);
    
    res.success = true;
    return true;
  }
  
  bool Server::addSoundSourceCallback(
    stdr_msgs::AddSoundSource::Request &req, 
    stdr_msgs::AddSoundSource::Response &res)
  {
    stdr_msgs::SoundSource new_source = req.newSource;
    if(_soundSourceMap.find(new_source.id) != _soundSourceMap.end())
    { 
      res.success = false;
      res.message = "Duplicate sound source is";
      return false;
    }
    
    _soundSourceMap.insert(std::pair<std::string, stdr_msgs::SoundSource>(
      new_source.id, new_source));
    
    stdr_msgs::SoundSourceVector soundSourceList;
    for(SoundSourceMapIt it = _soundSourceMap.begin() 
      ; it != _soundSourceMap.end() ; it++)
    {
      soundSourceList.sound_sources.push_back(it->second);
    }
    _soundSourceVectorPublisher.publish(soundSourceList);
    
    res.success = true;
    return true;
  }
  
  bool Server::deleteRfidTagCallback(
    stdr_msgs::DeleteRfidTag::Request &req, 
    stdr_msgs::DeleteRfidTag::Response &res)
  {
    
    std::string name = req.name;
    if(_rfidTagMap.find(name) != _rfidTagMap.end())
    {
      _rfidTagMap.erase(name);
      
      stdr_msgs::RfidTagVector rfidTagList;
      for(RfidTagMapIt it = _rfidTagMap.begin() ; 
        it != _rfidTagMap.end() ; it++)
      {
        rfidTagList.rfid_tags.push_back(it->second);
      }
      _rfidTagVectorPublisher.publish(rfidTagList);
    }
    else  
    {
      return false;
    }
    return true;
  }
  
  bool Server::deleteCO2SourceCallback(
    stdr_msgs::DeleteCO2Source::Request &req, 
    stdr_msgs::DeleteCO2Source::Response &res)
  {
    
    std::string name = req.name;
    if(_CO2SourceMap.find(name) != _CO2SourceMap.end())
    {
      _CO2SourceMap.erase(name);
      
      stdr_msgs::CO2SourceVector CO2SourceList;
      for(CO2SourceMapIt it = _CO2SourceMap.begin() ; 
        it != _CO2SourceMap.end() ; it++)
      {
        CO2SourceList.co2_sources.push_back(it->second);
      }
      _CO2SourceVectorPublisher.publish(CO2SourceList);
    }
    else  
    {
      return false;
    }
    return true;
  }
  
  bool Server::deleteThermalSourceCallback(
    stdr_msgs::DeleteThermalSource::Request &req, 
    stdr_msgs::DeleteThermalSource::Response &res)
  {
    
    std::string name = req.name;
    if(_thermalSourceMap.find(name) != _thermalSourceMap.end())
    {
      _thermalSourceMap.erase(name);
      
      stdr_msgs::ThermalSourceVector thermalSourceList;
      for(ThermalSourceMapIt it = _thermalSourceMap.begin() ; 
        it != _thermalSourceMap.end() ; it++)
      {
        thermalSourceList.thermal_sources.push_back(it->second);
      }
      _thermalSourceVectorPublisher.publish(thermalSourceList);
    }
    else  
    {
      return false;
    }
    return true;
  }
  
  bool Server::deleteSoundSourceCallback(
    stdr_msgs::DeleteSoundSource::Request &req, 
    stdr_msgs::DeleteSoundSource::Response &res)
  {
    
    std::string name = req.name;
    if(_soundSourceMap.find(name) != _soundSourceMap.end())
    {
      _soundSourceMap.erase(name);
      
      stdr_msgs::SoundSourceVector soundSourceList;
      for(SoundSourceMapIt it = _soundSourceMap.begin() ; 
        it != _soundSourceMap.end() ; it++)
      {
        soundSourceList.sound_sources.push_back(it->second);
      }
      _soundSourceVectorPublisher.publish(soundSourceList);
    }
    else  
    {
      return false;
    }
    return true;
  }

  bool Server::loadMapCallback(
    stdr_msgs::LoadMap::Request& req,
    stdr_msgs::LoadMap::Response& res) 
  {
    if (_mapServer) {
      ROS_WARN("Map already loaded!");
      return false;
    }
    _mapServer.reset(new MapServer(req.mapFile));
    activateActionServers();

    return true;        
  }

  bool Server::loadExternalMapCallback(
    stdr_msgs::LoadExternalMap::Request& req,
    stdr_msgs::LoadExternalMap::Response& res)
  {
    if (_mapServer) {
      ROS_WARN("Map already loaded!");
      return false;
    }
    _mapServer.reset(new MapServer(req.map));
    
    activateActionServers();

    return true;        
  }

  void Server::spawnRobotCallback(
    const stdr_msgs::SpawnRobotGoalConstPtr& goal) 
  {
    stdr_msgs::SpawnRobotResult result;
    
    std::string f_id;
    if(hasDublicateFrameIds(goal->description, f_id))
    {
      result.message = std::string("Double frame_id :") + f_id;
      _spawnRobotServer.setAborted(result);
      return;
    }
    
    if (addNewRobot(goal->description, &result)) {
      _spawnRobotServer.setSucceeded(result);
      
      stdr_msgs::RobotIndexedVectorMsg msg;
      for (RobotMap::iterator it = _robotMap.begin(); 
        it != _robotMap.end(); ++it) 
      {
        msg.robots.push_back( it->second );
      }
      
      _robotsPublisher.publish(msg);
      return;
    }

    _spawnRobotServer.setAborted();
  }

  void Server::deleteRobotCallback(
    const stdr_msgs::DeleteRobotGoalConstPtr&  goal) 
  {
    
    stdr_msgs::DeleteRobotResult result;
    
    if (deleteRobot(goal->name, &result)) {
      
      // publish to active_robots topic
      stdr_msgs::RobotIndexedVectorMsg msg;
      for (RobotMap::iterator it = _robotMap.begin(); 
        it != _robotMap.end(); ++it) 
      {
        msg.robots.push_back( it->second );
      }
      _robotsPublisher.publish(msg);
      _deleteRobotServer.setSucceeded(result);
      return;
    }
    
    _deleteRobotServer.setAborted(result);
  }
  
  void Server::registerRobotCallback(
    const stdr_msgs::RegisterRobotGoalConstPtr& goal) 
  {
    
    boost::unique_lock<boost::mutex> lock(_mut);
    stdr_msgs::RegisterRobotResult result;
    result.description = _robotMap[goal->name].robot;
    _registerRobotServer.setSucceeded(result);
    cond.notify_one();
  }

  void Server::activateActionServers(void) 
  {
    _spawnRobotServer.start();
    _registerRobotServer.start();
    _deleteRobotServer.start();
  }

  bool Server::addNewRobot(
    stdr_msgs::RobotMsg description, 
    stdr_msgs::SpawnRobotResult* result) 
  {
    
    stdr_msgs::RobotIndexedMsg namedRobot;
    
    namedRobot.robot = description;
    
    namedRobot.name = "robot" + boost::lexical_cast<std::string>(_id++);
    
    _robotMap.insert( std::make_pair(namedRobot.name, namedRobot) );
    
    nodelet::NodeletLoad srv;
    srv.request.name = namedRobot.name;
    srv.request.type = "stdr_robot/Robot";
    
    boost::unique_lock<boost::mutex> lock(_mut);
      
    if (_spawnRobotClient.call(srv)) {
      cond.wait(lock);
      
      result->indexedDescription = namedRobot;
      
      lock.unlock();
      return true;
    }
    
    lock.unlock();
    return false;
  }

  bool Server::deleteRobot(
    std::string name, 
    stdr_msgs::DeleteRobotResult* result) 
  {
    
    RobotMap::iterator it = _robotMap.find(name);
    
    if (it != _robotMap.end()) {
      
      nodelet::NodeletUnload srv;
      srv.request.name =  name;
      
      if (_unloadRobotClient.call(srv)) {
        
        if (srv.response.success) {
          _robotMap.erase(it);
        }
        
        result->success = srv.response.success;
        return srv.response.success;
      }
      
      result->success = false;
      return false;
    }
    
    ROS_WARN("Requested to delete robot, with name %s does not exist.", 
      name.c_str());
    
    result->success = false;
    return false;
  }
  
  bool Server::hasDublicateFrameIds(const stdr_msgs::RobotMsg& robot,
    std::string &f_id)
  {
    std::set<std::string> f_ids;
    for(unsigned int i = 0 ; i < robot.laserSensors.size() ; i++)
    {
      if(f_ids.find(robot.laserSensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.laserSensors[i].frame_id);
      }
      else
      {
        f_id = robot.laserSensors[i].frame_id;
        return true;
      }
    }
    for(unsigned int i = 0 ; i < robot.sonarSensors.size() ; i++)
    {
      if(f_ids.find(robot.sonarSensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.sonarSensors[i].frame_id);
      }
      else
      {
        f_id = robot.sonarSensors[i].frame_id;
        return true;
      }
    }
    for(unsigned int i = 0 ; i < robot.rfidSensors.size() ; i++)
    {
      if(f_ids.find(robot.rfidSensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.rfidSensors[i].frame_id);
      }
      else
      {
        f_id = robot.rfidSensors[i].frame_id;
        return true;
      }
    }
    for(unsigned int i = 0 ; i < robot.co2Sensors.size() ; i++)
    {
      if(f_ids.find(robot.co2Sensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.co2Sensors[i].frame_id);
      }
      else
      {
        f_id = robot.co2Sensors[i].frame_id;
        return true;
      }
    }
    for(unsigned int i = 0 ; i < robot.soundSensors.size() ; i++)
    {
      if(f_ids.find(robot.soundSensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.soundSensors[i].frame_id);
      }
      else
      {
        f_id = robot.soundSensors[i].frame_id;
        return true;
      }
    }
    for(unsigned int i = 0 ; i < robot.thermalSensors.size() ; i++)
    {
      if(f_ids.find(robot.thermalSensors[i].frame_id) == f_ids.end())
      {
        f_ids.insert(robot.thermalSensors[i].frame_id);
      }
      else
      {
        f_id = robot.thermalSensors[i].frame_id;
        return true;
      }
    }
    return false;
  }

} // end of namespace stdr_robot