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 <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.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);

      visualization_msgs::Marker toMarker(const stdr_msgs::CO2Source& msg,bool added);

      visualization_msgs::Marker toMarker(const stdr_msgs::ThermalSource& msg,bool added);

      visualization_msgs::Marker toMarker(const stdr_msgs::SoundSource& msg,bool added);

      visualization_msgs::Marker toMarker(const stdr_msgs::RfidTag& msg,bool added);

      void republishSources();

      template <class SourceMsg>
      visualization_msgs::Marker createMarker(const SourceMsg& msg,bool added);

      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::Publisher _sourceVectorPublisherRviz;

      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