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 
52 {
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 drive_motor_vel, double steer_motor_vel, 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 setWheelsParams(double wheel_radius, double wheel_separation);
149 
155  void setGearRatios(double drive_motor_gear_ratio, double steer_motor_gear_ratio);
156 
157 private:
158 
160  typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
161  typedef bacc::tag::rolling_window RollingWindow;
162 
168  void integrateExact(double linear, double angular);
169 
172 
174  double x_; // [m]
175  double y_; // [m]
176  double heading_; // [rad]
177 
179  double linear_; // [m/s]
180  double angular_; // [rad/s]
181 
185 
188  RollingMeanAcc linear_acc_;
189  RollingMeanAcc angular_acc_;
190 
192  IntegrationFunction integrate_fun_;
193 };
194 
195 } // namespace double_diff_drive_controller
196 
197 #endif /* ODOMETRY_H_ */
double getAngular() const
angular velocity getter
Definition: odometry.h:138
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:160
IntegrationFunction integrate_fun_
Integration funcion, used to integrate the odometry:
Definition: odometry.h:192
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:128
void setGearRatios(double drive_motor_gear_ratio, double steer_motor_gear_ratio)
Sets the gear ratio parameters: gear ratio.
Definition: odometry.cpp:121
double linear_
Current velocity:
Definition: odometry.h:179
double getHeading() const
heading getter
Definition: odometry.h:102
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:60
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:54
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:101
ros::Time timestamp_
Current timestamp:
Definition: odometry.h:171
double getY() const
y position getter
Definition: odometry.h:120
bacc::tag::rolling_window RollingWindow
Definition: odometry.h:161
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:71
boost::function< void(double, double)> IntegrationFunction
Integration function, used to integrate the odometry:
Definition: odometry.h:65
void setWheelsParams(double wheel_radius, double wheel_separation)
Sets the wheels parameters: radius and seperation.
Definition: odometry.cpp:114
bool update(double drive_motor_vel, double steer_motor_vel, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:82
double getLinear() const
linear velocity getter
Definition: odometry.h:129
size_t velocity_rolling_window_size_
Rolling mean accumulators for the linar and angular velocities:
Definition: odometry.h:187
double getX() const
x position getter
Definition: odometry.h:111
double wheel_radius_
Wheels kinematic parameters [m]:
Definition: odometry.h:183


moose_control
Author(s): Tony Baltovski
autogenerated on Wed Mar 10 2021 03:43:55