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  * Author: Masaru Morita
41  */
42 
44 
45 #include <boost/bind.hpp>
46 
48 {
49  namespace bacc = boost::accumulators;
50 
51  Odometry::Odometry(size_t velocity_rolling_window_size)
52  : timestamp_(0.0)
53  , x_(0.0)
54  , y_(0.0)
55  , heading_(0.0)
56  , linear_(0.0)
57  , angular_(0.0)
58  , wheel_separation_h_(0.0)
59  , wheel_radius_(0.0)
60  , rear_wheel_old_pos_(0.0)
61  , velocity_rolling_window_size_(velocity_rolling_window_size)
62  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
63  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
64  , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
65  {
66  }
67 
68  void Odometry::init(const ros::Time& time)
69  {
70  // Reset accumulators and timestamp:
72  timestamp_ = time;
73  }
74 
75  bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
76  {
78  const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_;
79 
81  //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
82  //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
83 
84  const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_;
85 
87  rear_wheel_old_pos_ = rear_wheel_cur_pos;
88 
90  const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5;
91  //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
92  const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
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_h, double wheel_radius)
127  {
128  wheel_separation_h_ = wheel_separation_h;
129  wheel_radius_ = wheel_radius;
130  }
131 
132  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
133  {
134  velocity_rolling_window_size_ = velocity_rolling_window_size;
135 
137  }
138 
139  void Odometry::integrateRungeKutta2(double linear, double angular)
140  {
141  const double direction = heading_ + angular * 0.5;
142 
144  x_ += linear * cos(direction);
145  y_ += linear * sin(direction);
146  heading_ += angular;
147  }
148 
154  void Odometry::integrateExact(double linear, double angular)
155  {
156  if (fabs(angular) < 1e-6)
157  integrateRungeKutta2(linear, angular);
158  else
159  {
161  const double heading_old = heading_;
162  const double r = linear/angular;
163  heading_ += angular;
164  x_ += r * (sin(heading_) - sin(heading_old));
165  y_ += -r * (cos(heading_) - cos(heading_old));
166  }
167  }
168 
170  {
171  linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
172  angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
173  }
174 
175 } // namespace diff_drive_controller
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
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
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
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:154
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:114
ros::Time timestamp_
Current timestamp:
Definition: odometry.h:182
double wheel_separation_h_
Wheel kinematic parameters [m]:
Definition: odometry.h:194
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