calibrate_base.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Fetch Robotics Inc.
3  * Copyright (C) 2014 Unbounded Robotics Inc.
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 
18 // Author: Michael Ferguson
19 
20 #include <cmath>
21 #include <fstream>
22 #include <boost/thread/recursive_mutex.hpp>
23 
24 #include <ros/ros.h>
25 #include <geometry_msgs/Twist.h>
26 #include <sensor_msgs/Imu.h>
27 #include <sensor_msgs/LaserScan.h>
28 #include <nav_msgs/Odometry.h>
29 
30 #define PI 3.14159265359
31 
33 {
34 public:
36  {
37  // Setup times
39 
40  // Get params
41  ros::NodeHandle nh("~");
42 
43  // Min/Max acceptable error when aligning with wall
44  nh.param<double>("min_angle", min_angle_, -0.5);
45  nh.param<double>("max_angle", max_angle_, 0.5);
46 
47  // How fast to accelerate
48  nh.param<double>("accel_limit", accel_limit_, 2.0);
49 
50  // cmd vel publisher
51  cmd_pub_ = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
52 
53  // Subscribe
56  scan_subscriber_ = n.subscribe("base_scan", 1, &BaseCalibration::laserCallback, this);
57 
58  resetInternal();
59  }
60 
61  void reset()
62  {
63  scan_.clear();
64  odom_.clear();
65  imu_.clear();
66  }
67 
68  std::string print()
69  {
70  std::stringstream ss;
71  ss << scan_r2_ << " " << imu_angle_ << " " << odom_angle_ << " " << scan_angle_;
72  return ss.str();
73  }
74 
75  std::string printCalibrationData()
76  {
77  double odom, imu;
78  ros::NodeHandle nh;
79  nh.param<double>("base_controller/track_width", odom, 0.37476);
80  nh.param<double>("imu/gyro/scale", imu, 0.001221729);
81 
82  // scaling to be computed
83  double odom_scale = 0.0;
84  double imu_scale = 0.0;
85 
86  // get sum
87  for (size_t i = 0; i < scan_.size(); ++i)
88  {
89  odom_scale += (scan_[i] - odom_[i]) / odom_[i];
90  imu_scale += (scan_[i] - imu_[i]) / imu_[i];
91  }
92  // divide sum by size
93  odom_scale /= scan_.size();
94  imu_scale /= scan_.size();
95  // output odom/imu values
96  std::stringstream ss;
97  ss << "odom: " << odom * (1.0 + odom_scale) << std::endl;
98  ss << "imu: " << imu * (1.0 + imu_scale) << std::endl;
99  return ss.str();
100  }
101 
103  bool align(bool verbose = false)
104  {
105  while (!ready_)
106  ros::Duration(0.1).sleep();
107 
108  std::cout << "aligning..." << std::endl;
109 
110  double velocity = 0.2;
111  if (scan_angle_ < 0)
112  velocity = -0.2;
113 
114  while (fabs(scan_angle_) > 0.2 || (scan_r2_ < 0.1))
115  {
116  if (verbose)
117  std::cout << scan_r2_ << " " << scan_angle_ << std::endl;
118  sendVelocityCommand(velocity);
119  ros::Duration(0.02).sleep();
120  }
121  sendVelocityCommand(0.0);
122  std::cout << "...done" << std::endl;
123  ros::Duration(0.25).sleep();
124 
125  return true;
126  }
127 
129  bool spin(double velocity, int rotations, bool verbose = false)
130  {
131  double scan_start = scan_angle_;
132 
133  align();
134  resetInternal();
135  std::cout << "spin..." << std::endl;
136 
137  // need to account for de-acceleration time (v^2/2a)
138  double angle = rotations * 2 * PI - (0.5 * velocity * velocity / accel_limit_);
139 
140  while (fabs(odom_angle_) < angle)
141  {
142  if (verbose)
143  std::cout << scan_angle_ << " " << odom_angle_ << " " << imu_angle_ << std::endl;
144  sendVelocityCommand(velocity);
145  ros::Duration(0.02).sleep();
146  }
147  sendVelocityCommand(0.0);
148  std::cout << "...done" << std::endl;
149 
150  // wait to stop
151  ros::Duration(0.5 + fabs(velocity) / accel_limit_).sleep();
152 
153  // save measurements
154  imu_.push_back(imu_angle_);
155  odom_.push_back(odom_angle_);
156  if (velocity > 0)
157  scan_.push_back(scan_start + 2 * rotations * PI - scan_angle_);
158  else
159  scan_.push_back(scan_start - 2 * rotations * PI - scan_angle_);
160 
161  return true;
162  }
163 
164 private:
165  void odometryCallback(const nav_msgs::Odometry::Ptr& odom)
166  {
167  boost::recursive_mutex::scoped_lock lock(data_mutex_);
168 
169  double dt = (odom->header.stamp - last_odom_stamp_).toSec();
170  odom_angle_ += odom->twist.twist.angular.z * dt;
171 
172  last_odom_stamp_ = odom->header.stamp;
173  }
174 
175  void imuCallback(const sensor_msgs::Imu::Ptr& imu)
176  {
177  boost::recursive_mutex::scoped_lock lock(data_mutex_);
178 
179  double dt = (imu->header.stamp - last_imu_stamp_).toSec();
180  imu_angle_ += imu->angular_velocity.z * dt;
181 
182  last_imu_stamp_ = imu->header.stamp;
183  }
184 
185  void laserCallback(const sensor_msgs::LaserScan::Ptr& scan)
186  {
187  boost::recursive_mutex::scoped_lock lock(data_mutex_);
188 
189  double angle = scan->angle_min;
190  double mean_x, mean_y, n;
191  mean_x = mean_y = n = 0;
192  int start = -1;
193  for (size_t i = 0; i < scan->ranges.size(); ++i, angle += scan->angle_increment)
194  {
195  if (angle < min_angle_ || angle > max_angle_)
196  continue;
197  if (start < 0)
198  start = i;
199 
200  // Compute point
201  double px = sin(angle) * scan->ranges[i];
202  double py = cos(angle) * scan->ranges[i];
203 
204  mean_x += px;
205  mean_y += py;
206  ++n;
207  }
208 
209  if (n == 0)
210  return;
211 
212  mean_x /= n;
213  mean_y /= n;
214 
215  angle = scan->angle_min + start * scan->angle_increment; // reset angle
216  double x, y, xx, xy, yy;
217  x = y = xx = xy = yy = n = 0;
218  for (size_t i = start; i < scan->ranges.size(); ++i, angle += scan->angle_increment)
219  {
220  if (angle > max_angle_)
221  break;
222 
223  // Compute point
224  double px = sin(angle) * scan->ranges[i] - mean_x;
225  double py = cos(angle) * scan->ranges[i] - mean_y;
226 
227  // Sums for simple linear regression
228  xx += px * px;
229  xy += px * py;
230  x += px;
231  y += py;
232  yy += py * py;
233  ++n;
234  }
235 
236  scan_dist_ = mean_y;
237  scan_angle_ = atan2((n*xy-x*y)/(n*xx-x*x), 1.0);
238  scan_r2_ = fabs(xy)/(xx * yy);
239  last_scan_stamp_ = scan->header.stamp;
240  ready_ = true;
241  }
242 
243  // Send a rotational velocity command
244  void sendVelocityCommand(double vel)
245  {
246  geometry_msgs::Twist twist;
247  twist.angular.z = vel;
248  cmd_pub_.publish(twist);
249  }
250 
251  // reset the odom/imu counters
253  {
254  boost::recursive_mutex::scoped_lock lock(data_mutex_);
256  }
257 
259 
263 
265  double odom_angle_;
266 
268  double imu_angle_;
269 
272 
274  double accel_limit_;
275 
276  std::vector<double> scan_;
277  std::vector<double> imu_;
278  std::vector<double> odom_;
279 
280  boost::recursive_mutex data_mutex_;
281  bool ready_;
282 };
283 
284 int main(int argc, char** argv)
285 {
286  ros::init(argc, argv,"base_calibration_node");
287  ros::NodeHandle nh;
288 
289  // for callbacks
291  spinner.start();
292 
293  BaseCalibration b(nh);
294  b.reset();
295 
296  // rotate at several different speeds
297  b.spin(0.5, 1);
298  b.spin(1.5, 1);
299  b.spin(3.0, 2);
300  b.spin(-0.5, 1);
301  b.spin(-1.5, 1);
302  b.spin(-3.0, 2);
303 
304  // TODO: drive towards wall, to calibrate rollout
305 
306  // output yaml file
307  {
308  // Generate datecode
309  char datecode[80];
310  {
311  std::time_t t = std::time(NULL);
312  std::strftime(datecode, 80, "%Y_%m_%d_%H_%M_%S", std::localtime(&t));
313  }
314  std::stringstream yaml_name;
315  yaml_name << "/tmp/base_calibration_" << datecode << ".yaml";
316  std::ofstream file;
317  file.open(yaml_name.str().c_str());
318  std::string cal = b.printCalibrationData();
319  file << cal;
320  file.close();
321  std::cout << cal;
322  }
323 
324  spinner.stop();
325  return 0;
326 }
ros::Publisher cmd_pub_
ros::Subscriber scan_subscriber_
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define PI
ros::Time last_scan_stamp_
bool align(bool verbose=false)
Align to the wall.
TFSIMD_FORCE_INLINE const tfScalar & y() const
geometry_msgs::TransformStamped t
ros::Time last_odom_stamp_
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void spinner()
std::vector< double > imu_
std::string printCalibrationData()
void odometryCallback(const nav_msgs::Odometry::Ptr &odom)
int main(int argc, char **argv)
std::string print()
bool param(const std::string &param_name, T &param_val, const T &default_val) const
BaseCalibration(ros::NodeHandle &n)
boost::recursive_mutex data_mutex_
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::Subscriber imu_subscriber_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< double > odom_
void imuCallback(const sensor_msgs::Imu::Ptr &imu)
std::vector< double > scan_
bool spin(double velocity, int rotations, bool verbose=false)
Spin and record imu, odom, scan.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
static Time now()
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
ros::Time last_imu_stamp_
void laserCallback(const sensor_msgs::LaserScan::Ptr &scan)
ros::Subscriber odom_subscriber_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
void sendVelocityCommand(double vel)


robot_calibration
Author(s): Michael Ferguson
autogenerated on Tue Nov 3 2020 17:30:30