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