stdr_server.h

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 
******************************************************************************/

#ifndef STDR_SERVER_H
#define STDR_SERVER_H

#define USAGE "\nUSAGE: stdr_server <map.yaml>\n" \
              "  map.yaml: map description file\n" 

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <stdr_server/map_server.h>
#include <stdr_msgs/LoadMap.h>
#include <stdr_msgs/LoadExternalMap.h>
#include <stdr_msgs/RegisterGui.h>
#include <stdr_msgs/RegisterRobotAction.h>
#include <stdr_msgs/SpawnRobotAction.h>
#include <stdr_msgs/DeleteRobotAction.h>
#include <stdr_msgs/RobotIndexedMsg.h>
#include <stdr_msgs/RobotIndexedVectorMsg.h>

#include <stdr_msgs/RfidTagVector.h>
#include <stdr_msgs/AddRfidTag.h>
#include <stdr_msgs/DeleteRfidTag.h>

#include <stdr_msgs/CO2SourceVector.h>
#include <stdr_msgs/AddCO2Source.h>
#include <stdr_msgs/DeleteCO2Source.h>

#include <stdr_msgs/ThermalSourceVector.h>
#include <stdr_msgs/AddThermalSource.h>
#include <stdr_msgs/DeleteThermalSource.h>

#include <stdr_msgs/SoundSourceVector.h>
#include <stdr_msgs/AddSoundSource.h>
#include <stdr_msgs/DeleteSoundSource.h>

#include <nodelet/NodeletLoad.h>
#include <nodelet/NodeletUnload.h>

namespace stdr_server {

  typedef boost::shared_ptr<MapServer> MapServerPtr;
  
  typedef actionlib::SimpleActionServer<stdr_msgs::SpawnRobotAction> 
    SpawnRobotServer;
  
  typedef actionlib::SimpleActionServer<stdr_msgs::RegisterRobotAction> 
    RegisterRobotServer;
  
  typedef actionlib::SimpleActionServer<stdr_msgs::DeleteRobotAction> 
    DeleteRobotServer;

  typedef std::map<std::string, stdr_msgs::RobotIndexedMsg> RobotMap;
  
  typedef std::map<std::string, stdr_msgs::RfidTag> RfidTagMap;
  typedef std::map<std::string, stdr_msgs::RfidTag>::iterator RfidTagMapIt;
  
  typedef std::map<std::string, stdr_msgs::CO2Source> CO2SourceMap;
  typedef std::map<std::string, stdr_msgs::CO2Source>::iterator CO2SourceMapIt;

  typedef std::map<std::string, stdr_msgs::ThermalSource> ThermalSourceMap;
  typedef std::map<std::string, stdr_msgs::ThermalSource>::iterator 
    ThermalSourceMapIt;
  
  typedef std::map<std::string, stdr_msgs::SoundSource> SoundSourceMap;
  typedef std::map<std::string, stdr_msgs::SoundSource>::iterator 
    SoundSourceMapIt;

  class Server 
  {
      
    public:
      
      Server(int argc, char** argv);
      
      
      bool loadMapCallback(stdr_msgs::LoadMap::Request& req,
        stdr_msgs::LoadMap::Response& res);
      
      bool loadExternalMapCallback(stdr_msgs::LoadExternalMap::Request& req,
        stdr_msgs::LoadExternalMap::Response& res);
      
      
      void spawnRobotCallback(const stdr_msgs::SpawnRobotGoalConstPtr& goal);
      
      void deleteRobotCallback(
        const stdr_msgs::DeleteRobotGoalConstPtr&  goal);
        
      void registerRobotCallback(
        const stdr_msgs::RegisterRobotGoalConstPtr& goal);
      
    private:
      
      void activateActionServers(void);
      
      bool addNewRobot(stdr_msgs::RobotMsg description, 
        stdr_msgs::SpawnRobotResult* result);
        
      bool deleteRobot(std::string name, stdr_msgs::DeleteRobotResult* result);
      
      bool addRfidTagCallback(
        stdr_msgs::AddRfidTag::Request &req, 
        stdr_msgs::AddRfidTag::Response &res);
        
      bool deleteRfidTagCallback(
        stdr_msgs::DeleteRfidTag::Request &req, 
        stdr_msgs::DeleteRfidTag::Response &res);
        
      bool addCO2SourceCallback(
        stdr_msgs::AddCO2Source::Request &req, 
        stdr_msgs::AddCO2Source::Response &res);
        
      bool deleteCO2SourceCallback(
        stdr_msgs::DeleteCO2Source::Request &req, 
        stdr_msgs::DeleteCO2Source::Response &res);
        
      bool addThermalSourceCallback(
        stdr_msgs::AddThermalSource::Request &req, 
        stdr_msgs::AddThermalSource::Response &res);
        
      bool deleteThermalSourceCallback(
        stdr_msgs::DeleteThermalSource::Request &req, 
        stdr_msgs::DeleteThermalSource::Response &res);
      
      bool addSoundSourceCallback(
        stdr_msgs::AddSoundSource::Request &req, 
        stdr_msgs::AddSoundSource::Response &res);
        
      bool deleteSoundSourceCallback(
        stdr_msgs::DeleteSoundSource::Request &req, 
        stdr_msgs::DeleteSoundSource::Response &res);
        
      bool hasDublicateFrameIds(const stdr_msgs::RobotMsg& robot, 
        std::string &f_id);
      
    private:
    
      ros::NodeHandle _nh;
      MapServerPtr _mapServer;
      
      ros::Publisher _robotsPublisher;
      
      ros::ServiceClient _spawnRobotClient;
      ros::ServiceClient _unloadRobotClient;
      ros::ServiceServer _loadMapService;
      ros::ServiceServer _loadExternalMapService;
      ros::ServiceServer _moveRobotService;
      
      RegisterRobotServer _registerRobotServer;
      SpawnRobotServer _spawnRobotServer;
      DeleteRobotServer _deleteRobotServer;
      
      RobotMap _robotMap;
      int _id;
      
      RfidTagMap _rfidTagMap;
      CO2SourceMap _CO2SourceMap;
      ThermalSourceMap _thermalSourceMap;
      SoundSourceMap _soundSourceMap;
      
      boost::mutex _mut;
      boost::condition_variable cond;
      
      ros::ServiceServer _addRfidTagServiceServer;
      ros::ServiceServer _deleteRfidTagServiceServer;
      ros::Publisher _rfidTagVectorPublisher;
      
      ros::ServiceServer _addCO2SourceServiceServer;
      ros::ServiceServer _deleteCO2SourceServiceServer;
      ros::Publisher _CO2SourceVectorPublisher;
      
      ros::ServiceServer _addThermalSourceServiceServer;
      ros::ServiceServer _deleteThermalSourceServiceServer;
      ros::Publisher _thermalSourceVectorPublisher;
      
      ros::ServiceServer _addSoundSourceServiceServer;
      ros::ServiceServer _deleteSoundSourceServiceServer;
      ros::Publisher _soundSourceVectorPublisher;
  };
}


#endif