test_angle.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2011, Robot Control and Pattern Recognition Group, Warsaw University of Technology
00006 #
00007 # All rights reserved.
00008 # 
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions are met:
00011 #     * Redistributions of source code must retain the above copyright
00012 #       notice, this list of conditions and the following disclaimer.
00013 #     * Redistributions in binary form must reproduce the above copyright
00014 #       notice, this list of conditions and the following disclaimer in the
00015 #       documentation and/or other materials provided with the distribution.
00016 #     * Neither the name of the <organization> nor the
00017 #       names of its contributors may be used to endorse or promote products
00018 #       derived from this software without specific prior written permission.
00019 # 
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00021 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00022 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00023 # DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00024 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00025 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00026 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00027 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00029 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030 
00031 
00032 import roslib
00033 roslib.load_manifest('pcl_to_scan')
00034 
00035 import rospy
00036 from sensor_msgs.msg import Imu
00037 from std_msgs.msg import Float64
00038 
00039 from math import *
00040 
00041 class TestAngle:
00042     def __init__(self):
00043         self.sub = rospy.Subscriber('imu', Imu, self.imu_cb)
00044         
00045         self.pub = rospy.Publisher('tilt_angle', Float64, None, False, True)
00046         
00047         self.sum_p = 0.0
00048         self.sum_r = 0.0
00049         self.cnt = 0
00050         self.measure = False
00051         self.duration = 10
00052         
00053 
00054     def setTilt(self):
00055         print "Set tilt..."
00056         self.pub.publish(0.0)
00057         rospy.sleep(5.0)
00058         if not rospy.is_shutdown():
00059             self.pub.publish(-20.0)
00060 
00061     def doMeasure(self):
00062         print "Do measure..."
00063         self.measure = True
00064         self.start_time = rospy.Time.now()
00065         rospy.sleep(self.duration)
00066         self.measure = False
00067         if not rospy.is_shutdown():
00068             rospy.loginfo("Current Kinect angle: pitch = %f, roll = %f" % (self.sum_p/self.cnt, self.sum_r/self.cnt))
00069             rospy.loginfo("[in degs: pitch = %f, roll = %f]" % (self.sum_p/self.cnt*180/pi, self.sum_r/self.cnt*180/pi))
00070 
00071     def imu_cb(self, msg):
00072         if (self.measure):
00073             x = msg.linear_acceleration.x
00074             y = msg.linear_acceleration.y
00075             z = msg.linear_acceleration.z
00076             pitch = asin(z/9.81)
00077             roll = asin(x/9.81)
00078             self.sum_p += pitch
00079             self.sum_r += roll
00080             self.cnt += 1
00081             #print "P: %10.7f    R: %10.7f  [P: %10.7f    R: %10.7f]" % (pitch, roll, self.sum_p/self.cnt, self.sum_r/self.cnt)
00082 
00083 
00084 def main():
00085     rospy.init_node('test_angle')
00086     tester = TestAngle()
00087     tester.setTilt()
00088     print "Wait..."
00089     rospy.sleep(5.0)
00090     tester.doMeasure()
00091     rospy.spin()
00092 
00093 
00094 if __name__ == '__main__':
00095     main()


pcl_to_scan
Author(s): Maciej Stefanczyk
autogenerated on Sun Oct 5 2014 23:44:14