free_space_navigation.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 import numpy
00006 import geometry_msgs.msg
00007 from geometry_msgs.msg import Twist
00008 from math import *
00009 from nav_msgs.msg import Odometry
00010 from std_msgs.msg import String
00011 import tf2_ros
00012 import copy
00013 
00014 LINEAR_VELOCITY_MINIMUM_THRESHOLD  = 0.2
00015 ANGULAR_VELOCITY_MINIMUM_THRESHOLD = 0.4
00016 class free_space_navigation():
00017 
00018     
00019 
00020     def poseCallback(self,pose_message):
00021 
00022         self.turtlebot_odom_pose.pose.pose.position.x=pose_message.pose.pose.position.x
00023         self.turtlebot_odom_pose.pose.pose.position.y=pose_message.pose.pose.position.y
00024         self.turtlebot_odom_pose.pose.pose.position.z=pose_message.pose.pose.position.z
00025 
00026         self.turtlebot_odom_pose.pose.pose.orientation.w=pose_message.pose.pose.orientation.w
00027         self.turtlebot_odom_pose.pose.pose.orientation.x=pose_message.pose.pose.orientation.x
00028         self.turtlebot_odom_pose.pose.pose.orientation.y=pose_message.pose.pose.orientation.y
00029         self.turtlebot_odom_pose.pose.pose.orientation.z=pose_message.pose.pose.orientation.z
00030 
00031  # a function that makes the robot move straight
00032  # @param speed: represents the speed of the robot the robot
00033  # @param distance: represents the distance to move by the robot
00034  # @param isForward: if True, the robot moves forward,otherwise, it moves backward
00035  #
00036  # Method 1: using tf and Calculate the distance between the two transformations
00037 
00038     def move_v1(self, speed, distance, isForward):
00039         #declare a Twist message to send velocity commands
00040         VelocityMessage = Twist()
00041         # declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00042         # the odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00043         listener = tf.TransformListener()
00044 
00045         #set the linear velocity to a positive value if isFoward is True
00046         if (isForward):
00047             VelocityMessage.linear.x =abs(speed)
00048         else: #else set the velocity to negative value to move backward
00049             VelocityMessage.linear.x =-abs(speed)
00050 
00051         # all velocities of other axes must be zero.
00052         VelocityMessage.linear.y =0
00053         VelocityMessage.linear.z =0
00054         #The angular velocity of all axes must be zero because we want  a straight motion
00055         VelocityMessage.angular.x = 0
00056         VelocityMessage.angular.y = 0
00057         VelocityMessage.angular.z =0
00058 
00059         distance_moved = 0.0
00060         loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)    
00061 
00062 
00063      #  First, we capture the initial transformation before starting the motion.
00064      # we call this transformation "init_transform"
00065      # It is important to "waitForTransform" otherwise, it might not be captured.
00066 
00067         try:
00068             #wait for the transform to be found
00069 
00070             listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0),rospy.Duration(10.0))
00071             #Once the transform is found,get the initial_transform transformation.
00072             (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00073             #listener.lookupTransform("/base_footprint", "/odom", rospy.Time(0),init_transform)
00074             start = 0.5 * sqrt(trans[0] ** 2 + trans[1] ** 2)
00075 
00076         except Exception:
00077             rospy.Duration(1.0)
00078 
00079         distance_moved = 0
00080         
00081         while True :
00082             rospy.loginfo("Turtlebot moves forwards") 
00083         #/***************************************
00084         # * STEP1. PUBLISH THE VELOCITY MESSAGE
00085         # ***************************************/
00086             self.velocityPublisher.publish(VelocityMessage)
00087             loop_rate.sleep()
00088         #/**************************************************
00089         # * STEP2. ESTIMATE THE DISTANCE MOVED BY THE ROBOT
00090         # *************************************************/
00091             try:
00092 
00093                 #wait for the transform to be found
00094                 listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0), rospy.Duration(10.0) )
00095                 #Once the transform is found,get the initial_transform transformation.
00096                 #listener.lookupTransform("/base_footprint", "/odom",rospy.Time(0))
00097                 (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00098             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00099                 rospy.Duration(1.0)
00100         # Calculate the distance moved by the robot
00101         # There are two methods that give the same result
00102         #
00103         # Method 1: Calculate the distance between the two transformations
00104         # Hint:
00105         #    --> transform.getOrigin().x(): represents the x coordinate of the transformation
00106         #    --> transform.getOrigin().y(): represents the y coordinate of the transformation
00107         #
00108         # calculate the distance moved
00109             end = 0.5 * sqrt(trans[0] ** 2 + trans[1] ** 2)
00110             distance_moved = distance_moved+abs(abs(float(end)) - abs(float(start)))
00111             if not (distance_moved<distance):
00112                 break
00113             
00114             #finally, stop the robot when the distance is moved
00115         VelocityMessage.linear.x =0 
00116         self.velocityPublisher.publish(VelocityMessage)
00117 
00118     
00119     def move_v2(self, speed, distance, isForward):
00120 
00121         #declare a Twist message to send velocity commands
00122         VelocityMessage = Twist()
00123         # declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00124         # the odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00125         # set the linear velocity to a positive value if isFoward is True
00126         listener = tf.TransformListener()
00127         #declare tf transform
00128         initial_turtlebot_odom_pose = Odometry()
00129         #init_transform: is the transformation before starting the motion
00130         init_transform = geometry_msgs.msg.TransformStamped()
00131         #current_transformation: is the transformation while the robot is moving
00132         current_transform = geometry_msgs.msg.TransformStamped()
00133 
00134         if (isForward):
00135             VelocityMessage.linear.x =abs(speed)
00136         else: #else set the velocity to negative value to move backward
00137             VelocityMessage.linear.x =-abs(speed)
00138         #all velocities of other axes must be zero.
00139         VelocityMessage.linear.y =0.0
00140         VelocityMessage.linear.z =0.0
00141         VelocityMessage.angular.x =0.0
00142         VelocityMessage.angular.y =0.0
00143         VelocityMessage.angular.z =0.0
00144 
00145         distance_moved = 0.0
00146 
00147         loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)
00148 
00149      # First, we capture the initial transformation before starting the motion.
00150      # we call this transformation "init_transform"
00151      # It is important to "waitForTransform" otherwise, it might not be captured.
00152         try:
00153             #wait for the transform to be found
00154 
00155             listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0),rospy.Duration(10.0))
00156             #Once the transform is found,get the initial_transform transformation.
00157             (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00158             #listener.lookupTransform("/base_footprint", "/odom", rospy.Time(0),init_transform)
00159             trans1_mat = tf.transformations.translation_matrix(trans)
00160             rot1_mat   = tf.transformations.quaternion_matrix(rot)
00161             mat1 = numpy.dot(trans1_mat, rot1_mat)
00162             init_transform.transform.translation = trans
00163             init_transform.transform.rotation =rot
00164             #print(mat1)
00165         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00166             rospy.Duration(1.0)
00167      
00168         while True :
00169             rospy.loginfo("Turtlebot moves forwards") 
00170         #/***************************************
00171         # * STEP1. PUBLISH THE VELOCITY MESSAGE
00172         # ***************************************/
00173             self.velocityPublisher.publish(VelocityMessage)
00174             loop_rate.sleep()
00175         #/**************************************************
00176         # * STEP2. ESTIMATE THE DISTANCE MOVED BY THE ROBOT
00177         # *************************************************/
00178             try:
00179 
00180                 #wait for the transform to be found
00181                 listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0), rospy.Duration(10.0) )
00182                 #Once the transform is found,get the initial_transform transformation.
00183                 #listener.lookupTransform("/base_footprint", "/odom",rospy.Time(0))
00184                 (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00185             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00186                 rospy.Duration(1.0)
00187             
00188             trans1_mat = tf.transformations.translation_matrix(trans)
00189             rot1_mat   = tf.transformations.quaternion_matrix(rot)
00190             mat2 = numpy.dot(trans1_mat, rot1_mat)
00191             #print(mat2)
00192             mat3 = numpy.dot(mat1, mat2)
00193             #print(mat3)
00194 
00195             trans3 = tf.transformations.translation_from_matrix(mat3)
00196             
00197             #print(trans3)
00198             rot3 = tf.transformations.quaternion_from_matrix(mat3)
00199             #print(rot3)
00200             
00201             current_transform.transform.translation = trans
00202             current_transform.transform.rotation =rot
00203             distance_moved = distance_moved + (0.5 * sqrt(trans3[0] ** 2 + trans3[1] ** 2))
00204             #print(distance_moved)
00205             #print (numpy.linalg.norm(trans3))
00206             
00207             if not (distance_moved<distance):
00208                 break
00209             
00210     #finally, stop the robot when the distance is moved
00211         VelocityMessage.linear.x =0
00212         self.velocityPublisher.publish(VelocityMessage)
00213     
00214 
00215 
00216     # a function that makes the robot move straight
00217     # @param speed: represents the speed of the robot the robot
00218     # @param distance: represents the distance to move by the robot
00219     # @param isForward: if True, the robot moves forward,otherwise, it moves backward
00220     #
00221     # Method 3: we use coordinates of the robot to estimate the distance
00222 
00223     def move_v3(self, speed, distance, isForward):
00224         #declare a Twist message to send velocity commands
00225             VelocityMessage = Twist()
00226         # declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00227         # the odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00228             listener = tf.TransformListener()
00229         #declare tf transform
00230             initial_turtlebot_odom_pose = Odometry()
00231         #init_transform: is the transformation before starting the motion
00232             init_transform = geometry_msgs.msg.TransformStamped()
00233         #current_transformation: is the transformation while the robot is moving
00234             current_transform = geometry_msgs.msg.TransformStamped()
00235 
00236         #set the linear velocity to a positive value if isFoward is True
00237             if (isForward):
00238                     VelocityMessage.linear.x =abs(speed)
00239             else: #else set the velocity to negative value to move backward
00240                     VelocityMessage.linear.x =-abs(speed)
00241 
00242         # all velocities of other axes must be zero.
00243             VelocityMessage.linear.y =0
00244             VelocityMessage.linear.z =0
00245         #The angular velocity of all axes must be zero because we want  a straight motion
00246             VelocityMessage.angular.x = 0
00247             VelocityMessage.angular.y = 0
00248             VelocityMessage.angular.z = 0
00249 
00250             distance_moved = 0.0
00251             loop_rate = rospy.Rate(20) # we publish the velocity at 10 Hz (10 times a second)    
00252             #we update the initial_turtlebot_odom_pose using the turtlebot_odom_pose global variable updated in the callback function poseCallback
00253             #we will use deepcopy() to avoid pointers confusion
00254             initial_turtlebot_odom_pose = copy.deepcopy(self.turtlebot_odom_pose)
00255 
00256             while True :
00257                     rospy.loginfo("Turtlebot moves forwards")
00258                     self.velocityPublisher.publish(VelocityMessage)
00259          
00260                     loop_rate.sleep()
00261                     
00262                     #rospy.Duration(1.0)
00263                     
00264                     distance_moved = distance_moved+abs(0.5 * sqrt(((self.turtlebot_odom_pose.pose.pose.position.x-initial_turtlebot_odom_pose.pose.pose.position.x) ** 2) +
00265                         ((self.turtlebot_odom_pose.pose.pose.position.x-initial_turtlebot_odom_pose.pose.pose.position.x) ** 2)))
00266                                         
00267                     if not (distance_moved<distance):
00268                         break
00269             
00270             #finally, stop the robot when the distance is moved
00271             VelocityMessage.linear.x =0
00272             self.velocityPublisher.publish(VelocityMessage)
00273 
00274     def degree2radian(self, degreeAngle):
00275         return (degreeAngle/57.2957795)
00276     
00277     def rotate(self,angular_velocity,radians,clockwise):
00278         rotateMessage = Twist()
00279        
00280         #declare tf transform 
00281         listener = tf.TransformListener()
00282         #init_transform: is the transformation before starting the motion
00283         init_transform = geometry_msgs.msg.TransformStamped()
00284         #current_transformation: is the transformation while the robot is moving
00285         current_transform = geometry_msgs.msg.TransformStamped()
00286         
00287         angle_turned = 0.0
00288 
00289         angular_velocity = (-angular_velocity, ANGULAR_VELOCITY_MINIMUM_THRESHOLD)[angular_velocity > ANGULAR_VELOCITY_MINIMUM_THRESHOLD]
00290 
00291         while(radians < 0):
00292             radians += 2* pi
00293 
00294         while(radians > 2* pi):
00295             radians -= 2* pi
00296         
00297         listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0), rospy.Duration(10.0) )
00298         (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00299         #listener.lookupTransform("/base_footprint", "/odom", rospy.Time(0),init_transform)
00300         
00301         init_transform.transform.translation = trans
00302         init_transform.transform.rotation =rot
00303 
00304         #since the rotation is only in the Z-axes 
00305         #start_angle = tf.transformations.#0.5 * sqrt(rot[2] ** 2)
00306         euler = tf.transformations.euler_from_quaternion(rot)
00307         roll = euler[0]
00308         pitch = euler[1]
00309         start_angle = euler[2]
00310 
00311         rotateMessage.linear.x = rotateMessage.linear.y = 0.0
00312         rotateMessage.angular.z = angular_velocity
00313 
00314         if (clockwise):
00315             rotateMessage.angular.z = -rotateMessage.angular.z
00316         
00317         
00318         loop_rate = rospy.Rate(20)
00319         
00320         while True:
00321             rospy.loginfo("Turtlebot is Rotating")
00322 
00323             self.velocityPublisher.publish(rotateMessage)
00324          
00325             loop_rate.sleep()
00326                     
00327             #rospy.Duration(1.0)
00328 
00329             try:
00330 
00331                 #wait for the transform to be found
00332                 listener.waitForTransform("/base_footprint", "/odom", rospy.Time(0), rospy.Duration(10.0) )
00333                 #Once the transform is found,get the initial_transform transformation.
00334                 #listener.lookupTransform("/base_footprint", "/odom",rospy.Time(0))
00335                 (trans,rot) = listener.lookupTransform('/base_footprint', '/odom', rospy.Time(0))
00336             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00337                 rospy.Duration(1.0)
00338 
00339             current_transform.transform.translation = trans
00340             current_transform.transform.rotation =rot
00341 
00342             #since the rotation is only in the Z-axes 
00343             #end_angle = 0.5 * sqrt( rot[2] ** 2)
00344             euler = tf.transformations.euler_from_quaternion(rot)
00345             roll = euler[0]
00346             pitch = euler[1]
00347             end_angle = euler[2]
00348             
00349             angle_turned = abs(end_angle - start_angle)
00350             print "angle_turned: %s" %angle_turned
00351             if (angle_turned > radians):
00352                 break
00353 
00354         self.velocityPublisher.publish(rotateMessage)
00355         loop_rate.sleep()
00356 
00357     def calculateYaw( x1, y1, x2,y2):
00358         bearing = atan2((y2 - y1),(x2 - x1))
00359         #if(bearing < 0) bearing += 2 * PI
00360         bearing *= 180.0 / pi
00361         return bearing
00362 
00363 
00364     def moveSquare(self,sideLength):
00365         for i in range(0, 4):
00366             self.move_v1(0.3, sideLength, True)
00367             #self.shutdown()
00368             self.rotate(0.2, self.degree2radian(90.0),True)
00369    
00370     def __init__(self):
00371         # initiliaze
00372         rospy.init_node('free_space_navigation', anonymous=True)
00373 
00374         # What to do you ctrl + c    
00375         rospy.on_shutdown(self.shutdown)
00376         self.turtlebot_odom_pose = Odometry()
00377         pose_message = Odometry()
00378         self.velocityPublisher = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10)
00379         self.pose_subscriber = rospy.Subscriber("/odom", Odometry, self.poseCallback)     
00380         # 2 HZ
00381         r = rospy.Rate(20)
00382         
00383         r.sleep()
00384 
00385         while not rospy.is_shutdown():
00386             sideLength=1
00387             self.moveSquare(sideLength)
00388             #self.rotate(0.3, self.degree2radian(90.0), True);
00389             #self.rotate(0.3, self.degree2radian(90.0), False);       
00390 
00391         
00392     def shutdown(self):
00393         # stop turtlebot
00394         rospy.loginfo("Stop Drawing Squares")
00395         self.velocityPublisher.publish(Twist())
00396         rospy.sleep(1)
00397     
00398  
00399 
00400     
00401 
00402 if __name__ == '__main__':
00403     try:
00404         free_space_navigation()
00405         rospy.spin()
00406     except rospy.ROSInterruptException:
00407         rospy.loginfo("node terminated.")


gapter
Author(s):
autogenerated on Thu Jun 6 2019 22:05:13