odometry.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, PAL Robotics, S.L.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PAL Robotics nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /*
36  * Author: Luca Marchionni
37  * Author: Bence Magyar
38  * Author: Enrique Fernández
39  * Author: Paul Mathieu
40  */
41 
42 #ifndef ODOMETRY_H_
43 #define ODOMETRY_H_
44 
45 #include <ros/time.h>
46 #include <boost/accumulators/accumulators.hpp>
47 #include <boost/accumulators/statistics/stats.hpp>
48 #include <boost/accumulators/statistics/rolling_mean.hpp>
49 #include <boost/function.hpp>
50 
51 namespace diff_drive_controller
52 {
53  namespace bacc = boost::accumulators;
54 
59  class Odometry
60  {
61  public:
62 
64  typedef boost::function<void(double, double)> IntegrationFunction;
65 
72  Odometry(size_t velocity_rolling_window_size = 10);
73 
78  void init(const ros::Time &time);
79 
87  bool update(double left_pos, double right_pos, const ros::Time &time);
88 
95  void updateOpenLoop(double linear, double angular, const ros::Time &time);
96 
101  double getHeading() const
102  {
103  return heading_;
104  }
105 
110  double getX() const
111  {
112  return x_;
113  }
114 
119  double getY() const
120  {
121  return y_;
122  }
123 
128  double getLinear() const
129  {
130  return linear_;
131  }
132 
137  double getAngular() const
138  {
139  return angular_;
140  }
141 
148  void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius);
149 
154  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
155 
156  private:
157 
159  typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
160  typedef bacc::tag::rolling_window RollingWindow;
161 
167  void integrateRungeKutta2(double linear, double angular);
168 
174  void integrateExact(double linear, double angular);
175 
179  void resetAccumulators();
180 
183 
185  double x_; // [m]
186  double y_; // [m]
187  double heading_; // [rad]
188 
190  double linear_; // [m/s]
191  double angular_; // [rad/s]
192 
197 
201 
204  RollingMeanAcc linear_acc_;
205  RollingMeanAcc angular_acc_;
206 
208  IntegrationFunction integrate_fun_;
209  };
210 }
211 
212 #endif /* ODOMETRY_H_ */
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:159
bool update(double left_pos, double right_pos, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:76
double left_wheel_old_pos_
Previou wheel position/state [rad]:
Definition: odometry.h:199
ros::Time timestamp_
Current timestamp:
Definition: odometry.h:182
double getHeading() const
heading getter
Definition: odometry.h:101
double linear_
Current velocity:
Definition: odometry.h:190
Odometry(size_t velocity_rolling_window_size=10)
Constructor Timestamp will get the current time value Value will be set to zero.
Definition: odometry.cpp:50
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:155
double getY() const
y position getter
Definition: odometry.h:119
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:140
double getAngular() const
angular velocity getter
Definition: odometry.h:137
double getX() const
x position getter
Definition: odometry.h:110
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:69
double x_
Current pose:
Definition: odometry.h:185
void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:126
boost::function< void(double, double)> IntegrationFunction
Integration function, used to integrate the odometry:
Definition: odometry.h:64
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:59
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:133
bacc::tag::rolling_window RollingWindow
Definition: odometry.h:160
size_t velocity_rolling_window_size_
Rolling mean accumulators for the linar and angular velocities:
Definition: odometry.h:203
double getLinear() const
linear velocity getter
Definition: odometry.h:128
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:114
double wheel_separation_
Wheel kinematic parameters [m]:
Definition: odometry.h:194
IntegrationFunction integrate_fun_
Integration funcion, used to integrate the odometry:
Definition: odometry.h:208
void resetAccumulators()
Reset linear and angular accumulators.
Definition: odometry.cpp:170


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05