00001
00002
00003 """
00004 diff_tf.py - follows the output of a wheel encoder and
00005 creates tf and odometry messages.
00006 some code borrowed from the arbotix diff_controller script
00007 A good reference: http://rossum.sourceforge.net/papers/DiffSteer/
00008
00009 Copyright (C) 2012 Jon Stephan.
00010
00011 This program is free software: you can redistribute it and/or modify
00012 it under the terms of the GNU General Public License as published by
00013 the Free Software Foundation, either version 3 of the License, or
00014 (at your option) any later version.
00015
00016 This program is distributed in the hope that it will be useful,
00017 but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00019 GNU General Public License for more details.
00020
00021 You should have received a copy of the GNU General Public License
00022 along with this program. If not, see <http://www.gnu.org/licenses/>.
00023
00024 ----------------------------------
00025 Portions of this code borrowed from the arbotix_python diff_controller.
00026
00027 diff_controller.py - controller for a differential drive
00028 Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
00029
00030 Redistribution and use in source and binary forms, with or without
00031 modification, are permitted provided that the following conditions are met:
00032 * Redistributions of source code must retain the above copyright
00033 notice, this list of conditions and the following disclaimer.
00034 * Redistributions in binary form must reproduce the above copyright
00035 notice, this list of conditions and the following disclaimer in the
00036 documentation and/or other materials provided with the distribution.
00037 * Neither the name of Vanadium Labs LLC nor the names of its
00038 contributors may be used to endorse or promote products derived
00039 from this software without specific prior written permission.
00040
00041 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00042 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00043 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00044 DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00045 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00046 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00047 OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00048 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00049 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00050 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00051
00052 """
00053
00054 import rospy
00055 import roslib
00056 roslib.load_manifest('differential_drive')
00057 from math import sin, cos, pi
00058
00059 from geometry_msgs.msg import Quaternion
00060 from geometry_msgs.msg import Twist
00061 from nav_msgs.msg import Odometry
00062 from tf.broadcaster import TransformBroadcaster
00063 from std_msgs.msg import Int16
00064
00065
00066 class DiffTf:
00067
00068
00069
00070 def __init__(self):
00071
00072 rospy.init_node("diff_tf")
00073 self.nodename = rospy.get_name()
00074 rospy.loginfo("-I- %s started" % self.nodename)
00075
00076
00077 self.rate = rospy.get_param('~rate',10.0)
00078 self.ticks_meter = float(rospy.get_param('ticks_meter', 50))
00079 self.base_width = float(rospy.get_param('~base_width', 0.245))
00080
00081 self.base_frame_id = rospy.get_param('~base_frame_id','base_link')
00082 self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom')
00083
00084 self.encoder_min = rospy.get_param('encoder_min', -32768)
00085 self.encoder_max = rospy.get_param('encoder_max', 32768)
00086 self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min )
00087 self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min )
00088
00089 self.t_delta = rospy.Duration(1.0/self.rate)
00090 self.t_next = rospy.Time.now() + self.t_delta
00091
00092
00093 self.enc_left = None
00094 self.enc_right = None
00095 self.left = 0
00096 self.right = 0
00097 self.lmult = 0
00098 self.rmult = 0
00099 self.prev_lencoder = 0
00100 self.prev_rencoder = 0
00101 self.x = 0
00102 self.y = 0
00103 self.th = 0
00104 self.dx = 0
00105 self.dr = 0
00106 self.then = rospy.Time.now()
00107
00108
00109 rospy.Subscriber("lwheel", Int16, self.lwheelCallback)
00110 rospy.Subscriber("rwheel", Int16, self.rwheelCallback)
00111 self.odomPub = rospy.Publisher("odom", Odometry)
00112 self.odomBroadcaster = TransformBroadcaster()
00113
00114
00115 def spin(self):
00116
00117 r = rospy.Rate(self.rate)
00118 while not rospy.is_shutdown():
00119 self.update()
00120 r.sleep()
00121
00122
00123
00124 def update(self):
00125
00126 now = rospy.Time.now()
00127 if now > self.t_next:
00128 elapsed = now - self.then
00129 self.then = now
00130 elapsed = elapsed.to_sec()
00131
00132
00133 if self.enc_left == None:
00134 d_left = 0
00135 d_right = 0
00136 else:
00137 d_left = (self.left - self.enc_left) / self.ticks_meter
00138 d_right = (self.right - self.enc_right) / self.ticks_meter
00139 self.enc_left = self.left
00140 self.enc_right = self.right
00141
00142
00143 d = ( d_left + d_right ) / 2
00144
00145 th = ( d_right - d_left ) / self.base_width
00146
00147 self.dx = d / elapsed
00148 self.dr = th / elapsed
00149
00150
00151 if (d != 0):
00152
00153 x = cos( th ) * d
00154 y = -sin( th ) * d
00155
00156 self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
00157 self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
00158 if( th != 0):
00159 self.th = self.th + th
00160
00161
00162 quaternion = Quaternion()
00163 quaternion.x = 0.0
00164 quaternion.y = 0.0
00165 quaternion.z = sin( self.th / 2 )
00166 quaternion.w = cos( self.th / 2 )
00167 self.odomBroadcaster.sendTransform(
00168 (self.x, self.y, 0),
00169 (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00170 rospy.Time.now(),
00171 self.base_frame_id,
00172 self.odom_frame_id
00173 )
00174
00175 odom = Odometry()
00176 odom.header.stamp = now
00177 odom.header.frame_id = self.odom_frame_id
00178 odom.pose.pose.position.x = self.x
00179 odom.pose.pose.position.y = self.y
00180 odom.pose.pose.position.z = 0
00181 odom.pose.pose.orientation = quaternion
00182 odom.child_frame_id = self.base_frame_id
00183 odom.twist.twist.linear.x = self.dx
00184 odom.twist.twist.linear.y = 0
00185 odom.twist.twist.angular.z = self.dr
00186 self.odomPub.publish(odom)
00187
00188
00189
00190
00191
00192 def lwheelCallback(self, msg):
00193
00194 enc = msg.data
00195 if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap):
00196 self.lmult = self.lmult + 1
00197
00198 if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap):
00199 self.lmult = self.lmult - 1
00200
00201 self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min))
00202 self.prev_lencoder = enc
00203
00204
00205 def rwheelCallback(self, msg):
00206
00207 enc = msg.data
00208 if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap):
00209 self.rmult = self.rmult + 1
00210
00211 if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap):
00212 self.rmult = self.rmult - 1
00213
00214 self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min))
00215 self.prev_rencoder = enc
00216
00217
00218
00219 if __name__ == '__main__':
00220 """ main """
00221 diffTf = DiffTf()
00222 diffTf.spin()
00223
00224
00225