Exemplo n.º 1
0
 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
Exemplo n.º 2
0
    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