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  * Author: Masaru Morita
41  */
42 
43 #ifndef ODOMETRY_ACKERMANN_STEERING_H_
44 #define ODOMETRY_ACKERMANN_STEERING_H_
45 
46 #include <ros/time.h>
47 #include <boost/accumulators/accumulators.hpp>
48 #include <boost/accumulators/statistics/stats.hpp>
49 #include <boost/accumulators/statistics/rolling_mean.hpp>
50 #include <boost/function.hpp>
51 
53 {
54  namespace bacc = boost::accumulators;
55 
60  class Odometry
61  {
62  public:
63 
65  typedef boost::function<void(double, double)> IntegrationFunction;
66 
73  Odometry(size_t velocity_rolling_window_size = 10);
74 
79  void init(const ros::Time &time);
80 
88  bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time);
89 
96  void updateOpenLoop(double linear, double angular, const ros::Time &time);
97 
102  double getHeading() const
103  {
104  return heading_;
105  }
106 
111  double getX() const
112  {
113  return x_;
114  }
115 
120  double getY() const
121  {
122  return y_;
123  }
124 
129  double getLinear() const
130  {
131  return linear_;
132  }
133 
138  double getAngular() const
139  {
140  return angular_;
141  }
142 
148  void setWheelParams(double wheel_reparation_h, double 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 
196 
199 
202  RollingMeanAcc linear_acc_;
203  RollingMeanAcc angular_acc_;
204 
206  IntegrationFunction integrate_fun_;
207  };
208 }
209 
210 #endif /* ODOMETRY_ACKERMANN_STEERING_H_ */
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:68
bacc::tag::rolling_window RollingWindow
Definition: odometry.h:160
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:51
size_t velocity_rolling_window_size_
Rolling mean accumulators for the linar and angular velocities:
Definition: odometry.h:201
double rear_wheel_old_pos_
Previous wheel position/state [rad]:
Definition: odometry.h:198
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:60
boost::function< void(double, double)> IntegrationFunction
Integration function, used to integrate the odometry:
Definition: odometry.h:65
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:159
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:139
double getAngular() const
angular velocity getter
Definition: odometry.h:138
void resetAccumulators()
Reset linear and angular accumulators.
Definition: odometry.cpp:169
void setWheelParams(double wheel_reparation_h, double wheel_radius)
Sets the wheel parameters: radius and separation.
Definition: odometry.cpp:126
double linear_
Current velocity:
Definition: odometry.h:190
double getHeading() const
heading getter
Definition: odometry.h:102
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:154
double getLinear() const
linear velocity getter
Definition: odometry.h:129
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:114
double getX() const
x position getter
Definition: odometry.h:111
ros::Time timestamp_
Current timestamp:
Definition: odometry.h:182
double wheel_separation_h_
Wheel kinematic parameters [m]:
Definition: odometry.h:194
double getY() const
y position getter
Definition: odometry.h:120
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:132
bool update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:75
IntegrationFunction integrate_fun_
Integration funcion, used to integrate the odometry:
Definition: odometry.h:206


ackermann_steering_controller
Author(s): Masaru Morita
autogenerated on Thu Apr 11 2019 03:08:13