API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 26th, 2018
62
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 7.63 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python
  2. import rospy
  3. from geometry_msgs.msg import *
  4. from sensor_msgs.msg import *
  5. from nav_msgs.msg import *
  6. import math
  7. import random
  8.  
  9. ##########################
  10. # BEGIN Global Variable Definitions
  11. front_distance = -1.0
  12. wall_distance = -1.0
  13. flag = 1
  14. # END Global Variable Definitions
  15. ##########################
  16.  
  17. ##########################
  18. # BEGIN ROS Topic Callback Function [DON'T MESS WITH THIS STUFF]
  19. ##########################
  20. def laserCallback(laser_data):
  21. #This function will set front_distance and wall_distance.  Both of these work in the same way.  If something is closer than the detection_distance for that sensor, the value will tell how close something is.  If nothing is closer than the detection distance, the value of -1 will be used.
  22.     global flag
  23.     if flag == 1:
  24.     print '***********************'
  25.     print len(laser_data.ranges)
  26.     print laser_data.angle_increment
  27.     print laser_data.angle_min
  28.     flag = 2
  29.     global front_distance
  30.     global wall_distance
  31. #HA : increaed as the sensor is able to get mesurments up to 3
  32.     front_detection_distance = 2.9
  33.     wall_detection_distance = 2.9
  34.  
  35.     wf_min_right = 10.0
  36.     wf_min_front = 10.0
  37.  #HA : we might need to change the cur_angle initializeation and the range for i as the actual range of the scan is 60  dgrees not 180  around y
  38.     cur_angle = laser_data.angle_min
  39. #HA:    cur_angle = laser_data.angle_min + (215 * laser_data.angle_increment)
  40.     for i in range(len(laser_data.ranges)):
  41. #HA:    for i in range(215,415):
  42.         if abs(cur_angle + math.pi/2) < (math.pi / 8):
  43. #HA:        if cur_angle > 1.91986:
  44.             #Wall sensor ((-5/8)*math.pi <= cur_angle <= (-3/8)*math.pi)
  45.             if laser_data.ranges[i] < wf_min_right:
  46.                 wf_min_right = laser_data.ranges[i]
  47.  
  48.         if abs(cur_angle) < (math.pi / 8):
  49. #HA:        if abs(cur_angle) < 1.8326:
  50.             #Front sensor ((-1/8)*math.pi <= cur_angle <= (1/8)*math.pi)
  51.             if laser_data.ranges[i] < wf_min_front:
  52.                 wf_min_front = laser_data.ranges[i]
  53.  
  54.         cur_angle = cur_angle + laser_data.angle_increment
  55.  
  56.     #Set the sensor variables
  57.     front_distance = -1
  58.     wall_distance = -1
  59.     if wf_min_front < front_detection_distance:
  60.         front_distance = wf_min_front
  61.     if wf_min_right < wall_detection_distance:
  62.         wall_distance = wf_min_right
  63.  
  64. ############################
  65. ##### END CALLBACK FUNCTION  
  66. ############################
  67.        
  68. if __name__ == '__main__':
  69.     rospy.init_node('lab3', anonymous=True) #Initialize the ros node
  70.     pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist) #Create our publisher to send drive commands to the robot
  71.     rospy.Subscriber("/scan", LaserScan, laserCallback) #Subscribe to the laser scan topic
  72.  
  73.     rate = rospy.Rate(10) #10 Hz  
  74.    
  75.     #SENSOR VARIABLES
  76.     global front_distance #How close is something in front of the robot (-1 = nothing is close)
  77.     global wall_distance  #How close is something on the right side of the robot (-1 = nothing is close)
  78.        
  79.     #########################################
  80.     # LAB 3 VARIABLE DECLARATION CODE : BEGIN
  81.     #########################################
  82.  
  83.     # PART C CODE HERE:
  84.     #  Define and initialize variables that  
  85.     #  you need inside the main loop
  86.     current_state = 'WHERE_AM_I'
  87.     safe_distance = 0.51
  88.     old_front_distance = front_distance
  89.     old_wall_distance = wall_distance
  90.     #HA: we also need the old command to be able to recognize when we get into blind kinect zone
  91.     old_twist_command = Twist()
  92.  
  93.     #######################################
  94.     # LAB 3 VARIABLE DECLARATION CODE : END
  95.     #######################################
  96.    
  97.     while not rospy.is_shutdown():
  98.        
  99.         #Declare the twist command that we will publish this time step
  100.         twist = Twist()
  101.              
  102.         ###############################
  103.         # LAB 3 INNER LOOP CODE : BEGIN
  104.         ###############################
  105.  
  106.         # PART C CODE HERE:
  107.         # Make sure that twist gets set with your drive command
  108.  
  109.         #   print current_state, front_distance, wall_distance
  110.         twist.angular.z = math.pi * 0.5
  111.         ################# WHERE_AM_I #################
  112.         if current_state == 'WHERE_AM_I':
  113.             if front_distance > 0 and wall_distance == -1:
  114.             #HA: if front_distance > safe_distance and wall_distance == -1:
  115.                 current_state = 'GET_TO_WALL'
  116.             elif front_distance == -1 and wall_distance > 0:
  117.             #HA: elif front_distance == -1 and wall_distance > safe_distance:
  118.                 current_state = 'FOLLOW_WALL'
  119.             else:
  120.                 twist.linear.x = random.randint(0, 5)
  121.                 twist.angular.z = math.pi / random.randint(1, 10)
  122.  
  123.         ################# GET_TO_WALL #################
  124.         elif current_state == 'GET_TO_WALL':
  125. #            twist.linear.x = front_distance - safe_distance
  126.             twist.angular.z = math.pi * 0.5
  127.             if  wall_distance > 0:
  128.             #HA: if  wall_distance > safe_distance:
  129.                 current_state = 'FOLLOW_WALL'
  130. #            if  front_distance == -1 and wall_distance == -1:
  131. #       current_state = 'TURN_RIGHT'
  132. #       elif  front_distance <= safe_distance:
  133. #       current_state = 'TURN_LEFT'
  134.  
  135.  
  136.         ################# FOLLOW_WALL #################
  137.         elif current_state == 'FOLLOW_WALL':
  138.             twist.linear.x = safe_distance / 2
  139.             #HA:             twist.linear.x = 1
  140.             if wall_distance > safe_distance * 2:
  141.                 twist.angular.z = math.pi * - 0.2
  142.             else:
  143.             #HA: double check if z value is too big
  144.                 twist.angular.z = math.pi * 10 * (old_wall_distance - wall_distance)
  145. #       print '-----', twist.linear.x, twist.angular.z
  146.  
  147. #HA: range is bigger than hallway dimensions this check need more conditions on both if and elif OR we can lower front_detection_distance back to 0.9 to be less than dimensions
  148.             if  front_distance == -1 and wall_distance == -1:
  149.                 current_state = 'TURN_RIGHT'
  150.             elif ( front_distance < safe_distance * 1.5 and front_distance != -1 ) or ( wall_distance != -1 and wall_distance <= safe_distance) :
  151.                 current_state = 'TURN_LEFT'
  152.  
  153.                
  154.         ################# TURN_LEFT #################
  155.         elif current_state == 'TURN_LEFT':
  156.         #HA: check if z value is too big
  157.             twist.angular.z = math.pi * 0.5
  158.             if  front_distance == -1 or front_distance > safe_distance : #HA: removed * 1.5
  159.                 current_state = 'FOLLOW_WALL'
  160.  
  161.                
  162.         ################# TURN_RIGHT #################
  163.         elif current_state == 'TURN_RIGHT':
  164.         #HA: check if z value is too big
  165.             twist.angular.z = math.pi * -0.5
  166. #            if  front_distance == -1 and wall_distance == -1:
  167. #       twist.linear.x = safe_distance / 2
  168.             if front_distance > 0 and wall_distance == -1:
  169.                 current_state = 'GET_TO_WALL'
  170.             elif front_distance == -1 and wall_distance > 0:
  171.                 current_state = 'FOLLOW_WALL'
  172.  
  173.  
  174.         old_front_distance = front_distance
  175.         old_wall_distance = wall_distance
  176.         old_twist_command = twist
  177.  
  178.         #############################
  179.         # LAB 3 INNER LOOP CODE : END
  180.         #############################
  181.         #HA : reduce the x command as the units are different than simulation
  182.         twist.linear.x = twist.linear.x * 0.01
  183.         #Publish drive command
  184.         pub.publish(twist)
  185.         rate.sleep() #Sleep until the next time to publish
  186.  
  187.     twist = Twist()
  188.     pub.publish(twist)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!