def set_pose(self,rpose):
     RealBot.set_pose(self,rpose)
     
     # We have to update walls here. WHY?
     for pose, dst in zip(self.info.ir_sensors.poses,np.polyval(self.ir_coeff, self.info.ir_sensors.readings)):
         if dst/self.info.ir_sensors.rmax <= 0.99:
             self.walls.add_point(Pose(dst) >> pose >> self.get_pose())
    def set_pose(self, rpose):
        RealBot.set_pose(self, rpose)

        # We have to update walls here. WHY?
        for pose, dst in zip(
                self.info.ir_sensors.poses,
                np.polyval(self.ir_coeff, self.info.ir_sensors.readings)):
            if dst / self.info.ir_sensors.rmax <= 0.99:
                self.walls.add_point(Pose(dst) >> pose >> self.get_pose())
    def __init__(self, pose, color=0xFFFFFF, options=None):

        # create shape
        self._shapes = Struct()

        self._shapes.base_plate = np.array([[0.0335, 0.0534, 1],
                                            [0.0429, 0.0534, 1],
                                            [0.0639, 0.0334, 1],
                                            [0.0686, 0.0000, 1],
                                            [0.0639, -0.0334, 1],
                                            [0.0429, -0.0534, 1],
                                            [0.0335, -0.0534, 1],
                                            [-0.0465, -0.0534, 1],
                                            [-0.0815, -0.0534, 1],
                                            [-0.1112, -0.0387, 1],
                                            [-0.1112, 0.0387, 1],
                                            [-0.0815, 0.0534, 1],
                                            [-0.0465, 0.0534, 1]])

        self._shapes.bbb = np.array([[-0.0914, -0.0406, 1],
                                     [-0.0944, -0.0376,
                                      1], [-0.0944, 0.0376, 1],
                                     [-0.0914, 0.0406,
                                      1], [-0.0429, 0.0406, 1],
                                     [-0.0399, 0.0376, 1],
                                     [-0.0399, -0.0376, 1],
                                     [-0.0429, -0.0406, 1]])

        self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356, 1],
                                            [-0.0429, 0.0233, 1],
                                            [-0.0479, 0.0233, 1],
                                            [-0.0479, -0.0356, 1]])

        self._shapes.bbb_rail_r = np.array([[-0.0914, -0.0356, 1],
                                            [-0.0914, 0.0233, 1],
                                            [-0.0864, 0.0233, 1],
                                            [-0.0864, -0.0356, 1]])

        self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
                                         [-0.0579, 0.0226, 1],
                                         [-0.0739, 0.0226, 1],
                                         [-0.0739, 0.0436, 1]])

        self._shapes.left_wheel = np.array([[0.0254, 0.0595, 1],
                                            [0.0254, 0.0335, 1],
                                            [-0.0384, 0.0335, 1],
                                            [-0.0384, 0.0595, 1]])

        self._shapes.left_wheel_ol = np.array([[0.0254, 0.0595, 1],
                                               [0.0254, 0.0335, 1],
                                               [-0.0384, 0.0335, 1],
                                               [-0.0384, 0.0595, 1]])

        self._shapes.right_wheel_ol = np.array([[0.0254, -0.0595, 1],
                                                [0.0254, -0.0335, 1],
                                                [-0.0384, -0.0335, 1],
                                                [-0.0384, -0.0595, 1]])

        self._shapes.right_wheel = np.array([[0.0254, -0.0595, 1],
                                             [0.0254, -0.0335, 1],
                                             [-0.0384, -0.0335, 1],
                                             [-0.0384, -0.0595, 1]])

        self._shapes.ir_1 = np.array([[-0.0732, 0.0534,
                                       1], [-0.0732, 0.0634, 1],
                                      [-0.0432, 0.0634, 1],
                                      [-0.0432, 0.0534, 1]])

        self._shapes.ir_2 = np.array([[0.0643, 0.0214, 1], [0.0714, 0.0285, 1],
                                      [0.0502, 0.0497, 1], [0.0431, 0.0426,
                                                            1]])

        self._shapes.ir_3 = np.array([[0.0636, -0.0042, 1],
                                      [0.0636, 0.0258, 1], [0.0736, 0.0258, 1],
                                      [0.0736, -0.0042, 1]])

        self._shapes.ir_4 = np.array([[0.0643, -0.0214,
                                       1], [0.0714, -0.0285, 1],
                                      [0.0502, -0.0497, 1],
                                      [0.0431, -0.0426, 1]])

        self._shapes.ir_5 = np.array([[-0.0732, -0.0534, 1],
                                      [-0.0732, -0.0634, 1],
                                      [-0.0432, -0.0634, 1],
                                      [-0.0432, -0.0534, 1]])

        self._shapes.bbb_usb = np.array([[-0.0824, -0.0418, 1],
                                         [-0.0694, -0.0418, 1],
                                         [-0.0694, -0.0278, 1],
                                         [-0.0824, -0.0278, 1]])

        ir_sensor_poses = [
            Pose(-0.0474, 0.0534, np.radians(90)),
            Pose(0.0613, 0.0244, np.radians(45)),
            Pose(0.0636, 0.0, np.radians(0)),
            Pose(0.0461, -0.0396, np.radians(-45)),
            Pose(-0.0690, -0.0534, np.radians(-90))
        ]

        self.info = Struct()
        self.info.wheels = Struct()

        self.info.wheels.radius = 0.0325
        self.info.wheels.base_length = 0.09925
        self.info.wheels.ticks_per_rev = 16
        self.info.wheels.left_ticks = 0
        self.info.wheels.right_ticks = 0

        self.info.wheels.max_velocity = 2 * pi * 130 / 60  # 130 RPM
        self.info.wheels.min_velocity = 2 * pi * 30 / 60  #  30 RPM

        self.info.wheels.vel_left = 0
        self.info.wheels.vel_right = 0

        self.info.ir_sensors = Struct()
        self.info.ir_sensors.poses = ir_sensor_poses
        self.info.ir_sensors.rmax = 0.3
        self.info.ir_sensors.rmin = 0.04
        self.info.ir_sensors.readings = [133] * len(self.info.ir_sensors.poses)

        self.walls = Cloud(0)  # Black, no readings

        # The constructor is called here because otherwise set_pose fails
        RealBot.__init__(self, pose, color)

        # Connect to bot...
        # This code will raise an exception if not able to connect

        if options is None:
            self.log("No IP/port supplied to connect to the robot")
            qb_comm.__init__(self, "localhost", "localhost", 5005)
        else:
            try:
                qb_comm.__init__(self, options.baseIP, options.robotIP,
                                 options.port)
            except AttributeError:
                self.log(
                    "No IP/port supplied in the options to connect to the robot"
                )
                qb_comm.__init__(self, "localhost", "localhost", 5005)

        self.ping()  # Check if the robot is there

        self.pause()  # Initialize self.__paused
    def __init__(self, pose, color = 0xFFFFFF, options = None):
       
        # create shape
        self._shapes = Struct()
        
        self._shapes.base_plate = np.array([[ 0.0335, 0.0534, 1],
                                            [ 0.0429, 0.0534, 1],
                                            [ 0.0639, 0.0334, 1],
                                            [ 0.0686, 0.0000, 1],
                                            [ 0.0639,-0.0334, 1],
                                            [ 0.0429,-0.0534, 1],
                                            [ 0.0335,-0.0534, 1],
                                            [-0.0465,-0.0534, 1],
                                            [-0.0815,-0.0534, 1],
                                            [-0.1112,-0.0387, 1],
                                            [-0.1112, 0.0387, 1],
                                            [-0.0815, 0.0534, 1],
                                            [-0.0465, 0.0534, 1]])
                         
        self._shapes.bbb = np.array([[-0.0914,-0.0406, 1],
                                    [-0.0944,-0.0376, 1],
                                    [-0.0944, 0.0376, 1],
                                    [-0.0914, 0.0406, 1],
                                    [-0.0429, 0.0406, 1],
                                    [-0.0399, 0.0376, 1],
                                    [-0.0399,-0.0376, 1],
                                    [-0.0429,-0.0406, 1]])
                    
        self._shapes.bbb_rail_l = np.array([[-0.0429, -0.0356,1],
                                            [-0.0429, 0.0233,1],
                                            [-0.0479, 0.0233,1],
                                            [-0.0479,-0.0356,1]])
                          
        self._shapes.bbb_rail_r = np.array([[-0.0914,-0.0356,1],
                                            [-0.0914, 0.0233,1],
                                            [-0.0864, 0.0233,1],
                                            [-0.0864,-0.0356,1]])
                          
        self._shapes.bbb_eth = np.array([[-0.0579, 0.0436, 1],
                                         [-0.0579, 0.0226, 1],
                                         [-0.0739, 0.0226, 1],
                                         [-0.0739, 0.0436, 1]])
                       
        self._shapes.left_wheel = np.array([[ 0.0254, 0.0595, 1],
                                            [ 0.0254, 0.0335, 1],
                                            [-0.0384, 0.0335, 1],
                                            [-0.0384, 0.0595, 1]])
                          
        self._shapes.left_wheel_ol = np.array([[ 0.0254, 0.0595, 1],
                                               [ 0.0254, 0.0335, 1],
                                               [-0.0384, 0.0335, 1],
                                               [-0.0384, 0.0595, 1]])
            
        self._shapes.right_wheel_ol = np.array([[ 0.0254,-0.0595, 1],
                                                [ 0.0254,-0.0335, 1],
                                                [-0.0384,-0.0335, 1],
                                                [-0.0384,-0.0595, 1]])
                         
        self._shapes.right_wheel = np.array([[ 0.0254,-0.0595, 1],
                                             [ 0.0254,-0.0335, 1],
                                             [-0.0384,-0.0335, 1],
                                             [-0.0384,-0.0595, 1]])
                         
        self._shapes.ir_1 = np.array([[-0.0732, 0.0534, 1],
                                      [-0.0732, 0.0634, 1],
                                      [-0.0432, 0.0634, 1],
                                      [-0.0432, 0.0534, 1]])
                    
        self._shapes.ir_2 = np.array([[ 0.0643, 0.0214, 1],
                                      [ 0.0714, 0.0285, 1],
                                      [ 0.0502, 0.0497, 1],
                                      [ 0.0431, 0.0426, 1]])
                    
        self._shapes.ir_3 = np.array([[ 0.0636,-0.0042, 1],
                                      [ 0.0636, 0.0258, 1],
                                      [ 0.0736, 0.0258, 1],
                                      [ 0.0736,-0.0042, 1]])
                    
        self._shapes.ir_4 = np.array([[ 0.0643,-0.0214, 1],
                                      [ 0.0714,-0.0285, 1],
                                      [ 0.0502,-0.0497, 1],
                                      [ 0.0431,-0.0426, 1]])
                    
        self._shapes.ir_5 = np.array([[-0.0732,-0.0534, 1],
                                      [-0.0732,-0.0634, 1],
                                      [-0.0432,-0.0634, 1],
                                      [-0.0432,-0.0534, 1]])

        self._shapes.bbb_usb = np.array([[-0.0824,-0.0418, 1],
                                         [-0.0694,-0.0418, 1],
                                         [-0.0694,-0.0278, 1],
                                         [-0.0824,-0.0278, 1]])
        
        ir_sensor_poses = [
                          Pose(-0.0474, 0.0534, np.radians(90)),
                          Pose( 0.0613, 0.0244, np.radians(45)),
                          Pose( 0.0636, 0.0, np.radians(0)),
                          Pose( 0.0461,-0.0396, np.radians(-45)),
                          Pose(-0.0690,-0.0534, np.radians(-90))
                          ]                          
        
        self.info = Struct()
        self.info.wheels = Struct()

        self.info.wheels.radius = 0.0325
        self.info.wheels.base_length = 0.09925
        self.info.wheels.ticks_per_rev = 16
        self.info.wheels.left_ticks = 0
        self.info.wheels.right_ticks = 0
        
        self.info.wheels.max_velocity = 2*pi*130/60 # 130 RPM
        self.info.wheels.min_velocity = 2*pi*30/60  #  30 RPM

        self.info.wheels.vel_left = 0
        self.info.wheels.vel_right = 0

        self.info.ir_sensors = Struct()
        self.info.ir_sensors.poses = ir_sensor_poses
        self.info.ir_sensors.rmax = 0.3
        self.info.ir_sensors.rmin = 0.04
        self.info.ir_sensors.readings = [133]*len(self.info.ir_sensors.poses)

        self.walls = Cloud(0) # Black, no readings

        # The constructor is called here because otherwise set_pose fails
        RealBot.__init__(self, pose, color)
        
        # Connect to bot...
        # This code will raise an exception if not able to connect
        
        if options is None:
            self.log("No IP/port supplied to connect to the robot")
            qb_comm.__init__(self, "localhost", "localhost", 5005)
        else:
            try:
                qb_comm.__init__(self, options.baseIP, options.robotIP, options.port)
            except AttributeError:
                self.log("No IP/port supplied in the options to connect to the robot")
                qb_comm.__init__(self, "localhost", "localhost", 5005)
                
        self.ping() # Check if the robot is there

        self.pause() # Initialize self.__paused