imu_bias_remover.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Chad Rockey
32  */
33 
34 #include <ros/ros.h>
35 
36 #include <sensor_msgs/Imu.h>
37 #include <geometry_msgs/Twist.h>
38 #include <nav_msgs/Odometry.h>
39 #include <geometry_msgs/Vector3Stamped.h>
40 
43 
45 bool use_odom_;
46 
49 
52 
53 // Implement an exponentially weighted moving average to calculate bias
55 geometry_msgs::Vector3 angular_velocity_accumulator;
56 
57 // Returns true if |val1| < val2
58 bool abslt(const double& val1, const double& val2){
59  return std::abs(val1) < val2;
60 }
61 
62 double accumulator_update(const double& alpha, const double& avg, const double& meas){
63  return alpha * meas + (1.0 - alpha) * avg;
64 }
65 
66 void cmd_vel_callback(const geometry_msgs::TwistConstPtr& msg){
67  if(use_cmd_vel_){
68  if(abslt(msg->linear.x, cmd_vel_threshold_) &&
69  abslt(msg->linear.y, cmd_vel_threshold_) &&
70  abslt(msg->linear.z, cmd_vel_threshold_) &&
71  abslt(msg->angular.x, cmd_vel_threshold_) &&
72  abslt(msg->angular.y, cmd_vel_threshold_) &&
73  abslt(msg->angular.z, cmd_vel_threshold_)){
74  twist_is_zero_ = true;
75  return;
76  }
77  }
78  twist_is_zero_ = false;
79 }
80 
81 void odom_callback(const nav_msgs::OdometryConstPtr& msg){
82  if(use_odom_){
83  if(abslt(msg->twist.twist.linear.x, odom_threshold_) &&
84  abslt(msg->twist.twist.linear.y, odom_threshold_) &&
85  abslt(msg->twist.twist.linear.z, odom_threshold_) &&
86  abslt(msg->twist.twist.angular.x, odom_threshold_) &&
87  abslt(msg->twist.twist.angular.y, odom_threshold_) &&
88  abslt(msg->twist.twist.angular.z, odom_threshold_)){
89  odom_is_zero_ = true;
90  return;
91  }
92  }
93  odom_is_zero_ = false;
94 }
95 
96 void imu_callback(const sensor_msgs::ImuConstPtr& msg){
97  sensor_msgs::ImuPtr imu(new sensor_msgs::Imu(*msg));
98 
99  if(twist_is_zero_ || odom_is_zero_){ // Update bias, set outputs to 0
103  imu->angular_velocity.x = 0.0;
104  imu->angular_velocity.y = 0.0;
105  imu->angular_velocity.z = 0.0;
106  } else { // Modify outputs by bias
107  imu->angular_velocity.x -= angular_velocity_accumulator.x;
108  imu->angular_velocity.y -= angular_velocity_accumulator.y;
109  imu->angular_velocity.z -= angular_velocity_accumulator.z;
110  }
111 
112  // Publish transformed message
113  pub_.publish(imu);
114 
115  // Publish bias information
116  geometry_msgs::Vector3StampedPtr bias(new geometry_msgs::Vector3Stamped());
117  bias->header = imu->header;
118  bias->vector = angular_velocity_accumulator;
119  bias_pub_.publish(bias);
120 }
121 
122 
123 int main(int argc, char **argv){
124  ros::init(argc, argv, "imu_bias_remover");
125  ros::NodeHandle n;
126  ros::NodeHandle pnh("~");
127 
128  // Initialize
129  twist_is_zero_ = false;
130  odom_is_zero_ = false;
134 
135  // Get parameters
136  pnh.param<bool>("use_cmd_vel", use_cmd_vel_, false);
137  pnh.param<bool>("use_odom", use_odom_, false);
138  pnh.param<double>("accumulator_alpha", accumulator_alpha_, 0.01);
139 
140  ros::Subscriber cmd_sub;
141  if(use_cmd_vel_){
142  cmd_sub = n.subscribe("cmd_vel", 10, cmd_vel_callback);
143  }
144  ros::Subscriber odom_sub;
145  if(use_odom_){
146  odom_sub = n.subscribe("odom", 10, odom_callback);
147  }
148 
149  pnh.param<double>("cmd_vel_threshold", cmd_vel_threshold_, 0.001);
150  pnh.param<double>("odom_threshold", odom_threshold_, 0.001);
151 
152  // Create publisher
153  pub_ = n.advertise<sensor_msgs::Imu>("imu_biased", 10);
154  bias_pub_ = n.advertise<geometry_msgs::Vector3Stamped>("bias", 10);
155 
156  // Imu Subscriber
157  ros::Subscriber sub = n.subscribe("imu", 100, imu_callback);
158 
159  ros::spin();
160 
161  return 0;
162 }
void publish(const boost::shared_ptr< M > &message) const
geometry_msgs::Vector3 angular_velocity_accumulator
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double accumulator_alpha_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void imu_callback(const sensor_msgs::ImuConstPtr &msg)
bool twist_is_zero_
bool use_cmd_vel_
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void cmd_vel_callback(const geometry_msgs::TwistConstPtr &msg)
bool use_odom_
ros::Publisher pub_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void odom_callback(const nav_msgs::OdometryConstPtr &msg)
bool abslt(const double &val1, const double &val2)
ros::Publisher bias_pub_
bool odom_is_zero_
double accumulator_update(const double &alpha, const double &avg, const double &meas)
double cmd_vel_threshold_
double odom_threshold_


imu_processors
Author(s): Chad Rockey
autogenerated on Thu Jun 4 2020 03:22:59