def run(self):
if self.not_inited:
return
#base_command = self.cmd.val(self.count)
#print 'publishing', base_command.forward_vel, base_command.rot_vel
if self.should_stop.val(self.count) and not self.has_stopped:
self.velocity_topic.publish(BaseVel(0,0))
#print 'stoppping'
self.has_stopped = True
say('done')
elif not self.has_stopped:
#print 'HAS STOPPED'
base_command = self.cmd.val(self.count)
msg = BaseVel(base_command.forward_vel, base_command.rot_vel)
#print 'publishing', base_command.forward_vel, base_command.rot_vel
self.velocity_topic.publish(msg)
self.count = self.count + 1
def cursor_moved(self, point):
#Transform into base's coordinate frame
point3d = np.matrix([point.x, point.y, point.z, 1.0]).T
print 'FollowBehavior.cursor_moved: got point', point3d.T
new_point = self.base_T_camera * point3d
sio = StringIO()
print >>sio, int(round(np.linalg.norm(point3d[0:3]) * 100.0))
new_saying = sio.getvalue()
if new_saying != self.last_said:
say(new_saying)
self.last_said = new_saying
#Drop the z, store as homogeneous coordinates
goal2d = new_point[0:2, 0]
print 'FollowBehavior.cursor_moved: 2d goal', goal2d.T
self.local_pos.remember(n.ConstNode(goal2d))
self.has_stopped = False
