Karto.cpp

Go to the documentation of this file.

/*
 * Copyright 2010 SRI International
 *
 * 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 <math.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
 
#include "open_karto/Karto.h"
 
namespace karto
{
 
 
  SensorManager* SensorManager::GetInstance()
  {
    static Singleton<SensorManager> sInstance;
    return sInstance.Get();
  }
 
 
  Object::Object()
    : m_pParameterManager(new ParameterManager())
  {
  }
 
  Object::Object(const Name& rName)
    : m_Name(rName)
    , m_pParameterManager(new ParameterManager())
  {
  }
 
  Object::~Object()
  {
    delete m_pParameterManager;
    m_pParameterManager = NULL;
  }
 
 
  Module::Module(const std::string& rName)
    : Object(rName)
  {
  }
 
  Module::~Module()
  {
  }
 
 
  Sensor::Sensor(const Name& rName)
    : Object(rName)
  {
    m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
  }
 
  Sensor::~Sensor()
  {
  }
 
 
  SensorData::SensorData(const Name& rSensorName)
    : Object()
    , m_StateId(-1)
    , m_UniqueId(-1)
    , m_SensorName(rSensorName)
    , m_Time(0.0)
  {
  }
 
  SensorData::~SensorData()
  {
    forEach(CustomDataVector, &m_CustomData)
    {
      delete *iter;
    }
 
    m_CustomData.clear();
  }
 
 
  void CellUpdater::operator() (kt_int32u index)
  {
    kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
    kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
    kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
 
    m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
  }
 
 
  std::ostream& operator << (std::ostream& rStream, Exception& rException)
  {
    rStream << "Error detect: " << std::endl;
    rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
    rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;
 
    return rStream;
  }
 
 
  const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
                                                             CoordinateConverter* pCoordinateConverter,
                                                             kt_bool ignoreThresholdPoints,
                                                             kt_bool flipY) const
  {
    PointVectorDouble pointReadings;
 
    Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();
 
    // compute point readings
    kt_int32u beamNum = 0;
    kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
    for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
    {
      kt_double rangeReading = pRangeReadings[i];
 
      if (ignoreThresholdPoints)
      {
        if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
        {
          continue;
        }
      }
      else
      {
        rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
      }
 
      kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();
 
      Vector2<kt_double> point;
 
      point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
      point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
 
      if (pCoordinateConverter != NULL)
      {
        Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
        point.SetX(gridPoint.GetX());
        point.SetY(gridPoint.GetY());
      }
 
      pointReadings.push_back(point);
    }
 
    return pointReadings;
  }
 
  kt_bool LaserRangeFinder::Validate(SensorData* pSensorData)
  {
    LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);
 
    // verify number of range readings in LaserRangeScan matches the number of expected range readings
    if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
    {
      std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
                << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
      return false;
    }
 
    return true;
  }
 
 
  void ParameterManager::Clear()
  {
    forEach(karto::ParameterVector, &m_Parameters)
    {
      delete *iter;
    }
 
    m_Parameters.clear();
 
    m_ParameterLookup.clear();
  }
 
  void ParameterManager::Add(AbstractParameter* pParameter)
  {
    if (pParameter != NULL && pParameter->GetName() != "")
    {
      if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
      {
        m_Parameters.push_back(pParameter);
 
        m_ParameterLookup[pParameter->GetName()] = pParameter;
      }
      else
      {
        m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());
 
        assert(false);
      }
    }
  }
 
 
  /*
  std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
  {
    "Custom",
 
    "Sick_LMS100",
    "Sick_LMS200",
    "Sick_LMS291",
 
    "Hokuyo_UTM_30LX",
    "Hokuyo_URG_04LX"
  };
   */
}  // namespace karto