def __init__(self, loop: asyncio.AbstractEventLoop, datadir = "data"):
self.datadir = datadir
self.rvr = SpheroRvrAsync(dal=SerialAsyncDal(loop))
self.oakD = OakD()
self.oakD.startDevice()
self.datadir = f"v2-{int(time.time())}"
os.mkdir(self.datadir)
def __init__(self, loop: asyncio.AbstractEventLoop, datadir: str = "data"):
self.datadir = datadir
self.rvr = SpheroRvrAsync(dal=SerialAsyncDal(loop))
self.camera = PiCamera(resolution=(320, 240), framerate=30)
self.gps = gps(mode=WATCH_ENABLE | WATCH_NEWSTYLE)
self.busy = False
try:
self.interp = tf.lite.Interpreter("steer.tflite")
self.interp.allocate_tensors()
except Exception:
pass
async def init_sphero(self):
# init the rvr
# TODO fix asyncio problems and use the asyncio version
logging.info("RVR: connecting")
self.dal = SerialAsyncDal(asyncio.get_running_loop())
self.rvr = SpheroRvrAsync(self.dal)
# fix rvr blocking all default logging after init...
null_handler = [
h for h in logging.getLogger().handlers if h.name == "null_handler"
][0]
logging.getLogger().removeHandler(null_handler)
logging.info("RVR: waking up")
await self.rvr.wake()
logging.info("RVR: resetting yaw")
await self.rvr.reset_yaw()
logging.info("RVR: init done")
import sys
import os
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), '../../')))
import RPi.GPIO as GPIO
import asyncio
from sphero_sdk import SpheroRvrAsync
from sphero_sdk import SerialAsyncDal
import time
loop = asyncio.get_event_loop()
rvr = SpheroRvrAsync(dal=SerialAsyncDal(loop))
GPIO.setmode(GPIO.BCM)
right_trigger = 20
right_echo = 21
left_trigger = 27
left_echo = 18
right_slesh = 15
left_slesh = 15
GPIO.setup(left_trigger, GPIO.OUT)
GPIO.setup(left_echo, GPIO.IN)
GPIO.setup(right_trigger, GPIO.OUT)
GPIO.setup(right_echo, GPIO.IN)
def distance_left():
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../../../')))
import asyncio
from sphero_sdk import SpheroRvrAsync
from sphero_sdk import SerialAsyncDal
from sphero_sdk import InfraredCodes
from sphero_sdk import RawMotorModesEnum
loop = asyncio.get_event_loop()
rvr = SpheroRvrAsync(
dal=SerialAsyncDal(
loop
)
)
async def main():
""" This program sets up RVR to communicate with another robot, e.g. BOLT, capable of infrared communication.
"""
await rvr.wake()
# Give RVR time to wake up
await asyncio.sleep(2)
await rvr.start_robot_to_robot_infrared_broadcasting(
far_code=InfraredCodes.one.value,
def __init__( self, loop ): self.rvr = SpheroRvrAsync( dal=SerialAsyncDal( loop ) )
service=RvrStreamingServices.color_detection,
handler=color_detected_handler)
if debug: print("Starting accelerometer handler")
await rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.accelerometer,
handler=accelerometer_handler)
if debug: print("Starting ambient light handler")
await rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.ambient_light,
handler=ambient_light_handler)
if debug: print("Starting encoder handler")
await rvr.sensor_control.add_sensor_data_handler(
service=RvrStreamingServices.encoders, handler=encoder_handler)
if debug: print("Starting sensor control")
global delay
await rvr.sensor_control.start(interval=delay)
if __name__ == '__main__':
try:
loop = asyncio.get_event_loop()
args = sys.argv[1:]
rvr = SpheroRvrAsync(dal=SerialAsyncDal(loop, port_id=args[0]))
asyncio.ensure_future(main())
loop.run_forever()
except KeyboardInterrupt:
print('\nProgram terminated with keyboard interrupt.')
loop.run_until_complete(
asyncio.gather(rvr.sensor_control.clear(), rvr.close()))
from geometry_msgs.msg import PoseWithCovariance
from geometry_msgs.msg import Pose
from geometry_msgs.msg import TwistWithCovariance
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
MAX_SENSOR_VALUE = 2**31
loop = asyncio.get_event_loop()
rvr = SpheroRvrAsync(dal=SerialAsyncDal(loop
# device='/dev/ttyTHS1',
))
logging.basicConfig(level=logging.DEBUG)
num_msgs_received = {}
position = Point()
orientation = Quaternion()
pose = Pose(
position=position,
orientation=orientation,
)
pose_with_covariance = PoseWithCovariance(pose=pose, )
linear = Vector3()
