linear_pose_filter_2d.h

Go to the documentation of this file.

/*
 * Copyright 2015 Fetch Robotics Inc.
 * Author: Griswald Brooks
 *
 * 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/>.
 */

#ifndef FETCH_AUTO_DOCK_LINEAR_POSE_FILTER_H
#define FETCH_AUTO_DOCK_LINEAR_POSE_FILTER_H

// STL includes.
#include <vector>
#include <deque>

// ROS includes.
#include <geometry_msgs/Pose.h>

// Boost includes.
#include <boost/shared_ptr.hpp>

class LinearPoseFilter2D
{
public:
  LinearPoseFilter2D(const std::vector<float>& b, const std::vector<float>& a);

  void setCoeff(const std::vector<float>& b, const std::vector<float>& a);

  geometry_msgs::Pose filter(const geometry_msgs::Pose& pose);

  void reset();

  void setFilterState(const geometry_msgs::Pose& input_pose, const geometry_msgs::Pose& output_pose);

  void setFilterState(const std::vector<geometry_msgs::Pose>& input_poses,
                      const std::vector<geometry_msgs::Pose>& output_poses);

  void setFilterState(const geometry_msgs::Pose& input_pose);
  void setFilterState(const std::vector<geometry_msgs::Pose>& input_poses);
private:
  geometry_msgs::Pose originPose();

  float getUnnormalizedYaw(geometry_msgs::Pose pose, float reference_yaw);

  float getNewestOutputYaw();

  std::deque<geometry_msgs::Pose> out_;   
  std::deque<geometry_msgs::Pose> in_;    
  std::deque<float> yaw_out_;             
  std::deque<float> yaw_in_;              
  std::vector<float> b_;                  
  std::vector<float> a_;                  
};

typedef boost::shared_ptr<LinearPoseFilter2D> LinearPoseFilter2DPtr;

#endif  // FETCH_AUTO_DOCK_LINEAR_POSE_FILTER_H