SensorRegistry.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2011, SRI International (R)
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#include <map>
#include <iostream>
 
#ifdef USE_TBB
#include <tbb/mutex.h>
#endif
 
#include <OpenKarto/SensorRegistry.h>
#include <OpenKarto/Exception.h>
#include <OpenKarto/Sensor.h>
#include <OpenKarto/Logger.h>
 
namespace karto
{
 
 
  // Note to not change the Sensor* to SensorPtr or SmartPointer<Sensor>. 
  struct SensorRegistryPrivate
  {
    List<Sensor*> m_Sensors;
 
    typedef std::map<Identifier, Sensor*> SensorManagerMap;
    SensorManagerMap m_SensorMap;
  };
 
  SensorRegistry::SensorRegistry()
    : m_pSensorRegistryPrivate(new SensorRegistryPrivate())
  {
  }
 
  SensorRegistry::~SensorRegistry()
  {
    m_pSensorRegistryPrivate->m_Sensors.Clear();
    delete m_pSensorRegistryPrivate;
  }
 
  SensorRegistry* SensorRegistry::GetInstance()
  {
#ifdef USE_TBB
    static tbb::mutex myMutex;
    tbb::mutex::scoped_lock lock(myMutex);
#endif
 
    static SmartPointer<SensorRegistry> sInstance = new SensorRegistry();
    return sInstance;
  }
 
  void SensorRegistry::RegisterSensor(Sensor* pSensor)
  {
    if (pSensor != NULL)
    {
      if (pSensor->GetIdentifier().GetScope() != "Karto/System")
      {
        Validate(pSensor);
 
        Log(LOG_DEBUG, String("Registering sensor: [") + pSensor->GetIdentifier().ToString() + "]");
      }
 
      if ((m_pSensorRegistryPrivate->m_SensorMap.find(karto::Identifier(pSensor->GetIdentifier())) != m_pSensorRegistryPrivate->m_SensorMap.end()))
      {
        String errorMessage;
        errorMessage.Append("Cannot register sensor: already registered: [");
        errorMessage.Append(pSensor->GetIdentifier().ToString());
        errorMessage.Append("]");
 
        throw Exception(errorMessage);
      }
 
      m_pSensorRegistryPrivate->m_SensorMap[karto::Identifier(pSensor->GetIdentifier())] = pSensor;
      m_pSensorRegistryPrivate->m_Sensors.Add(pSensor);
    }
  }
 
  void SensorRegistry::UnregisterSensor(Sensor* pSensor)
  {
    if (pSensor != NULL)
    {
      if (pSensor->GetIdentifier().GetScope() != "Karto/System")
      {
        Log(LOG_DEBUG, String("Unregistering sensor: [") + pSensor->GetIdentifier().ToString() + "]");
      }
 
      if (m_pSensorRegistryPrivate->m_SensorMap.find(pSensor->GetIdentifier()) != m_pSensorRegistryPrivate->m_SensorMap.end())
      {
        m_pSensorRegistryPrivate->m_SensorMap.erase(pSensor->GetIdentifier());
 
        m_pSensorRegistryPrivate->m_Sensors.Remove(pSensor);
      }
      else
      {
        String errorMessage;
        errorMessage.Append("Cannot unregister sensor: not registered: [");
        errorMessage.Append(pSensor->GetIdentifier().ToString());
        errorMessage.Append("]");
 
        throw Exception(errorMessage);
      }
    }
  }
 
  Sensor* SensorRegistry::GetSensorByName(const Identifier& rName)
  {
    if (m_pSensorRegistryPrivate->m_SensorMap.find(rName) != m_pSensorRegistryPrivate->m_SensorMap.end())
    {
      Sensor* pSensor = m_pSensorRegistryPrivate->m_SensorMap[rName];
 
      assert(pSensor != NULL);
 
      return pSensor;
    }
 
    String errorMessage;
    errorMessage.Append("Sensor not registered: [");
    errorMessage.Append(rName.ToString());
    errorMessage.Append("]");
    throw Exception(errorMessage);
  }
 
  void SensorRegistry::Clear()
  {
    m_pSensorRegistryPrivate->m_Sensors.Clear();
    m_pSensorRegistryPrivate->m_SensorMap.clear();
  }
 
  void SensorRegistry::Validate(Sensor* pSensor)
  {
    if (pSensor == NULL)
    {
      throw Exception("Invalid sensor: NULL");
    }
    else if (pSensor->GetIdentifier().Size() == 0)
    {
      throw Exception("Invalid sensor: Nameless");        
    }
  }
 
}