PoseIntegrator.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H
#define SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H

#include <memory>
#include <string>

#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Devices/DeviceFilters/DeviceFilter.h"
#include "SurgSim/Framework/Timer.h"
#include "SurgSim/Math/RigidTransform.h"

namespace SurgSim
{
namespace DataStructures
{
class DataGroupCopier;
};

namespace Devices
{

SURGSIM_STATIC_REGISTRATION(PoseIntegrator);

class PoseIntegrator : public DeviceFilter
{
public:
    typedef Math::RigidTransform3d PoseType;

    explicit PoseIntegrator(const std::string& name);

    SURGSIM_CLASSNAME(SurgSim::Devices::PoseIntegrator);

    const PoseType& integrate(const PoseType& pose);

    void initializeInput(const std::string& device, const DataStructures::DataGroup& inputData) override;

    void handleInput(const std::string& device, const DataStructures::DataGroup& inputData) override;

    void setReset(const std::string& name);

protected:
    void filterOutput(const std::string& device, 
                      const SurgSim::DataStructures::DataGroup& dataToFilter, 
                      SurgSim::DataStructures::DataGroup* result) override;

private:
    PoseType m_poseResult;

    Framework::Timer m_timer;

    std::shared_ptr<DataStructures::DataGroupCopier> m_copier;

    std::string m_resetName;

    SurgSim::DataStructures::OptionalValue<SurgSim::Math::RigidTransform3d> m_resetPose;
};


};  // namespace Devices
};  // namespace SurgSim

#endif  // SURGSIM_DEVICES_DEVICEFILTERS_POSEINTEGRATOR_H