def test_leds(self):
""" This program demonstrates how to set the all the LEDs.
"""
rvr = SpheroRvrObserver()
rvr.wake()
# Give RVR time to wake up
time.sleep(2)
rvr.set_all_leds(led_group=RvrLedGroups.all_lights.value,
led_brightness_values=[
color for _ in range(10)
for color in Colors.off.value
])
# Delay to show LEDs change
time.sleep(1)
rvr.set_all_leds(led_group=RvrLedGroups.all_lights.value,
led_brightness_values=[
color for _ in range(10) for color in [255, 0, 0]
])
# Delay to show LEDs change
time.sleep(1)
rvr.close()
def wake_rvr(self):
""" This program will wake up RVR.
"""
rvr = SpheroRvrObserver()
rvr.wake()
# Give RVR time to wake up
time.sleep(2)
def led_show_b(self):
""" This program demonstrates how to set multiple LEDs on RVR using the LED control helper.
"""
rvr = SpheroRvrObserver()
rvr.reset_yaw()
try:
rvr.wake()
# Give RVR time to wake up
time.sleep(2)
rvr.led_control.turn_leds_off()
# Delay to show LEDs change
time.sleep(1)
rvr.led_control.set_multiple_leds_with_enums(
leds=[
RvrLedGroups.headlight_left, RvrLedGroups.headlight_right
],
colors=[Colors.green, Colors.blue])
# Delay to show LEDs change
time.sleep(1)
rvr.led_control.set_multiple_leds_with_rgb(
leds=[
RvrLedGroups.headlight_left, RvrLedGroups.headlight_right
],
colors=[255, 0, 0, 0, 255, 0])
# Delay to show LEDs change
time.sleep(1)
except KeyboardInterrupt:
print('\nProgram terminated with keyboard interrupt.')
finally:
rvr.close()
def main():
connected = False
# The callback for when the client receives a CONNACK response from the server.
def on_connect(client, userdata, flags, rc):
nonlocal connected
connected = rc == 0
print("Connected with result code " + str(rc))
client = mqtt.Client()
client.on_connect = on_connect
client.connect("10.24.95.233", 8883, 60)
rvr = SpheroRvrObserver()
rvr.wake()
time.sleep(2)
rvr.reset_yaw()
def imu_handler(imu_data):
data = imu_data["IMU"]
if connected:
client.publish("sphero/imu", json.dumps(data))
def color_detected_handler(color_detected_data):
data = color_detected_data["ColorDetection"]
if connected:
client.publish("sphero/color", json.dumps(data))
def accelerometer_handler(accelerometer_data):
data = accelerometer_data["Accelerometer"]
if connected:
client.publish("sphero/accelerometer", json.dumps(data))
def ambient_light_handler(ambient_light_data):
data = ambient_light_data["AmbientLight"]
if connected:
client.publish("sphero/ambient_light", json.dumps(data))
rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.imu, handler=imu_handler)
rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.color_detection,
handler=color_detected_handler)
rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.accelerometer,
handler=accelerometer_handler)
rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.ambient_light,
handler=ambient_light_handler)
rvr.sensor_control.start(interval=250)
try:
client.loop_forever()
except KeyboardInterrupt:
print("Keyboard interrupt..")
finally:
client.disconnect()
client.loop_stop()
class SpheroHandler:
def __init__(self):
self.imu = ttypes.IMU(False, 0, 0, 0)
self.accelerometer = ttypes.Accelerometer(False, 0, 0, 0)
self.color = ttypes.Color(False, 0, 0, 0, 0, 0)
self.ambient = ttypes.AmbientLight(False, 0)
self.rvr = SpheroRvrObserver()
self.rvr.wake()
time.sleep(2)
self.rvr.reset_yaw()
self.rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.imu, handler=self.imu_handler)
self.rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.color_detection,
handler=self.color_detected_handler)
self.rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.accelerometer,
handler=self.accelerometer_handler)
self.rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.ambient_light,
handler=self.ambient_light_handler)
self.rvr.sensor_control.start(interval=250)
def sense(self):
return ttypes.SensorData(self.imu, self.accelerometer, self.ambient,
self.color)
def drive_with_heading(self, speed, heading, flags):
#print(f"speed={speed}, heading={heading}, flags={flags}")
self.rvr.drive_with_heading(
speed=speed, # Valid speed values are 0-255
heading=heading, # Valid heading values are 0-359
flags=flags)
time.sleep(0.1)
def imu_handler(self, imu_data):
data = imu_data["IMU"]
self.imu.is_valid = data["is_valid"]
self.imu.roll = data["Roll"]
self.imu.pitch = data["Pitch"]
self.imu.yaw = data["Yaw"]
def color_detected_handler(self, color_detected_data):
data = color_detected_data["ColorDetection"]
self.color.is_valid = data["is_valid"]
self.color.r = data["R"]
self.color.g = data["G"]
self.color.b = data["B"]
self.color.index = data["Index"]
self.color.confidence = data["Confidence"]
def accelerometer_handler(self, accelerometer_data):
data = accelerometer_data["Accelerometer"]
self.accelerometer.is_valid = data["is_valid"]
self.accelerometer.x = data["X"]
self.accelerometer.Y = data["Y"]
self.accelerometer.Z = data["Z"]
def ambient_light_handler(self, ambient_light_data):
data = ambient_light_data["AmbientLight"]
self.ambient.is_valid = data["is_valid"]
self.ambient.value = data["Light"]
break
self.rvr.raw_motors(left_mode=left_mode,
left_speed=abs(left_speed),
right_mode=right_mode,
right_speed=abs(right_speed))
rospy.sleep(1)
if left_speed == 0 and right_speed == 0:
break
if __name__ == '__main__':
try:
rospy.init_node('drive_control_server')
rvr = SpheroRvrObserver()
rvr.wake()
# Give RVR time to wake up
rospy.sleep(2)
rvr.reset_yaw()
server = DriveControlServer(rvr)
rospy.spin()
except rospy.ROSInterruptException:
pass
finally:
rvr.close()