Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 from pkg import *
00030 from std_msgs.msg import Position
00031 from std_msgs.msg import BaseVel
00032 from std_msgs.msg import RobotBase2DOdom
00033 import sys
00034 import numpy as np
00035 import nodes as n
00036 import camera as cam
00037 import math
00038 from pyrob.voice import say
00039 from StringIO import StringIO
00040
00041 class FollowBehavior:
00042 def __init__(self, velocity_topic):
00043 self.not_inited = True
00044 self.velocity_topic = velocity_topic
00045 self.last_said = ''
00046
00047 def init_pose(self, robot_pose):
00048 R = cam.Rx(math.radians(90)) * cam.Ry(math.radians(-90))
00049 T = np.matrix([-.095, 0,.162]).T
00050 self.base_T_camera = cam.homo_transform3d(R, T)
00051 self.robot_pose = n.ConstNode(robot_pose)
00052 self.local_pos = n.nav.V_KeepLocal_P2d_V(self.robot_pose, n.ConstNode(np.matrix([0.0, 0.0]).T))
00053 self.attr = n.nav.V_LinearAttract_V(self.local_pos, dead_zone = 0.20, far_dist = 1.0)
00054 self.cmd = n.nav.R_Conv_V(self.attr, allow_backwards_driving = False)
00055 self.should_stop = self.attr.is_done()
00056
00057 self.cmd.max_vel = .5
00058 self.has_stopped = False
00059 self.count = 0
00060 self.attr.verbosity = -1
00061 print 'FollowBehavior: ready!'
00062
00063 def cursor_moved(self, point):
00064
00065 point3d = np.matrix([point.x, point.y, point.z, 1.0]).T
00066 print 'FollowBehavior.cursor_moved: got point', point3d.T
00067 new_point = self.base_T_camera * point3d
00068 sio = StringIO()
00069 print >>sio, int(round(np.linalg.norm(point3d[0:3]) * 100.0))
00070 new_saying = sio.getvalue()
00071 if new_saying != self.last_said:
00072 say(new_saying)
00073 self.last_said = new_saying
00074
00075
00076 goal2d = new_point[0:2, 0]
00077 print 'FollowBehavior.cursor_moved: 2d goal', goal2d.T
00078 self.local_pos.remember(n.ConstNode(goal2d))
00079 self.has_stopped = False
00080
00081 def robot_moved(self, update):
00082 if self.not_inited:
00083 self.init_pose(n.Pose2D(update.pos.x, update.pos.y, update.pos.th))
00084 self.not_inited = False
00085 else:
00086 self.robot_pose.const = n.Pose2D(update.pos.x, update.pos.y, update.pos.th)
00087
00088 def run(self):
00089 if self.not_inited:
00090 return
00091
00092
00093 if self.should_stop.val(self.count) and not self.has_stopped:
00094 self.velocity_topic.publish(BaseVel(0,0))
00095
00096 self.has_stopped = True
00097 say('done')
00098 elif not self.has_stopped:
00099
00100 base_command = self.cmd.val(self.count)
00101 msg = BaseVel(base_command.forward_vel, base_command.rot_vel)
00102
00103 self.velocity_topic.publish(msg)
00104 self.count = self.count + 1
00105
00106 class FakeTopic:
00107 def publish(self, something):
00108 pass
00109
00110 class FakePoint:
00111 def __init__(self, x, y, z):
00112 self.x = x
00113 self.y = y
00114 self.z = z
00115
00116
00117 if __name__ == '__main__':
00118 import time
00119 pub = rospy.TopicPub('cmd_vel', BaseVel)
00120 follow_behavior = FollowBehavior(pub)
00121 rospy.TopicSub('odom', RobotBase2DOdom, follow_behavior.robot_moved)
00122 rospy.TopicSub(CURSOR_TOPIC, Position, follow_behavior.cursor_moved)
00123 rospy.ready(sys.argv[0])
00124
00125 while not rospy.isShutdown():
00126 follow_behavior.run()
00127 time.sleep(0.016)
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270