Skip to content

Commit dc38d7f

Browse files
author
Enrique Fernandez
committed
Create linear/quadratic meas cov model classes
Both model also support a wheel resolution param, which is 0.0 by default.
1 parent 290e8f9 commit dc38d7f

12 files changed

+574
-59
lines changed

‎diff_drive_controller/CMakeLists.txt‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,12 +49,16 @@ include_directories(
4949
include ${catkin_INCLUDE_DIRS}
5050
include ${Boost_INCLUDE_DIRS}
5151
include ${EIGEN_INCLUDE_DIRS}
52-
include ${sophus_INCLUDE_DIRS}
5352
include ${CERES_INCLUDE_DIRS})
5453

5554
roslint_cpp()
5655

57-
add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
56+
add_library(${PROJECT_NAME}
57+
src/diff_drive_controller.cpp
58+
src/linear_meas_covariance_model.cpp
59+
src/quadratic_meas_covariance_model.cpp
60+
src/odometry.cpp
61+
src/speed_limiter.cpp)
5862
# Note that the entry for ${Ceres_LIBRARIES} was removed as we only used headers from that package
5963
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
6064
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)

‎diff_drive_controller/cfg/DiffDriveController.cfg‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@ gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multi
1616
gen.add("k_l", double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0)
1717
gen.add("k_r", double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)
1818

19+
gen.add("wheel_resolution", double_t, 0, "Wheel position resolution [rad] (used in the Covariance model).", 0.0, 0.0, 10.0)
20+
1921
gen.add("publish_cmd_vel_limited", bool_t, 0, "Publish limited velocity command.", False)
2022
gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)
2123

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -205,6 +205,8 @@ namespace diff_drive_controller
205205
double k_l;
206206
double k_r;
207207

208+
double wheel_resolution;
209+
208210
bool publish_state;
209211
bool publish_cmd_vel_limited;
210212

@@ -216,8 +218,9 @@ namespace diff_drive_controller
216218
, wheel_separation_multiplier(1.0)
217219
, left_wheel_radius_multiplier(1.0)
218220
, right_wheel_radius_multiplier(1.0)
219-
, k_l(1.0)
220-
, k_r(1.0)
221+
, k_l(0.1)
222+
, k_r(0.1)
223+
, wheel_resolution(0.0001)
221224
, publish_state(false)
222225
, publish_cmd_vel_limited(false)
223226
, control_frequency_desired(0.0)
@@ -241,6 +244,8 @@ namespace diff_drive_controller
241244
double k_l_;
242245
double k_r_;
243246

247+
double wheel_resolution_; // [rad]
248+
244249
/// Timeout to consider cmd_vel commands old:
245250
double cmd_vel_timeout_;
246251

‎diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h‎

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2016, Clearpath Robotics, Inc.
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: Enrique Fernández
37+
*/
38+
39+
#ifndef LINEAR_MEAS_COVARIANCE_MODEL_H_
40+
#define LINEAR_MEAS_COVARIANCE_MODEL_H_
41+
42+
#include <diff_drive_controller/meas_covariance_model.h>
43+
44+
namespace diff_drive_controller
45+
{
46+
47+
/**
48+
* \brief Linear Meas(urement) Covariance Model
49+
*
50+
* The odometry covariance is computed according with the model presented in:
51+
*
52+
* [Siegwart, 2004]:
53+
* Roland Siegwart, Illah R. Nourbakhsh
54+
* Introduction to Autonomous Mobile Robots
55+
* 1st Edition, 2004
56+
*
57+
* Section:
58+
* 5.2.4 'An error model for odometric position estimation' (pp. 186-191)
59+
*
60+
* Although the twist covariance doesn't appear explicitly, the implementation
61+
* here is based on the same covariance model used for the pose covariance.
62+
*
63+
* The model also includes the wheel resolution, as a constant additive
64+
* diagonal covariance, that can be easily disabled by using a zero
65+
* (ideal/perfect) wheel resolution.
66+
*/
67+
class LinearMeasCovarianceModel : public MeasCovarianceModel
68+
{
69+
public:
70+
/**
71+
* \brief Constructor
72+
* \param[in] wheel_resolution Wheel resolution [rad]
73+
*/
74+
LinearMeasCovarianceModel(const double wheel_resolution = 0.0);
75+
76+
/**
77+
* \brief Destructor
78+
*/
79+
virtual ~LinearMeasCovarianceModel()
80+
{}
81+
82+
/**
83+
* \brief Integrates w/o computing the jacobians
84+
* \param [in] dp_l Left wheel position increment [rad]
85+
* \param [in] dp_r Right wheel position increment [rad]
86+
* \return Meas(urement) covariance
87+
*/
88+
const MeasCovariance& compute(const double dp_l, const double dp_r);
89+
};
90+
91+
} // namespace diff_drive_controller
92+
93+
#endif /* LINEAR_MEAS_COVARIANCE_MODEL_H_ */

‎diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2016, Clearpath Robotics, Inc.
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: Enrique Fernández
37+
*/
38+
39+
#ifndef MEAS_COVARIANCE_MODEL_H_
40+
#define MEAS_COVARIANCE_MODEL_H_
41+
42+
#include <Eigen/Core>
43+
44+
namespace diff_drive_controller
45+
{
46+
47+
/**
48+
* \brief Meas(urement) Covariance Model
49+
*/
50+
class MeasCovarianceModel
51+
{
52+
public:
53+
/// Meas(urement) covariance type:
54+
typedef Eigen::Matrix2d MeasCovariance;
55+
56+
/**
57+
* \brief Constructor
58+
* \param[in] wheel_resolution Wheel resolution [rad]
59+
*/
60+
MeasCovarianceModel(const double wheel_resolution = 0.0)
61+
: wheel_resolution_(wheel_resolution)
62+
{}
63+
64+
virtual ~MeasCovarianceModel()
65+
{}
66+
67+
/**
68+
* \brief Integrates w/o computing the jacobians
69+
* \param [in] dp_l Left wheel position increment [rad]
70+
* \param [in] dp_r Right wheel position increment [rad]
71+
* \return Meas(urement) covariance
72+
*/
73+
virtual const MeasCovariance& compute(const double dp_l, const double dp_r) = 0;
74+
75+
/**
76+
* \brief Left wheel covariance gain getter
77+
* \return Left wheel covariance gain
78+
*/
79+
double getKl() const
80+
{
81+
return k_l_;
82+
}
83+
84+
/**
85+
* \brief Right wheel covariance gain getter
86+
* \return Right wheel covariance gain
87+
*/
88+
double getKr() const
89+
{
90+
return k_r_;
91+
}
92+
93+
/**
94+
* \brief Wheel resolution getter
95+
* \return Wheel resolution [rad]
96+
*/
97+
double getWheelResolution() const
98+
{
99+
return wheel_resolution_;
100+
}
101+
102+
/**
103+
* \brief Left wheel covariance gain setter
104+
* \param[in] k_l Left wheel covariance gain
105+
*/
106+
void setKl(const double k_l)
107+
{
108+
k_l_ = k_l;
109+
}
110+
111+
/**
112+
* \brief Right wheel covariance gain setter
113+
* \param[in] k_r Right wheel covariance gain
114+
*/
115+
void setKr(const double k_r)
116+
{
117+
k_r_ = k_r;
118+
}
119+
120+
/**
121+
* \brief Wheel resolution setter
122+
* \param[in] wheel_resolution Wheel resolution [rad]
123+
*/
124+
void setWheelResolution(const double wheel_resolution)
125+
{
126+
wheel_resolution_ = wheel_resolution;
127+
}
128+
129+
protected:
130+
/// Meas(urement) covariance:
131+
MeasCovariance meas_covariance_;
132+
133+
/// Meas(urement) Covariance Model gains:
134+
double k_l_;
135+
double k_r_;
136+
137+
/// Wheel resolution [rad]:
138+
double wheel_resolution_;
139+
};
140+
141+
} // namespace diff_drive_controller
142+
143+
#endif /* MEAS_COVARIANCE_MODEL_H_ */

‎diff_drive_controller/include/diff_drive_controller/odometry.h‎

Lines changed: 9 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,8 @@
5050
#include <boost/accumulators/statistics/rolling_mean.hpp>
5151
#include <boost/function.hpp>
5252

53+
#include <diff_drive_controller/meas_covariance_model.h>
54+
5355
#include <diff_drive_controller/integrate_function.h>
5456

5557
namespace diff_drive_controller
@@ -59,19 +61,6 @@ namespace diff_drive_controller
5961
/**
6062
* \brief The Odometry class handles odometry readings
6163
* (2D pose and velocity with related timestamp)
62-
*
63-
* The odometry covariance is computed according with the model presented in:
64-
*
65-
* [Siegwart, 2004]:
66-
* Roland Siegwart, Illah R. Nourbakhsh
67-
* Introduction to Autonomous Mobile Robots
68-
* 1st Edition, 2004
69-
*
70-
* Section:
71-
* 5.2.4 'An error model for odometric position estimation' (pp. 186-191)
72-
*
73-
* Although the twist covariance doesn't appear explicitly, the implementation
74-
* here is based on the same covariance model used for the pose covariance.
7564
*/
7665
class Odometry
7766
{
@@ -85,7 +74,7 @@ namespace diff_drive_controller
8574
typedef Covariance PoseCovariance;
8675
typedef Covariance TwistCovariance;
8776

88-
typedef Eigen::Matrix2d MeasCovariance;
77+
typedef MeasCovarianceModel::MeasCovariance MeasCovariance;
8978

9079
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
9180

@@ -259,8 +248,11 @@ namespace diff_drive_controller
259248
* \brief Sets the Measurement Covariance Model parameters: k_l and k_r
260249
* \param[in] k_l Left wheel velocity multiplier
261250
* \param[in] k_r Right wheel velocity multiplier
251+
* \param[in] wheel_resolution Wheel resolution [rad] (assumed the same for
252+
* both wheels
262253
*/
263-
void setMeasCovarianceParams(const double k_l, const double k_r);
254+
void setMeasCovarianceParams(const double k_l, const double k_r,
255+
const double wheel_resolution);
264256

265257
/**
266258
* \brief Velocity rolling window size setter
@@ -295,13 +287,6 @@ namespace diff_drive_controller
295287
*/
296288
void updateIncrementalPose(const double dp_l, const double dp_r);
297289

298-
/**
299-
* \brief Update the measurement covariance
300-
* \param[in] dp_l Left wheel position increment [rad]
301-
* \param[in] dp_r Right wheel position increment [rad]
302-
*/
303-
void updateMeasCovariance(const double dp_l, const double dp_r);
304-
305290
/**
306291
* \brief Reset linear and angular accumulators
307292
*/
@@ -339,18 +324,14 @@ namespace diff_drive_controller
339324
TwistCovariance twist_covariance_;
340325
TwistCovariance minimum_twist_covariance_;
341326

342-
/// Measurement covariance:
343-
MeasCovariance meas_covariance_;
327+
/// Meas(urement) Covariance Model:
328+
boost::shared_ptr<MeasCovarianceModel> meas_covariance_model_;
344329

345330
/// Wheel kinematic parameters [m]:
346331
double wheel_separation_;
347332
double left_wheel_radius_;
348333
double right_wheel_radius_;
349334

350-
/// Measurement Covariance Model parameters:
351-
double k_l_;
352-
double k_r_;
353-
354335
/// Previous wheel position/state [rad]:
355336
double left_position_previous_;
356337
double right_position_previous_;

0 commit comments

Comments
 (0)