neato.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # ROS node for the Neato Robot Vacuum
00004 # Copyright (c) 2010 University at Albany. All right reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the University at Albany nor the names of its 
00014 #       contributors may be used to endorse or promote products derived 
00015 #       from this software without specific prior written permission.
00016 # 
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 
00028 """
00029 ROS node for Neato XV-11 Robot Vacuum.
00030 """
00031 
00032 __author__ = "ferguson@cs.albany.edu (Michael Ferguson)"
00033 
00034 import roslib; roslib.load_manifest("neato_node")
00035 import rospy
00036 from math import sin,cos
00037 
00038 from sensor_msgs.msg import LaserScan
00039 from geometry_msgs.msg import Quaternion
00040 from geometry_msgs.msg import Twist
00041 from nav_msgs.msg import Odometry
00042 from tf.broadcaster import TransformBroadcaster
00043 
00044 from neato_driver.neato_driver import xv11, BASE_WIDTH, MAX_SPEED
00045 
00046 class NeatoNode:
00047 
00048     def __init__(self):
00049         """ Start up connection to the Neato Robot. """
00050         rospy.init_node('neato')
00051 
00052         self.port = rospy.get_param('~port', "/dev/ttyUSB0")
00053         rospy.loginfo("Using port: %s"%(self.port))
00054 
00055         self.robot = xv11(self.port)
00056 
00057         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
00058         self.scanPub = rospy.Publisher('base_scan', LaserScan)
00059         self.odomPub = rospy.Publisher('odom',Odometry)
00060         self.odomBroadcaster = TransformBroadcaster()
00061 
00062         self.cmd_vel = [0,0] 
00063 
00064     def spin(self):        
00065         encoders = [0,0]
00066 
00067         self.x = 0                  # position in xy plane
00068         self.y = 0
00069         self.th = 0
00070         then = rospy.Time.now()
00071 
00072         # things that don't ever change
00073         scan_link = rospy.get_param('~frame_id','base_laser_link')
00074         scan = LaserScan(header=rospy.Header(frame_id=scan_link)) 
00075         scan.angle_min = 0
00076         scan.angle_max = 6.26
00077         scan.angle_increment = 0.017437326
00078         scan.range_min = 0.020
00079         scan.range_max = 5.0
00080         odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link')
00081     
00082         # main loop of driver
00083         r = rospy.Rate(5)
00084         self.robot.requestScan()
00085         while not rospy.is_shutdown():
00086             # prepare laser scan
00087             scan.header.stamp = rospy.Time.now()    
00088             #self.robot.requestScan()
00089             scan.ranges = self.robot.getScanRanges()
00090 
00091             # get motor encoder values
00092             left, right = self.robot.getMotors()
00093 
00094             # send updated movement commands
00095             self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
00096             
00097             # ask for the next scan while we finish processing stuff
00098             self.robot.requestScan()
00099             
00100             # now update position information
00101             dt = (scan.header.stamp - then).to_sec()
00102             then = scan.header.stamp
00103 
00104             d_left = (left - encoders[0])/1000.0
00105             d_right = (right - encoders[1])/1000.0
00106             encoders = [left, right]
00107             
00108             dx = (d_left+d_right)/2
00109             dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
00110 
00111             x = cos(dth)*dx
00112             y = -sin(dth)*dx
00113             self.x += cos(self.th)*x - sin(self.th)*y
00114             self.y += sin(self.th)*x + cos(self.th)*y
00115             self.th += dth
00116 
00117             # prepare tf from base_link to odom
00118             quaternion = Quaternion()
00119             quaternion.z = sin(self.th/2.0)
00120             quaternion.w = cos(self.th/2.0)
00121 
00122             # prepare odometry
00123             odom.header.stamp = rospy.Time.now()
00124             odom.pose.pose.position.x = self.x
00125             odom.pose.pose.position.y = self.y
00126             odom.pose.pose.position.z = 0
00127             odom.pose.pose.orientation = quaternion
00128             odom.twist.twist.linear.x = dx/dt
00129             odom.twist.twist.angular.z = dth/dt
00130 
00131             # publish everything
00132             self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
00133                 then, "base_link", "odom" )
00134             self.scanPub.publish(scan)
00135             self.odomPub.publish(odom)
00136 
00137             # wait, then do it again
00138             r.sleep()
00139 
00140         # shut down
00141         self.robot.setLDS("off")
00142         self.robot.setTestMode("off") 
00143 
00144     def cmdVelCb(self,req):
00145         x = req.linear.x * 1000
00146         th = req.angular.z * (BASE_WIDTH/2) 
00147         k = max(abs(x-th),abs(x+th))
00148         # sending commands higher than max speed will fail
00149         if k > MAX_SPEED:
00150             x = x*MAX_SPEED/k; th = th*MAX_SPEED/k
00151         self.cmd_vel = [ int(x-th) , int(x+th) ]
00152 
00153 if __name__ == "__main__":    
00154     robot = NeatoNode()
00155     robot.spin()
00156 


neato_node
Author(s): Michael Ferguson
autogenerated on Mon Oct 6 2014 02:47:37