searcher.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('assistive_teleop')
00004 import rospy
00005 import math
00006 import decimal
00007 import numpy as np
00008 import tf
00009 from tf import transformations as tft
00010 from geometry_msgs.msg import (PoseWithCovarianceStamped, PointStamped, 
00011                                 Twist, Point32, PoseStamped)
00012 from sensor_msgs.msg import LaserScan, PointCloud
00013 from std_msgs.msg import String
00014 
00015 class Follower:
00016     def __init__(self):
00017         rospy.init_node('relative_servoing')
00018         rospy.Subscriber('robot_pose_ekf/odom_combined', 
00019                          PoseWithCovarianceStamped, 
00020                          self.update_position)
00021         rospy.Subscriber('/base_scan', LaserScan, self.update_base_laser)
00022         rospy.Subscriber('/servo_goal', PoseStamped, self.update_goal)
00023         self.vel_out = rospy.Publisher('base_controller/command', Twist)
00024         self.rate_test = rospy.Publisher('rate_test', String)
00025         self.tfl = tf.TransformListener()
00026         self.goal_out = rospy.Publisher('/servo_dest', PoseStamped, latch=True)
00027         self.left_out = rospy.Publisher('/left_points', PointCloud)
00028         self.right_out = rospy.Publisher('/right_points', PointCloud)
00029         self.front_out = rospy.Publisher('/front_points', PointCloud)
00030         #Initialize variables, so as not to spew errors before seeing a goal
00031         self.command = Twist()
00032         self.goal_received = False
00033         self.goal_present = False
00034         self.rot_safe = True
00035         self.bfp_goal = PoseStamped()
00036         self.odom_goal = PoseStamped()
00037         self.x_max = 0.5
00038         self.x_min = 0.05
00039         self.y_man = 0.3
00040         self.y_min = 0.05
00041         self.z_max = 0.5
00042         self.z_min = 0.05
00043         self.ang_goal = 0.0
00044         self.ang_thresh_small = 0.01
00045         self.ang_thresh_big = 0.04
00046         self.ang_thresh = self.ang_thresh_big
00047         self.retreat_thresh = 0.3
00048         self.curr_pos = PoseWithCovarianceStamped()
00049         self.dist_thresh = 0.15
00050         self.left = [[],[]]
00051         self.right = [[],[]]
00052         self.front = [[],[]]
00053 
00054     def update_goal(self, msg):
00055         msg.header.stamp = rospy.Time.now()
00056                 
00057         self.tfl.waitForTransform(msg.header.frame_id, '/base_footprint',
00058                                   msg.header.stamp, rospy.Duration(0.5))
00059         self.bfp_goal = self.tfl.transformPose('/base_footprint', msg)
00060         self.tfl.waitForTransform(msg.header.frame_id, 'odom_combined',
00061                                   msg.header.stamp, rospy.Duration(0.5))
00062         self.odom_goal = self.tfl.transformPose('/odom_combined', msg)
00063         self.goal_out.publish(self.odom_goal)
00064         
00065         ang_to_goal = math.atan2(self.bfp_goal.pose.position.y,
00066                                  self.bfp_goal.pose.position.x)
00067         #The desired angular position 
00068         #(current angle in odom, plus the robot-relative change to face goal)
00069         self.ang_goal = self.curr_ang[2] + ang_to_goal
00070         self.goal_received = True
00071         print "New Goal: \r\n %s" %self.bfp_goal
00072 
00073     def update_position(self, msg):
00074         self.curr_ang=tft.euler_from_quaternion([msg.pose.pose.orientation.x,
00075                                                  msg.pose.pose.orientation.y,
00076                                                  msg.pose.pose.orientation.z,
00077                                                  msg.pose.pose.orientation.w])
00078         # Normalized via unwrap relative to 0; (keeps between -pi/pi)
00079         self.ang_diff = np.unwrap([0, self.ang_goal - self.curr_ang[2]])[1]
00080         #print "Ang Diff: %s" %self.ang_diff
00081 
00082         self.dist_to_goal = ((self.odom_goal.pose.position.x-
00083                               msg.pose.pose.position.x)**2 + 
00084                               (self.odom_goal.pose.position.y-
00085                               msg.pose.pose.position.y)**2)**(1./2.)
00086         #print "Dist to goal: %s" %self.dist_to_goal
00087 
00088         if self.goal_received:
00089             if ((self.dist_to_goal < self.dist_thresh) and 
00090                 (abs(self.ang_diff) < self.ang_thresh)):
00091                 self.goal_present = False
00092                 self.goal_received = False
00093                 print "Achieved Goal!"
00094             else:
00095                 self.goal_present = True
00096 
00097     def update_base_laser(self, msg):
00098         max_angle = msg.angle_max
00099         ranges = np.array(msg.ranges)
00100         angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
00101         #Filter out noise(<0.003), points >1m, leaves obstacles
00102         near_angles = np.extract(np.logical_and(ranges<1, ranges>0.003),
00103                                  angles)
00104         near_ranges = np.extract(np.logical_and(ranges<1, ranges>0.003),
00105                                  ranges)
00106         self.bad_side = np.sign(near_angles[np.argmax(abs(near_angles))])
00107         #print "bad side: %s" %bad_side # (1 (pos) = left, -1 = right)
00108         #print "Min in Ranges: %s" %min(ranges)
00109        
00110         #if len(near_ranges) > 0:
00111         xs = near_ranges * np.cos(near_angles)
00112         ys = near_ranges * np.sin(near_angles)
00113         #print "xs: %s" %xs
00114         self.points = np.vstack((xs,ys))
00115         #print "Points: %s" %points
00116         self.bfp_points = np.vstack((np.add(0.275, xs),ys))
00117         #print "bfp Points: %s" %bfp_points
00118         self.bfp_dists = np.sqrt(np.add(np.square(self.bfp_points[0][:]),
00119                                         np.square(self.bfp_points[1][:])))
00120         #print min(self.bfp_dists)
00121         if len(self.bfp_dists) >0:
00122             if min(self.bfp_dists) > 0.5:
00123                 self.rot_safe = True
00124             else:
00125                 self.rot_safe = False
00126         else:
00127             self.rot_safe = True
00128         
00129         self.left = np.vstack((xs[np.nonzero(ys>0.35)[0]],
00130                                ys[np.nonzero(ys>0.35)[0]]))
00131         self.right= np.vstack((xs[np.nonzero(ys<-0.35)[0]],
00132                                 ys[np.nonzero(ys<-0.35)[0]]))
00133         self.front = np.vstack(
00134                           (np.extract(np.logical_and(ys<0.35,ys>-0.35),xs),
00135                            np.extract(np.logical_and(ys<0.35, ys>-0.35),ys)))
00136 
00137         front_dist = (self.front[:][0]**2+self.front[:][1]**2)**(1/2)
00138 
00139         ##Testing and Visualization:###
00140         if len(self.left[:][0]) > 0:
00141             leftScan  = PointCloud()
00142             leftScan.header.frame_id = '/base_laser_link'
00143             leftScan.header.stamp = rospy.Time.now()
00144         
00145             for i in range(len(self.left[0][:])):
00146                 pt = Point32()
00147                 pt.x = self.left[0][i]
00148                 pt.y = self.left[1][i]
00149                 pt.z = 0
00150                 leftScan.points.append(pt)
00151             
00152             self.left_out.publish(leftScan)
00153 
00154         if len(self.right[:][0]) > 0:
00155             rightScan = PointCloud()
00156             rightScan.header.frame_id = '/base_laser_link'
00157             rightScan.header.stamp = rospy.Time.now()
00158 
00159             for i in range(len(self.right[:][0])):
00160                 pt = Point32()
00161                 pt.x = self.right[0][i]
00162                 pt.y = self.right[1][i]
00163                 pt.z = 0
00164                 rightScan.points.append(pt)
00165             
00166             self.right_out.publish(rightScan)
00167 
00168         if len(self.front[:][0]) > 0:
00169             frontScan = PointCloud()
00170             frontScan.header.frame_id = 'base_laser_link'
00171             frontScan.header.stamp = rospy.Time.now()
00172             
00173             for i in range(len(self.front[:][0])):
00174                 pt = Point32()
00175                 pt.x = self.front[0][i]
00176                 pt.y = self.front[1][i]
00177                 pt.z = 0
00178                 frontScan.points.append(pt)
00179             
00180             self.front_out.publish(frontScan)
00181 
00182     def set_rot(self):
00183         if abs(self.ang_diff) < self.ang_thresh: 
00184             #Fully oriented, relax constraint to avoid osscilation
00185             self.ang_thresh = self.ang_thresh_big
00186             self.command.angular.z = 0.0
00187         else: 
00188             #Not oriented, continue until pointed within small constraint
00189             if self.rot_safe: # Check for obstacles in the rotation radius
00190                 self.command.angular.z = (np.sign(self.ang_diff)*
00191                                           np.clip(abs(0.35*self.ang_diff),
00192                                                   0.1, 0.5))
00193                 self.ang_thresh = self.ang_thresh_small
00194             else:
00195                 print "Cannot Rotate, obstacles nearby"
00196 
00197     def set_trans(self):
00198         if (abs(self.ang_diff) < math.pi/6 and
00199             self.dist_to_goal > self.dist_thresh):
00200             #Facing the right direction and not there yet, so start moving.
00201             self.command.linear.x=np.clip(self.dist_to_goal*0.125, 0.05, 0.3)
00202         else:
00203             self.command.linear.x = 0.0
00204 
00205     def slide(self):
00206        # Determine left/right movement speed for strafing obstacle avoidance 
00207         push_from_left = push_from_right = 0.0
00208         #print "LEFT: \r\n %s" %self.left
00209         if len(self.left[:][0]) > 0:
00210             lefts = np.extract(self.left[:][1]<0.45, self.left[:][1])
00211            # print "lefts:\r\n %s" %lefts
00212             if len(lefts) > 0:
00213                 #left_dists = np.sqrt(np.add(np.square(lefts[:][0]),
00214                 #                            np.square(lefts[:][1])))
00215                 #push_from_left = np.sum(np.reciprocal(np.exp(lefts)))
00216                 push_from_left = -0.45 + min(lefts)
00217                 #print "lpush: \r\n %s" %push_from_left
00218         if len(self.right[:][0]) > 0:
00219             rights = np.extract(self.right[:][1]>-0.45, self.right[:][1])
00220            # print "rights:\r\n %s" %rights
00221             if len(rights) > 0:
00222                 #right_dists = np.sqrt(np.add(np.square(rights[:][0]),
00223                 #                             np.square(rights[:][1])))
00224                 #push_from_right = np.sum(np.reciprocal(np.exp(rights)))
00225                 push_from_right = 0.45 + max(rights)
00226                 #print "rpush: \r\n %s" %push_from_right
00227         #print "Push: %s" %(push_from_right + push_from_left)
00228         slide = push_from_right + push_from_left
00229         self.command.linear.y = np.sign(slide)*np.clip(abs(slide), 0.04, 0.07)
00230         print "Slide speed (m/s): %s" %slide
00231 
00232         ##Determine rotation to avoid obstacles in front of robot#
00233     def avoid_obstacles(self):
00234         if len(self.front[0][:]) > 0:
00235             if min(self.front[0][:]) < self.retreat_thresh: 
00236                 #(round-up on corner-to-corner radius of robot) -
00237                 # 0.275 (x diff from base laser link to base footprint)
00238                 #print "front[0][:] %s" %self.front[0][:]
00239                 front_dists = np.sqrt(np.add(np.square(self.front[0][:]),
00240                                              np.square(self.front[1][:])))
00241                 min_x = self.front[0][np.argmin(front_dists)]
00242                 min_y = self.front[1][np.argmin(front_dists)]
00243                 #print "min x/y: %s,%s" %(min_x, min_y)
00244                 self.command.linear.x = (-np.sign(min_x)*
00245                                           np.clip(abs(min_x),0.05,0.1))
00246                 self.command.linear.y = (-np.sign(min_y)*
00247                                           np.clip(abs(min_y),0.05,0.1)) 
00248                 self.command.angular.z = 0.0
00249                 # This should probably be avoided...
00250                 print "TOO CLOSE: Back up slowly..." 
00251                 self.retreat_thresh = 0.4
00252             elif min(self.front[0][:]) < 0.45: 
00253                 self.retreat_thresh = 0.3
00254                 print "Turning Away from obstacles in front"
00255                 #self.command.linear.x = 0.0
00256                # front_dists = np.sqrt(np.add(np.square(self.front[0][:]),
00257                #                              np.square(self.front[1][:])))
00258                # min_x = self.front[0][np.argmin(front_dists)]
00259                # min_y = self.front[1][np.argmin(front_dists)]
00260                # min_ang = math.atan2(min_y, min_x)
00261                # print "min_ang: %s" %min_ang
00262                # self.command.angular.z 
00263 
00264                 lfobs = self.front[0][np.logical_and(self.front[1]>0,
00265                                                      self.front[0]<0.45)]
00266                 rfobs = self.front[0][np.logical_and(self.front[1]<0,
00267                                                      self.front[0]<0.45)]
00268                # print "lfobs: %s" %lfobs
00269                 if len(lfobs) == 0:
00270                     self.command.linear.y = 0.07
00271                 #print "rfobs: %s" %rfobs
00272                 elif len(rfobs) == 0:
00273                     self.command.linear.y += -0.07
00274                 weight = np.reciprocal(np.sum(np.reciprocal(rfobs)) -
00275                                        np.sum(np.reciprocal(lfobs)))
00276                 if weight > 0:
00277                     self.command.angular.z += 0.05 
00278                 else:
00279                     self.command.angular.z += -0.05
00280             else:
00281                 self.retreat_thresh = 0.3
00282 
00283     def left_clear(self): # Base Laser cannot see obstacles beside the back edge of the robot, which could cause problems, especially just after passing through doorways...
00284         if len(self.left[0][:])>0:
00285             #Find points directly to the right or left of the robot (not in front or behind)
00286             # x<0.1 (not in front), x>-0.8 (not behind)
00287             left_obs = self.left[:, self.left[1,:]<0.4] #points too close.
00288             if len(left_obs[0][:])>0:
00289                 left_obs = left_obs[:, np.logical_and(left_obs[0,:]<0.1,
00290                                                       left_obs[0,:]>-0.8)]
00291                 if len(left_obs[:][0])>0:
00292                     print "Obstacle immediately to the left, cannot move."
00293                     self.command.linear.y = 0
00294 
00295     def right_clear(self):
00296         if len(self.right[0][:])>0:
00297             #Find points directly to the right or left of the robot (not in front or behind)
00298             # x<0.1 (not in front), x>-0.8 (not behind)
00299            right_obs = self.right[:, self.right[1,:]>-0.4] #points too close.
00300            if len(right_obs[0][:])>0:
00301                 right_obs = right_obs[:, np.logical_and(right_obs[0,:]<0.1,
00302                                                         right_obs[0,:]>-0.8)]
00303                 if len(right_obs[:][0])>0:
00304                     print "Obstacle immediately to the right, cannot move."
00305                     self.command.linear.y = 0
00306 
00307     def run(self):
00308         if self.goal_present:
00309             self.set_rot()
00310             self.set_trans()
00311             self.slide()
00312             self.avoid_obstacles()
00313             if self.command.linear.y > 0:
00314                 self.left_clear()
00315             elif self.command.linear.y < 0:
00316                 self.right_clear()
00317             print "Sending vel_command: \r\n %s" %self.command
00318             self.vel_out.publish(self.command)
00319         else:
00320             pass
00321 
00322 if __name__ == '__main__':
00323     Follower = Follower()
00324     r = rospy.Rate(10)
00325     while not rospy.is_shutdown():
00326         Follower.run()
00327         r.sleep()


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34