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__