odometry.cpp
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 
36 
37 #include <boost/bind.hpp>
38 
40 {
41  namespace bacc = boost::accumulators;
42 
43  Odometry::Odometry(size_t velocity_rolling_window_size)
44  : last_update_timestamp_(0.0)
45  , x_(0.0)
46  , y_(0.0)
47  , heading_(0.0)
48  , linear_(0.0)
49  , linear_x_(0.0)
50  , linear_y_(0.0)
51  , angular_(0.0)
52  , steering_track_(0.0)
53  , wheel_steering_y_offset_(0.0)
54  , wheel_radius_(0.0)
55  , wheel_base_(0.0)
56  , wheel_old_pos_(0.0)
57  , velocity_rolling_window_size_(velocity_rolling_window_size)
58  , linear_accel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
59  , linear_jerk_acc_(RollingWindow::window_size = velocity_rolling_window_size)
60  , front_steer_vel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
61  , rear_steer_vel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
62  {
63  }
64 
65  void Odometry::init(const ros::Time& time)
66  {
67  // Reset accumulators and timestamp:
70  }
71 
72  bool Odometry::update(const double &fl_speed, const double &fr_speed,
73  const double &rl_speed, const double &rr_speed,
74  double front_steering, double rear_steering, const ros::Time &time)
75  {
76  const double front_tmp = cos(front_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_;
77  const double front_left_tmp = front_tmp/sqrt(1-steering_track_*front_tmp*cos(front_steering)
78  +pow(steering_track_*front_tmp/2,2));
79  const double front_right_tmp = front_tmp/sqrt(1+steering_track_*front_tmp*cos(front_steering)
80  +pow(steering_track_*front_tmp/2,2));
81  const double fl_speed_tmp = fl_speed * (1/(1-wheel_steering_y_offset_*front_left_tmp));
82  const double fr_speed_tmp = fr_speed * (1/(1-wheel_steering_y_offset_*front_right_tmp));
83  const double front_linear_speed = wheel_radius_ * copysign(1.0, fl_speed_tmp+fr_speed_tmp)*
84  sqrt((pow(fl_speed,2)+pow(fr_speed,2))/(2+pow(steering_track_*front_tmp,2)/2.0));
85 
86  const double rear_tmp = cos(rear_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_;
87  const double rear_left_tmp = rear_tmp/sqrt(1-steering_track_*rear_tmp*cos(rear_steering)
88  +pow(steering_track_*rear_tmp/2,2));
89  const double rear_right_tmp = rear_tmp/sqrt(1+steering_track_*rear_tmp*cos(rear_steering)
90  +pow(steering_track_*rear_tmp/2,2));
91  const double rl_speed_tmp = rl_speed * (1/(1-wheel_steering_y_offset_*rear_left_tmp));
92  const double rr_speed_tmp = rr_speed * (1/(1-wheel_steering_y_offset_*rear_right_tmp));
93  const double rear_linear_speed = wheel_radius_ * copysign(1.0, rl_speed_tmp+rr_speed_tmp)*
94  sqrt((pow(rl_speed_tmp,2)+pow(rr_speed_tmp,2))/(2+pow(steering_track_*rear_tmp,2)/2.0));
95 
96  angular_ = (front_linear_speed*front_tmp + rear_linear_speed*rear_tmp)/2.0;
97 
98  linear_x_ = (front_linear_speed*cos(front_steering) + rear_linear_speed*cos(rear_steering))/2.0;
99  linear_y_ = (front_linear_speed*sin(front_steering) - wheel_base_*angular_/2.0
100  + rear_linear_speed*sin(rear_steering) + wheel_base_*angular_/2.0)/2.0;
101  linear_ = copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2));
102 
104  const double dt = (time - last_update_timestamp_).toSec();
105  if (dt < 0.0001)
106  return false; // Interval too small to integrate with
107 
108  last_update_timestamp_ = time;
111 
114  linear_jerk_acc_((linear_accel_prev_ - bacc::rolling_mean(linear_accel_acc_))/dt);
115  linear_accel_prev_ = bacc::rolling_mean(linear_accel_acc_);
116  front_steer_vel_acc_((front_steer_vel_prev_ - front_steering)/dt);
117  front_steer_vel_prev_ = front_steering;
118  rear_steer_vel_acc_((rear_steer_vel_prev_ - rear_steering)/dt);
119  rear_steer_vel_prev_ = rear_steering;
120  return true;
121  }
122 
123  void Odometry::setWheelParams(double steering_track, double wheel_steering_y_offset, double wheel_radius, double wheel_base)
124  {
125  steering_track_ = steering_track;
126  wheel_steering_y_offset_ = wheel_steering_y_offset;
127  wheel_radius_ = wheel_radius;
128  wheel_base_ = wheel_base;
129  }
130 
131  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
132  {
133  velocity_rolling_window_size_ = velocity_rolling_window_size;
134 
136  }
137 
138  void Odometry::integrateXY(double linear_x, double linear_y, double angular)
139  {
140  const double delta_x = linear_x*cos(heading_) - linear_y*sin(heading_);
141  const double delta_y = linear_x*sin(heading_) + linear_y*cos(heading_);
142 
143  x_ += delta_x;
144  y_ += delta_y;
145  heading_ += angular;
146  }
147 
148  void Odometry::integrateRungeKutta2(double linear, double angular)
149  {
150  const double direction = heading_ + angular * 0.5;
151 
153  x_ += linear * cos(direction);
154  y_ += linear * sin(direction);
155  heading_ += angular;
156  }
157 
158  void Odometry::integrateExact(double linear, double angular)
159  {
160  if (fabs(angular) < 1e-6)
161  integrateRungeKutta2(linear, angular);
162  else
163  {
165  const double heading_old = heading_;
166  const double r = linear/angular;
167  heading_ += angular;
168  x_ += r * (sin(heading_) - sin(heading_old));
169  y_ += -r * (cos(heading_) - cos(heading_old));
170  }
171  }
172 
174  {
175  linear_accel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
176  linear_jerk_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
177  front_steer_vel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
178  rear_steer_vel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
179  }
180 
181 } // namespace four_wheel_steering_controller
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
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
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
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:205
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:148
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:65
double steering_track_
Wheel kinematic parameters [m]:
Definition: odometry.h:248
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