def set_rgbled(self, value):
"""
Value is a 3-part tuple representing the RGB values of the color, each value being an integer in the range 0-254.
"""
assert len(value) == 3
for i, v in enumerate(value):
assert 0 <= i <= 254
get_service_proxy(c.HEAD, c.ID_LED)(i, v)
def shutdown(self):
rospy.loginfo('Shutting down...')
# Stop QR tracker.
rospy.ServiceProxy('/qr_tracker/stop', std_srvs.srv.Empty)()
# Stop all head motion.
get_service_proxy(c.HEAD, c.ID_ALL_STOP)()
rospy.loginfo('Done.')
def set_pan_angle(self, angle, timeout=30):
# print 'setting pan angle to %s...' % angle
t0 = time.time()
while time.time() - t0 < timeout:
get_service_proxy(c.HEAD, c.ID_PAN_ANGLE)(angle)
# Wait for response.
for _ in range(10):
time.sleep(.1)
if abs(self.pan_angle - angle) < 3:
# print 'achieved!'
return
def set_tilt_angle(self, angle, timeout=30, margin=5):
assert c.TILT_MIN <= angle <= c.TILT_MAX
# print 'setting tilt angle to %s...' % angle
t0 = time.time()
while time.time() - t0 < timeout:
get_service_proxy(c.HEAD, c.ID_TILT_ANGLE)(angle)
# Wait for response.
for _ in range(10):
time.sleep(.1)
if abs(self.tilt_angle - angle) < margin:
# print 'achieved!'
return
def stop(self):
if not self.check_action(c.MOTOR_BREAK):
return
print 'stop'
proxy = get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)
print 'proxy:', proxy
proxy(left=0, right=0)
def tilt_head(self, angle):
assert c.TILT_MIN <= angle <= c.TILT_MAX
get_service_proxy(c.HEAD, c.ID_TILT_ANGLE)(angle)
def set_head_pan_angle(self, angle):
angle = angle % 360
assert 0 <= angle <= 360
get_service_proxy(c.HEAD, c.ID_PAN_ANGLE)(angle)
def disable_ultrasonics(self):
get_service_proxy(c.TORSO, c.ID_SONAR_POWER)(0)
def reset_power_button_light(self):
get_service_proxy(c.TORSO, c.ID_LED_AUTO)(1)
def turn_power_button_light_on(self):
get_service_proxy(c.TORSO, c.ID_LED_AUTO)(0)
get_service_proxy(c.TORSO, c.ID_LED)(1)
def stop(self):
get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)(left=0, right=0)
def set_ultrabright(self, value):
"""
Value is a single integer between 0-254 representing brightness with 0 being off and 254 being full brightness.
"""
assert 0 <= value <= 254
get_service_proxy(c.HEAD, c.ID_LED)(3, value)
def pivot_right_ccw(self):
if not self.check_action(c.MOTOR_PIVOT_RIGHT_CCW):
return
print 'pivot_right_ccw'
proxy = get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)
proxy(left=-self.motor_speed, right=0)
def turn_right(self):
if not self.check_action(c.MOTOR_TURN_CW):
return
proxy = get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)
proxy(left=+self.motor_speed, right=-self.motor_speed)
def go_forward(self):
if not self.check_action(c.MOTOR_FORWARD):
return
print 'forward'
proxy = get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)
proxy(left=self.motor_speed, right=self.motor_speed)
def reverse(self):
if not self.check_action(c.MOTOR_REVERSE):
return
print 'reverse'
proxy = get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)
proxy(left=-self.motor_speed, right=-self.motor_speed)
def go_forwards(self, speed=c.MOTOR_DEFAULT_SPEED):
get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)(left=speed, right=speed)
def go_right(self, speed=c.MOTOR_DEFAULT_SPEED):
get_service_proxy(c.TORSO, c.ID_MOTOR_SPEED)(left=-speed, right=+speed)
def request_sensor_update(self):
# service_name = get_topic_name(c.HEAD, c.ID_FORCE_SENSORS)
# service_type = packet_to_service_type(c.ID_FORCE_SENSORS)
# rospy.ServiceProxy(service_name, service_type)()
get_service_proxy(c.HEAD, c.ID_FORCE_SENSORS)()
def enable_ultrasonics(self):
print 'Enabling ultrasonics...'
get_service_proxy(c.TORSO, c.ID_SONAR_POWER)(1)
