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 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 shutdown_rvr(self):
""" This program will shut down RVR.
"""
rvr = SpheroRvrObserver()
time.sleep(1)
rvr.close()
def righty(self):
rvr = SpheroRvrObserver()
rvr.reset_yaw()
rvr.drive_control.turn_right_degrees(
heading=0, # Valid heading values are 0-359
amount=90)
def wake_rvr(self):
""" This program will wake up RVR.
"""
rvr = SpheroRvrObserver()
rvr.wake()
# Give RVR time to wake up
time.sleep(2)
def drive_backward(self):
rvr = SpheroRvrObserver()
rvr.reset_yaw()
rvr.drive_control.drive_backward_seconds(
speed=16,
heading=0, # Valid heading values are 0-359
time_to_drive=1)
rvr.close()
def drive_forward_rally(self):
""" This program will move RVR forward fast.
"""
rvr = SpheroRvrObserver()
rvr.reset_yaw()
rvr.drive_control.drive_forward_seconds(
speed=128,
heading=0, # Valid heading values are 0-359
time_to_drive=1)
rvr.close()
def drive_forward_crawl(self):
""" This program will move RVR forwards slow.
"""
rvr = SpheroRvrObserver()
rvr.reset_yaw()
rvr.drive_control.drive_forward_seconds(
speed=64,
heading=0, # Valid heading values are 0-359
time_to_drive=.5)
rvr.close()
def drive_forward_lightspeed(self):
""" This program will move RVR forward at lightspeed.
"""
rvr = SpheroRvrObserver()
rvr.reset_yaw()
rvr.drive_control.drive_forward_seconds(
speed=255,
heading=0, # Valid heading values are 0-359
time_to_drive=.2)
rvr.close()
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()
import os
import sys
import time
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), '../../../')))
from sphero_sdk import SpheroRvrObserver
from sphero_sdk import SpheroRvrTargets
from sphero_sdk import ApiResponseCodesEnum
from sphero_sdk.common.log_level import LogLevel
rvr = SpheroRvrObserver(log_level=LogLevel.Debug_Verbose)
def main():
""" This sample uses the generate_api_error command to intentionally generate an error response from RVR
with the specified response code. Under normal circumstances certain commands request a response from RVR.
If an error response is detected then it can be handled by the SDK's response logic. However, if a
command does not have expected output, and therefore doesn't request a response (e.g. drive_with_heading),
then RVR does not generate an error response if one occurs. Setting the request_error_responses_only flag
on RVR will enable it to generate error responses even if no output is expected, and give a program the
ability to catch any of the following 10 error response codes:
Response Code 0x00: success
Response Code 0x01: bad_did
Response Code 0x02: bad_cid
Response Code 0x03: not_yet_implemented
Response Code 0x04: restricted
Response Code 0x05: bad_data_length
Response Code 0x06: failed
Response Code 0x07: bad_data_value
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"]
def will_sleep_handler():
print('RVR is about to sleep...')
rvr.wake()
if __name__ == '__main__':
global rvr
try:
rospy.init_node('rvr_controller')
stay_awake = rospy.get_param('rvr_control/stay_awake')
rvr = SpheroRvrObserver()
rvr.wake()
rospy.sleep(2)
if stay_awake:
rvr.on_will_sleep_notify(will_sleep_handler)
sphero_sensor_pub = SpheroSensorPublisher(rvr)
drive_control = DriveControlServer(rvr)
rospy.spin()
except rospy.ROSInterruptException:
pass
finally:
rvr.close()
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()
from subprocess import call
# for getting the hostname and IP of the underlying system
import socket
# For Gamepad / joystick
import struct, array
from fcntl import ioctl
#-------------------
# RVR stuff
#-------------------
# create the RVR object.
# This also lets the robot do a firmware check every now and then.
print("Starting RVR observer...")
rvr = SpheroRvrObserver()
# RVR battery voltage handler
def battery_percentage_handler(battery_percentage):
print("The battery has {0:2d} % left.".format(
battery_percentage["percentage"]))
# store globally
global batteryPercent
batteryPercent = battery_percentage["percentage"]
# RVR battery state handler
#def battery_voltage_state_change_handler(battery_voltage_state):
# #print("The battery voltage state is {0:1d}.".format(battery_voltage_state["state"]))
# global batteryState
class MyServer(BaseHTTPRequestHandler):
""" A special implementation of BaseHTTPRequestHander for reading data from
and control GPIO of a Raspberry Pi
"""
rvr = SpheroRvrObserver()
def do_HEAD(self):
""" do_HEAD() can be tested use curl command
'curl -I http://server-ip-address:port'
"""
self.send_response(200)
self.send_header('Content-type', 'text/html')
self.end_headers()
def _redirect(self, path):
self.send_response(303)
self.send_header('Content-type', 'text/html')
self.send_header('Location', path)
self.end_headers()
def do_GET(self):
""" do_GET() can be tested using curl command
'curl http://server-ip-address:port'
"""
self.do_HEAD()
with open('control.html', 'rb') as file:
self.wfile.write(file.read())
def do_POST(self):
# Post data as dict
post_data = json.loads(
self.rfile.read(int(
self.headers['Content-Length'])).decode("utf-8"))
print("~~~~~~~~~~~~~~~~~~~~~")
print(post_data)
if post_data.get('system') == "travel":
if post_data.get('type') == "on":
print("on ")
if post_data.get('command') == "left":
# turn left
print("left")
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.reverse.value,
left_duty_cycle=128, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.forward.value,
right_duty_cycle=128 # Valid duty cycle range is 0-255
)
elif post_data.get('command') == "forward":
# go forward
print("forward")
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.forward.value,
left_duty_cycle=64, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.forward.value,
right_duty_cycle=64 # Valid duty cycle range is 0-255
)
elif post_data.get('command') == "right":
# turn right
print("right")
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.forward.value,
left_duty_cycle=128, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.reverse.value,
right_duty_cycle=128 # Valid duty cycle range is 0-255
)
elif post_data.get('command') == "backward":
# go backward
print("backward")
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.reverse.value,
left_duty_cycle=64, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.reverse.value,
right_duty_cycle=64 # Valid duty cycle range is 0-255
)
if post_data.get('type') == "off":
print("off")
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.off.value,
left_duty_cycle=0, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.off.value,
right_duty_cycle=0 # Valid duty cycle range is 0-255
)
"""
if post_data.get('command') == "left":
print("left");
elif post_data.get('command') == "forward":
print("forward");
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.off.value,
left_duty_cycle=0, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.off.value,
right_duty_cycle=0 # Valid duty cycle range is 0-255
)
elif post_data.get('command') == "right":
print("right");
elif post_data.get('command') == "backward":
print("backward");
self.rvr.raw_motors(
left_mode=RawMotorModesEnum.off.value,
left_duty_cycle=0, # Valid duty cycle range is 0-255
right_mode=RawMotorModesEnum.off.value,
right_duty_cycle=0 # Valid duty cycle range is 0-255
)
"""
elif post_data.get('system') == 'piLED':
GPIO.output(28, not GPIO.input(28))
elif post_data.get('system') == 'RVRSetup':
if post_data.get('command') == "on":
# setup RVR
print("Waking RVR")
self.rvr.wake()
time.sleep(2)
self.rvr.reset_yaw()
elif post_data.get('command') == "off":
print("shutdown RVR")
self.rvr.close()
"""
if post_data == 'On':
GPIO.output(28, GPIO.HIGH)
ser.write('on'.encode())
else:
GPIO.output(28, GPIO.LOW)
ser.write('off'.encode())
"""
self.do_HEAD()
self.wfile.write("received".encode("utf-8"))
import os
import sys
import time
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), '../../../')))
from sphero_sdk import SpheroRvrObserver
rvr = SpheroRvrObserver()
# Handler for the active controller stopped notification.
# After sending a stop command, your program can wait for
# this async to confirm that the robot has come to a stop
def stopped_handler():
print('RVR has stopped')
def main():
""" This program has RVR drive around using the normalized RC drive command.
"""
try:
rvr.wake()
# Give RVR time to wake up
time.sleep(2)
# Register the handler for the stopped notification
rvr.on_robot_has_stopped_notify(handler=stopped_handler)
# RVR stuff
#-------------------
import os
import sys
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), './lib/')))
from sphero_sdk import SpheroRvrObserver
from sphero_sdk import Colors
from sphero_sdk import RvrLedGroups
from sphero_sdk import DriveFlagsBitmask
# create the RVR object.
# This also lets the robot do a firmware check every now and then.
print("Starting RVR observer...")
rvr = SpheroRvrObserver()
def turnLEDs(R, G, B):
rvr.led_control.set_all_leds_rgb(R, G, B)
# Delay to show LEDs change
sleep(1)
#-------------------------
# Wake up, Mr. Freeman!
#-------------------------
print("Waking up RVR...")
rvr.wake()
# Give RVR time to wake up
sleep(2)