odometry.h

Go to the documentation of this file.

#ifndef ODOMETRY_H_
#define ODOMETRY_H_

#include <ros/time.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/function.hpp>
#include <ackermann_controller/joint.h>

namespace ackermann_controller
{
namespace bacc = boost::accumulators;

class Odometry
{
public:

    typedef boost::function<void(double, double)> IntegrationFunction;

    Odometry(size_t velocity_rolling_window_size = 10);

    void init(const ros::Time &time);

    bool update(
        const std::vector<ActuatedJoint>& steering_joints,
        const std::vector<Wheel>& odometry_joints,
        const ros::Time &time);

    void updateOpenLoop(double linear, double angular, const ros::Time &time);

    double getHeading() const
    {
        return heading_;
    }

    double getX() const
    {
        return x_ + wheelbase_ * (1.0 - cos(heading_));
    }

    double getY() const
    {
        return y_ - wheelbase_ * sin(heading_);
    }

    double getLinear() const
    {
        return linear_;
    }

    double getAngular() const
    {
        return angular_;
    }

    void setWheelbase(double wheelbase)
    {
        wheelbase_ = wheelbase;
    }

    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);

private:

    typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
    typedef bacc::tag::rolling_window RollingWindow;

    void integrateRungeKutta2(double linear, double angular);

    void integrateExact(double linear, double angular);

    void resetAccumulators();

    ros::Time timestamp_;

    double x_;        //   [m]
    double y_;        //   [m]
    double heading_;  // [rad]

    double linear_;  //   [m/s]
    double angular_; // [rad/s]

    double wheelbase_;

    std::map<std::string, double> wheels_old_pos_;

    size_t velocity_rolling_window_size_;
    RollingMeanAcc linear_acc_;
    RollingMeanAcc angular_acc_;

    IntegrationFunction integrate_fun_;
};
}

#endif /* ODOMETRY_H_ */