odometry.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Irstea
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 Irstea 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 #ifndef ODOMETRY_H_
36 #define ODOMETRY_H_
37 
38 #include <ros/time.h>
39 #include <boost/accumulators/accumulators.hpp>
40 #include <boost/accumulators/statistics/stats.hpp>
41 #include <boost/accumulators/statistics/rolling_mean.hpp>
42 #include <boost/function.hpp>
43 
45 {
46  namespace bacc = boost::accumulators;
47 
52  class Odometry
53  {
54  public:
55 
57  typedef boost::function<void(double, double)> IntegrationFunction;
58 
65  Odometry(size_t velocity_rolling_window_size = 10);
66 
71  void init(const ros::Time &time);
72 
84  bool update(const double& fl_speed, const double& fr_speed, const double& rl_speed, const double& rr_speed,
85  double front_steering, double rear_steering, const ros::Time &time);
86 
91  double getHeading() const
92  {
93  return heading_;
94  }
95 
100  double getX() const
101  {
102  return x_;
103  }
104 
109  double getY() const
110  {
111  return y_;
112  }
113 
114 
119  double getLinear() const
120  {
121  return linear_;
122  }
123 
128  double getLinearX() const
129  {
130  return linear_x_;
131  }
132 
137  double getLinearY() const
138  {
139  return linear_y_;
140  }
141 
146  double getAngular() const
147  {
148  return angular_;
149  }
150 
155  double getLinearAcceleration() const
156  {
157  return bacc::rolling_mean(linear_accel_acc_);
158  }
159 
164  double getLinearJerk() const
165  {
166  return bacc::rolling_mean(linear_jerk_acc_);
167  }
168 
173  double getFrontSteerVel() const
174  {
175  return bacc::rolling_mean(front_steer_vel_acc_);
176  }
177 
182  double getRearSteerVel() const
183  {
184  return bacc::rolling_mean(rear_steer_vel_acc_);
185  }
186 
194  void setWheelParams(double steering_track, double wheel_steering_y_offset, double wheel_radius, double wheel_base);
195 
200  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
201 
202  private:
203 
205  typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
206  typedef bacc::tag::rolling_window RollingWindow;
207 
214  void integrateXY(double linear_x, double linear_y, double angular);
215 
221  void integrateRungeKutta2(double linear, double angular);
222 
228  void integrateExact(double linear, double angular);
229 
233  void resetAccumulators();
234 
237 
239  double x_; // [m]
240  double y_; // [m]
241  double heading_; // [rad]
242 
244  double linear_, linear_x_, linear_y_; // [m/s]
245  double angular_; // [rad/s]
246 
251  double wheel_base_;
252 
255 
258  RollingMeanAcc linear_accel_acc_;
259  RollingMeanAcc linear_jerk_acc_;
260  RollingMeanAcc front_steer_vel_acc_;
261  RollingMeanAcc rear_steer_vel_acc_;
264  };
265 }
266 
267 #endif /* ODOMETRY_H_ */
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:131
size_t velocity_rolling_window_size_
Rolling mean accumulators for the linar and angular velocities:
Definition: odometry.h:257
double getX() const
x position getter
Definition: odometry.h:100
double getLinearY() const
linear velocity getter along Y on the robot base link frame
Definition: odometry.h:137
double getLinearAcceleration() const
linear acceleration getter
Definition: odometry.h:155
boost::function< void(double, double)> IntegrationFunction
Integration function, used to integrate the odometry:
Definition: odometry.h:57
double getLinear() const
linear velocity getter norm
Definition: odometry.h:119
double wheel_old_pos_
Previous wheel position/state [rad]:
Definition: odometry.h:254
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:43
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:52
double getY() const
y position getter
Definition: odometry.h:109
ros::Time last_update_timestamp_
Current timestamp:
Definition: odometry.h:236
bool update(const double &fl_speed, const double &fr_speed, const double &rl_speed, const double &rr_speed, double front_steering, double rear_steering, const ros::Time &time)
Updates the odometry class with latest wheels and steerings position.
Definition: odometry.cpp:72
double linear_
Current velocity:
Definition: odometry.h:244
bacc::tag::rolling_window RollingWindow
Definition: odometry.h:206
void setWheelParams(double steering_track, double wheel_steering_y_offset, double wheel_radius, double wheel_base)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:123
void integrateXY(double linear_x, double linear_y, double angular)
Integrates the velocities (linear on x and y and angular)
Definition: odometry.cpp:138
void resetAccumulators()
Reset linear and angular accumulators.
Definition: odometry.cpp:173
double getLinearJerk() const
linear jerk getter
Definition: odometry.h:164
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:205
double getFrontSteerVel() const
front steering velocity getter
Definition: odometry.h:173
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:148
double getHeading() const
heading getter
Definition: odometry.h:91
double getRearSteerVel() const
rear steering velocity getter
Definition: odometry.h:182
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:65
double getAngular() const
angular velocity getter
Definition: odometry.h:146
double steering_track_
Wheel kinematic parameters [m]:
Definition: odometry.h:248
double getLinearX() const
linear velocity getter along X on the robot base link frame
Definition: odometry.h:128
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:158


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Apr 18 2020 03:58:13