SensorData.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_SensorData_h__
#define __OpenKarto_SensorData_h__

#include <vector>

#include <OpenKarto/List.h>
#include <OpenKarto/Object.h>
#include <OpenKarto/Geometry.h>
#include <OpenKarto/Sensor.h>
#include <OpenKarto/Objects.h>
#include <OpenKarto/PoseTransform.h>
#include <OpenKarto/RigidBodyTransform.h>
#include <OpenKarto/SensorRegistry.h>
#include <OpenKarto/AbstractGpsEstimationManager.h>

namespace karto
{




  typedef List<kt_double> RangeReadingsList;

  typedef List<kt_double> DoubleList;


  struct SensorDataPrivate;

  class KARTO_EXPORT SensorData : public Object
  {
    KARTO_RTTI();

  protected:
    SensorData(const Identifier& rSensorIdentifier);

    //@cond EXCLUDE
    virtual ~SensorData();
    //@endcond
    
  public:
    inline kt_int32s GetStateId() const
    {
      return m_StateId;
    }

    inline void SetStateId(kt_int32s stateId)
    {
      m_StateId = stateId;
    }

    inline kt_int32s GetUniqueId() const
    {
      return m_UniqueId;
    }

    inline void SetUniqueId(kt_int32u uniqueId)
    {
      m_UniqueId = uniqueId;
    }
    
    inline kt_int64s GetTime() const
    {
      return m_Time;
    }

    inline void SetTime(kt_int64s time)
    {
      m_Time = time;
    }

    inline const Identifier& GetSensorIdentifier() const
    {
      return m_SensorIdentifier;
    }

    inline void SetSensorIdentifier(const Identifier& rSensorIdentifier)
    {
      m_SensorIdentifier = rSensorIdentifier;
    }

    void AddCustomItem(CustomItem* pCustomItem);
    
    const CustomItemList& GetCustomItems() const;

     kt_bool HasCustomItem();

  private:
    // restrict the following functions
    SensorData(const SensorData&);
    const SensorData& operator=(const SensorData&);

  private:
    SensorDataPrivate* m_pSensorDataPrivate;

    kt_int32s m_StateId;

    kt_int32s m_UniqueId;
    
    Identifier m_SensorIdentifier;

    kt_int64s m_Time;
  }; // SensorData

  KARTO_TYPE(SensorData);


  class KARTO_EXPORT LaserRangeScan : public SensorData
  {
    KARTO_RTTI();

  public:
    LaserRangeScan(const Identifier& rSensorIdentifier);

    LaserRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rRangeReadings);

  protected:
    //@cond EXCLUDE
    virtual ~LaserRangeScan();
    //@endcond
    
  public:
    inline const RangeReadingsList& GetRangeReadings() const
    {
      return m_RangeReadings;
    }
  
    inline void SetRangeReadings(const RangeReadingsList& rRangeReadings)
    {
      m_RangeReadings = rRangeReadings;
    }

    inline LaserRangeFinder* GetLaserRangeFinder() const
    {
      return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
    }

  private:
    // restrict the following functions
    LaserRangeScan(const LaserRangeScan&);
    const LaserRangeScan& operator=(const LaserRangeScan&);

  private:
    RangeReadingsList m_RangeReadings;
  }; // LaserRangeScan

  KARTO_TYPE(LaserRangeScan);


  class DrivePose : public SensorData
  {
    KARTO_RTTI();

  public:
    DrivePose(const Identifier& rSensorIdentifier)
      : SensorData(rSensorIdentifier)
    {
    }

  protected:
    //@cond EXCLUDE
    virtual ~DrivePose()
    {
    }
    //@endcond

  public:
    inline const Pose2& GetOdometricPose() const 
    {
      return m_OdometricPose;
    }

    inline void SetOdometricPose(const Pose2& rPose)
    {
      m_OdometricPose = rPose;
    }

  private:
    // restrict the following functions
    DrivePose(const DrivePose&);
    const DrivePose& operator=(const DrivePose&);

  private:
    Pose2 m_OdometricPose;
  }; // class DrivePose

  KARTO_TYPE(DrivePose);


  class KARTO_EXPORT LocalizedObject : public SensorData
  {
    KARTO_RTTI();

  public:
    LocalizedObject(const Identifier& rSensorIdentifier);
    
  protected:
    //@cond EXCLUDE
    virtual ~LocalizedObject();
    //@endcond
    
  public:
    inline const Pose2& GetOdometricPose() const 
    {
      return m_OdometricPose;
    }
    
    inline void SetOdometricPose(const Pose2& rOdometricPose)
    {
      m_OdometricPose = rOdometricPose;
    }
    
    inline const Pose2& GetCorrectedPose() const 
    {
      return m_CorrectedPose;
    }
    
    virtual inline void SetCorrectedPose(const Pose2& rPose)
    {
      m_CorrectedPose = rPose;
    }

    inline gps::PointGps GetGpsReading() const
    {
      return m_GpsReading;
    }
    
    inline void SetGpsReading(const gps::PointGps& rGpsReading)
    {
      m_GpsReading = rGpsReading;
      m_IsGpsReadingValid = true;
    }
    
    inline kt_bool IsGpsReadingValid() const
    {
      return m_IsGpsReadingValid;
    }
    
    inline gps::PointGps GetGpsEstimate() const
    {
      if (m_pGpsEstimationManager != NULL)
      {
        return m_pGpsEstimationManager->GetGpsEstimate(this);
      }
      else
      {
        return m_GpsEstimate;
      }
    }
    
    inline void SetGpsEstimate(const gps::PointGps& rGpsEstimate)
    {
      if (m_pGpsEstimationManager != NULL)
      {
        m_pGpsEstimationManager->SetGpsEstimate(this, rGpsEstimate);
      }
      else
      {
        m_GpsEstimate = rGpsEstimate;
        m_IsGpsEstimateValid = true;
      }
    }
    
    inline kt_bool IsGpsEstimateValid() const
    {
      if (m_pGpsEstimationManager != NULL)
      {
        return m_pGpsEstimationManager->IsGpsEstimateValid(this);
      }
      else
      {
        return m_IsGpsEstimateValid;
      }
    }
    
    inline void SetGpsEstimationManager(AbstractGpsEstimationManager* pGpsEstimationManager)
    {
      m_pGpsEstimationManager = pGpsEstimationManager;
    }

  private:
    Pose2 m_OdometricPose;
    
    Pose2 m_CorrectedPose;

    gps::PointGps m_GpsReading;
    
    kt_bool m_IsGpsReadingValid;

    gps::PointGps m_GpsEstimate;
    
    kt_bool m_IsGpsEstimateValid;

    AbstractGpsEstimationManager* m_pGpsEstimationManager;
  }; // LocalizedObject

  KARTO_TYPE(LocalizedObject);

  typedef SmartPointer<LocalizedObject> LocalizedObjectPtr;

  typedef List<LocalizedObjectPtr> LocalizedObjectList;


  class AbstractGpsEstimationManager;

  class KARTO_EXPORT LocalizedLaserScan : public LocalizedObject
  {    
    KARTO_RTTI();

  public:
    inline LaserRangeFinder* GetLaserRangeFinder() const
    {
      return SensorRegistry::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorIdentifier());
    }

    virtual inline void SetCorrectedPose(const Pose2& rCorrectedPose)
    {
      LocalizedObject::SetCorrectedPose(rCorrectedPose);
      
      m_IsDirty = true;
    }
    
    inline const Pose2& GetBarycenterPose() const 
    {
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        const_cast<LocalizedLaserScan*>(this)->Update();
      }
      
      return m_BarycenterPose;
    }
    
    inline Pose2 GetReferencePose(kt_bool useBarycenter) const 
    {
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        const_cast<LocalizedLaserScan*>(this)->Update();
      }
      
      return useBarycenter ? GetBarycenterPose() : GetSensorPose();
    }    
    
    inline Pose2 GetSensorPose() const
    {
      return GetSensorAt(GetCorrectedPose());
    }
    
    void SetSensorPose(const Pose2& rSensorPose)
    {
      Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
      kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
      kt_double offsetHeading = deviceOffsetPose2.GetHeading();
      kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
      kt_double correctedHeading = math::NormalizeAngle(rSensorPose.GetHeading());
      Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
                                      offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
                                      offsetHeading);
      
      SetCorrectedPose(rSensorPose - worldSensorOffset);
      
      Update();
    }
    
    inline Pose2 GetSensorAt(const Pose2& rPose) const
    {
      return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
    }    
    
    inline const BoundingBox2& GetBoundingBox() const
    {
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        const_cast<LocalizedLaserScan*>(this)->Update();
      }
      
      return m_BoundingBox;
    }

    const Vector2dList& GetPointReadings(kt_bool wantFiltered = false) const;
    
    inline const RangeReadingsList& GetRangeReadings() const
    {
      return m_RangeReadings;
    }

    inline kt_size_t GetNumberOfRangeReadings() const
    {
      return m_RangeReadings.Size();
    }

  protected:
    LocalizedLaserScan(const Identifier& rSensorIdentifier);
    virtual ~LocalizedLaserScan();

    void Update();
    
    virtual void ComputePointReadings() = 0;

    virtual const Vector2dList& GetFilteredPointReadings() const
    {
      return m_FilteredPointReadings;
    }
    
    virtual const Vector2dList& GetUnfilteredPointReadings() const
    {
      return m_UnfilteredPointReadings;
    }

  protected:
    Vector2dList m_FilteredPointReadings;
    
    Vector2dList m_UnfilteredPointReadings;
    
    RangeReadingsList m_RangeReadings;
    
  private:
    Identifier m_SensorIdentifier;
        
    Pose2 m_BarycenterPose;
    
    BoundingBox2 m_BoundingBox;
    
    kt_bool m_IsDirty;
    
  }; // LocalizedLaserScan
  
  KARTO_TYPE(LocalizedLaserScan);

  typedef SmartPointer<LocalizedLaserScan> LocalizedLaserScanPtr;
  
  typedef List<LocalizedLaserScanPtr> LocalizedLaserScanList;
  
  
  class KARTO_EXPORT LocalizedPointScan : public LocalizedLaserScan
  {
    KARTO_RTTI();
   
  public:
    LocalizedPointScan(const Identifier& rSensorIdentifier, const Vector2dList& rLocalPoints);
    
    inline const DoubleList& GetLocalAngles() const
    {
      return m_LocalAngles;
    }    
    
  protected:
    //@cond EXCLUDE
    virtual ~LocalizedPointScan();
    //@endcond
    
  private:
    virtual void ComputePointReadings();

  private:
    LocalizedPointScan(const LocalizedPointScan&);
    const LocalizedPointScan& operator=(const LocalizedPointScan&);
    
  private:
    Vector2dList m_LocalPointReadings;
    DoubleList m_LocalAngles;
  }; // LocalizedPointScan
  
  KARTO_TYPE(LocalizedPointScan);

  
  class KARTO_EXPORT LocalizedRangeScan : public LocalizedLaserScan
  {
    KARTO_RTTI();

  public:
    LocalizedRangeScan(const Identifier& rSensorIdentifier, const RangeReadingsList& rReadings);

    KARTO_DEPRECATED KARTO_FORCEINLINE LocalizedRangeScan(const Identifier& rSensorIdentifier, std::vector<kt_double>& rRangeReadings)
      : LocalizedLaserScan(rSensorIdentifier)
    {
      m_RangeReadings.Resize(static_cast<kt_int32u>(rRangeReadings.size()));
      if (rRangeReadings.size() > 0)
      {
        memcpy(&(m_RangeReadings[0]), &(rRangeReadings[0]), sizeof(kt_double) * rRangeReadings.size());
      }
    }
  
  protected:
    //@cond EXCLUDE
    virtual ~LocalizedRangeScan();
    //@endcond
    
  private:
    virtual void ComputePointReadings();
    
  private:
    LocalizedRangeScan(const LocalizedRangeScan&);
    const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
  }; // LocalizedRangeScan

  KARTO_TYPE(LocalizedRangeScan);


}

#endif // __OpenKarto_SensorData_h__