00001
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
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
00068
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
00079 self.ang_diff = np.unwrap([0, self.ang_goal - self.curr_ang[2]])[1]
00080
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
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
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
00108
00109
00110
00111 xs = near_ranges * np.cos(near_angles)
00112 ys = near_ranges * np.sin(near_angles)
00113
00114 self.points = np.vstack((xs,ys))
00115
00116 self.bfp_points = np.vstack((np.add(0.275, xs),ys))
00117
00118 self.bfp_dists = np.sqrt(np.add(np.square(self.bfp_points[0][:]),
00119 np.square(self.bfp_points[1][:])))
00120
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
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
00185 self.ang_thresh = self.ang_thresh_big
00186 self.command.angular.z = 0.0
00187 else:
00188
00189 if self.rot_safe:
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
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
00207 push_from_left = push_from_right = 0.0
00208
00209 if len(self.left[:][0]) > 0:
00210 lefts = np.extract(self.left[:][1]<0.45, self.left[:][1])
00211
00212 if len(lefts) > 0:
00213
00214
00215
00216 push_from_left = -0.45 + min(lefts)
00217
00218 if len(self.right[:][0]) > 0:
00219 rights = np.extract(self.right[:][1]>-0.45, self.right[:][1])
00220
00221 if len(rights) > 0:
00222
00223
00224
00225 push_from_right = 0.45 + max(rights)
00226
00227
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
00233 def avoid_obstacles(self):
00234 if len(self.front[0][:]) > 0:
00235 if min(self.front[0][:]) < self.retreat_thresh:
00236
00237
00238
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
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
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
00256
00257
00258
00259
00260
00261
00262
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
00269 if len(lfobs) == 0:
00270 self.command.linear.y = 0.07
00271
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):
00284 if len(self.left[0][:])>0:
00285
00286
00287 left_obs = self.left[:, self.left[1,:]<0.4]
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
00298
00299 right_obs = self.right[:, self.right[1,:]>-0.4]
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()