test_slow_drive.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2012, Yujin Robot
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Yujin Robot nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 # Move following a 90cm radius circle while publishing accumulated and
00036 # average error on angular velocity, using gyroscope data as reference
00037 # Useful for testing passive wheels configurations
00038 # See https://github.com/yujinrobot/kobuki/issues/202 for more details
00039 
00040 
00041 import roslib; roslib.load_manifest('kobuki_testsuite')
00042 import rospy
00043 import sys
00044 
00045 from tf.transformations import euler_from_quaternion
00046 from math import degrees
00047 
00048 from sensor_msgs.msg import Imu
00049 from std_msgs.msg import Float32
00050 from nav_msgs.msg import Odometry
00051 from geometry_msgs.msg import Twist
00052 from geometry_msgs.msg import Quaternion
00053 
00054 
00055 class Test_slow_drive:
00056     # constructor
00057     def __init__(self):
00058         rospy.init_node("test_slow_drive")
00059         
00060         if (len(sys.argv) > 1):
00061             multip = int(sys.argv[1])
00062         else:
00063             multip = 1
00064         self.count = 0
00065         self.int_err = 0.0
00066         self.avg_err = 0.0
00067         self.target_v = 0.09*abs(multip)
00068         self.target_w = 0.1*multip
00069         self.twist = Twist()
00070         self.cmd_vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist)
00071         self.int_err_pub = rospy.Publisher("/int_err", Float32)
00072         self.avg_err_pub = rospy.Publisher("/avg_err", Float32)
00073 
00074         rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, self.ImuCallback)
00075         rospy.Subscriber("/odom", Odometry, self.OdomCallback)
00076       
00077     def OdomCallback(self, data):
00078         self.odom = data.pose.pose.position.x
00079     
00080     def ImuCallback(self, data):
00081         self.count = self.count + 1
00082         if (self.count > 100):
00083             self.real_w = data.angular_velocity.z
00084             self.int_err += abs(self.target_w - self.real_w)
00085             self.avg_err =  self.int_err/(self.count - 100)
00086             
00087             self.int_err_pub.publish(self.int_err)
00088             self.avg_err_pub.publish(self.avg_err)
00089 
00090         self.twist.linear.x = self.target_v
00091         self.twist.angular.z = self.target_w
00092         self.cmd_vel_pub.publish(self.twist)
00093       
00094 
00095 test_slow_drive_obj = Test_slow_drive()
00096 
00097 while not rospy.is_shutdown():
00098     rospy.spin()
00099 print ''
00100 print test_slow_drive_obj.int_err
00101 print test_slow_drive_obj.avg_err
00102 print '' # for clean output


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Wed Sep 16 2015 04:35:48