odometry.cpp
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 
43 
44 #include <boost/bind.hpp>
45 
46 namespace diff_drive_controller
47 {
48  namespace bacc = boost::accumulators;
49 
50  Odometry::Odometry(size_t velocity_rolling_window_size)
51  : timestamp_(0.0)
52  , x_(0.0)
53  , y_(0.0)
54  , heading_(0.0)
55  , linear_(0.0)
56  , angular_(0.0)
57  , wheel_separation_(0.0)
58  , left_wheel_radius_(0.0)
59  , right_wheel_radius_(0.0)
60  , left_wheel_old_pos_(0.0)
61  , right_wheel_old_pos_(0.0)
62  , velocity_rolling_window_size_(velocity_rolling_window_size)
63  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
64  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
65  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
66  {
67  }
68 
69  void Odometry::init(const ros::Time& time)
70  {
71  // Reset accumulators and timestamp:
73  timestamp_ = time;
74  }
75 
76  bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
77  {
79  const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
80  const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
81 
83  const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
84  const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
85 
87  left_wheel_old_pos_ = left_wheel_cur_pos;
88  right_wheel_old_pos_ = right_wheel_cur_pos;
89 
91  const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
92  const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
93 
95  integrate_fun_(linear, angular);
96 
98  const double dt = (time - timestamp_).toSec();
99  if (dt < 0.0001)
100  return false; // Interval too small to integrate with
101 
102  timestamp_ = time;
103 
105  linear_acc_(linear/dt);
106  angular_acc_(angular/dt);
107 
108  linear_ = bacc::rolling_mean(linear_acc_);
109  angular_ = bacc::rolling_mean(angular_acc_);
110 
111  return true;
112  }
113 
114  void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
115  {
117  linear_ = linear;
118  angular_ = angular;
119 
121  const double dt = (time - timestamp_).toSec();
122  timestamp_ = time;
123  integrate_fun_(linear * dt, angular * dt);
124  }
125 
126  void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
127  {
128  wheel_separation_ = wheel_separation;
129  left_wheel_radius_ = left_wheel_radius;
130  right_wheel_radius_ = right_wheel_radius;
131  }
132 
133  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
134  {
135  velocity_rolling_window_size_ = velocity_rolling_window_size;
136 
138  }
139 
140  void Odometry::integrateRungeKutta2(double linear, double angular)
141  {
142  const double direction = heading_ + angular * 0.5;
143 
145  x_ += linear * cos(direction);
146  y_ += linear * sin(direction);
147  heading_ += angular;
148  }
149 
155  void Odometry::integrateExact(double linear, double angular)
156  {
157  if (fabs(angular) < 1e-6)
158  integrateRungeKutta2(linear, angular);
159  else
160  {
162  const double heading_old = heading_;
163  const double r = linear/angular;
164  heading_ += angular;
165  x_ += r * (sin(heading_) - sin(heading_old));
166  y_ += -r * (cos(heading_) - cos(heading_old));
167  }
168  }
169 
171  {
172  linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
173  angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
174  }
175 
176 } // namespace diff_drive_controller
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 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
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:140
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
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
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