Sensor.h

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/>.
 */
 
#pragma once
 
#ifndef __OpenKarto_Sensor_h__
#define __OpenKarto_Sensor_h__
 
#include <OpenKarto/List.h>
#include <OpenKarto/Object.h>
 
namespace karto
{
 
 
 
  typedef List<Vector2d> Vector2dList;
 
 
  class SensorData;
 
  class KARTO_EXPORT Sensor : public Object
  {
    KARTO_RTTI();
 
  protected:
    Sensor(const Identifier& rName);
 
    //@cond EXCLUDE
    virtual ~Sensor();
    //@endcond
    
  public:
    inline const Pose2& GetOffsetPose() const
    {
      return m_pOffsetPose->GetValue();
    }
    
    inline void SetOffsetPose(const Pose2& rPose)
    {
      m_pOffsetPose->SetValue(rPose);
    }
 
    virtual void Validate() = 0;
 
    virtual void Validate(SensorData* pSensorData) = 0;
 
  private:
    // restrict the following functions
    Sensor(const Sensor&);
    const Sensor& operator=(const Sensor&);
 
  private:
    Parameter<Pose2>* m_pOffsetPose;
  }; // Sensor
 
  KARTO_TYPE(Sensor);
 
  typedef SmartPointer<Sensor> SensorPtr;
  
  typedef List<SensorPtr> SensorList;
 
 
  class Drive : public Sensor
  {
    KARTO_RTTI();
 
  public:
    Drive(const Identifier& rName)
      : Sensor(rName)
    {
    }
 
  protected:
    //@cond EXCLUDE
    virtual ~Drive()
    {
    }
    //@endcond
 
  public:
    virtual void Validate()
    {
    }
 
    virtual void Validate(SensorData* pSensorData)
    {
      if (pSensorData == NULL)
      {
        throw Exception("SensorData == NULL");
      }
    }
 
  private:
    // restrict the following functions
    Drive(const Drive&);
    const Drive& operator=(const Drive&);
  }; // class Drive
 
  KARTO_TYPE(Drive);
 
 
  typedef enum
  {
    LaserRangeFinder_Custom = 0,
 
    LaserRangeFinder_Sick_LMS100 = 1,
    LaserRangeFinder_Sick_LMS200 = 2,
    LaserRangeFinder_Sick_LMS291 = 3,
 
    LaserRangeFinder_Hokuyo_UTM_30LX = 4,
    LaserRangeFinder_Hokuyo_URG_04LX = 5    
  } LaserRangeFinderType;
 
  KARTO_EXPORT void RegisterLaserRangeFinderType();
 
  KARTO_AUTO_TYPE(LaserRangeFinderType, &RegisterLaserRangeFinderType);
 
 
  class LocalizedLaserScan;
  class CoordinateConverter;
  class LaserRangeFinder;
 
  class KARTO_EXPORT LaserRangeFinder : public Sensor
  {
    KARTO_RTTI();
 
  protected:
    //@cond EXCLUDE
    virtual ~LaserRangeFinder();
    //@endcond
    
  public:   
    inline kt_double GetMinimumRange() const
    {
      return m_pMinimumRange->GetValue();
    }
 
    inline void SetMinimumRange(kt_double minimumRange)
    {
      m_pMinimumRange->SetValue(minimumRange);
      
      SetRangeThreshold(GetRangeThreshold());
    }
 
    inline kt_double GetMaximumRange() const
    {
      return m_pMaximumRange->GetValue();
    }
 
    inline void SetMaximumRange(kt_double maximumRange)
    {
      m_pMaximumRange->SetValue(maximumRange);
 
      SetRangeThreshold(GetRangeThreshold());
    }
 
    inline kt_double GetRangeThreshold() const
    {
      return m_pRangeThreshold->GetValue();
    }
 
    void SetRangeThreshold(kt_double rangeThreshold);
 
    inline kt_double GetMinimumAngle() const
    {
      return m_pMinimumAngle->GetValue();
    }
    
    inline void SetMinimumAngle(kt_double minimumAngle)
    {
      m_pMinimumAngle->SetValue(minimumAngle);
 
      Update();
    }
 
    inline kt_double GetMaximumAngle() const
    {
      return m_pMaximumAngle->GetValue();
    }
    
    inline void SetMaximumAngle(kt_double maximumAngle)
    {
      m_pMaximumAngle->SetValue(maximumAngle);
 
      Update();
    }
    
    inline kt_double GetAngularResolution() const
    {
      return m_pAngularResolution->GetValue();
    }
    
    void SetAngularResolution(kt_double angularResolution);
 
    inline kt_int64s GetType()
    {
      return m_pType->GetValue();
    }
 
    inline kt_int32u GetNumberOfRangeReadings() const
    {
      return m_NumberOfRangeReadings;
    }
 
    virtual void Validate();
 
    virtual void Validate(SensorData* pSensorData);
 
    const Vector2dList GetPointReadings(LocalizedLaserScan* pLocalizedLaserScan, CoordinateConverter* pCoordinateConverter, kt_bool ignoreThresholdPoints = true, kt_bool flipY = false) const;
 
  public:
    static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Identifier& rName);
 
  private:
    LaserRangeFinder(const Identifier& rName);
    
    void Update()
    {
      m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() - GetMinimumAngle()) / GetAngularResolution()) + 1);
    }
 
  private:
    LaserRangeFinder(const LaserRangeFinder&);
    const LaserRangeFinder& operator=(const LaserRangeFinder&);
 
  private:
    // sensor m_Parameters
    Parameter<kt_double>* m_pMinimumAngle;
    Parameter<kt_double>* m_pMaximumAngle;
 
    Parameter<kt_double>* m_pAngularResolution;
 
    Parameter<kt_double>* m_pMinimumRange;
    Parameter<kt_double>* m_pMaximumRange;
 
    Parameter<kt_double>* m_pRangeThreshold;
 
    ParameterEnum* m_pType;
 
    kt_int32u m_NumberOfRangeReadings;    
  }; // LaserRangeFinder
 
  KARTO_TYPE(LaserRangeFinder);
 
  typedef SmartPointer<LaserRangeFinder> LaserRangeFinderPtr;
  
 
}
 
#endif // __OpenKarto_Sensor_h__