def start(self) -> None:
"""Sets up the server socket and waits for incoming connections."""
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
logger.info('Socket created')
try:
self.sock.bind((HOST, PORT))
except socket.error as err:
print('Bind failed. Error code: {} Message: {}'.format(
str(err[0]), err[1]))
sys.exit()
logger.info('Socket bound to %s:%d' % (HOST, PORT))
self.sock.listen()
logger.info('Socket listening for connections')
try:
drive_con = DrivingController(Motor(), Servo())
lidar = Lidar()
while True:
# Wait to accept a connection through a blocking call
conn, addr = self.sock.accept()
logger.info('Socket connected with %s:%d' % (addr[0], addr[1]))
client_con = ClientController(conn, addr, drive_con, lidar)
except KeyboardInterrupt:
logger.warning('Server interrupted by KeyboardInterrupt')
if client_con is not None:
client_con.shutdown()
elif drive_con is not None:
drive_con.shutdown()
finally:
self.sock.close()
logger.info('Socket closed')
class Map:
"""
Handles the map of the world.
On a global scale: has a start, destination, and a list of waypoints. Coord system: GPS coordinates
On a local scale: has a 2D polar map of the world around the boat. Coord system: moves and oriented with the boat
"""
def __init__(self):
self.wind_vane = WindVane()
self.gps = GPS()
self.lidar = Lidar()
self.world_map = load_world_map()
def update_surroundings(self):
"""
Updates the computer's knowledge of the surroundings.
:return:
"""
self.wind_vane.read_wind_vane()
self.gps.read_gps()
self.lidar.read_lidar()
def __init__(self):
#rp.init_node("car", anonymous=True)
joyData = joy.Joy().getData()
vesc = Vesc()
zed = Zed()
lidar = Lidar()
# Need to implement a way to kill a mode when a new one is activated.
if joyData.buttons[1]: #A
mode = auto.LineFollow(zed)
elif joyData.buttons[2]: #B
mode = auto.LaneCenter(lidar)
elif joyData.buttons[3]: #X
mode = auto.Serpentine(zed, lidar)
def on_init(self):
# Map
self._map = Map()
self._map.load_from_file(self._map_file)
self._space_converter = geometry.SpaceConverter(
self._width / (self._map.meters_per_cell * self._map.width),
(self._map._global_origin_x, self._map._global_origin_y))
# Mbot
self._mbot = Mbot(self._map,
max_trans_speed=self._mbot_max_trans_speed,
max_angular_speed=self._mbot_max_angular_speed,
real_time_factor=self._real_time_factor)
# Lidar
self._lidar = Lidar(lambda at_time: self._mbot.get_pose(at_time),
self._map,
self._space_converter,
use_noise=self._use_noise,
dist_measure_sigma=self._lidar_dist_measure_sigma,
theta_step_sigma=self._lidar_theta_step_sigma,
num_ranges_noise=self._lidar_num_ranges_noise,
real_time_factor=self._real_time_factor)
# Pygame
pygame.init()
height = self._space_converter.to_pixel(self._map.height *
self._map.meters_per_cell)
pygame.display.set_mode((self._width, height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self._display_surf = pygame.display.get_surface()
self._display_surf.blit(self._map.render(self._space_converter),
(0, 0))
pygame.display.flip()
# Add sprites
if self._render_lidar:
self._sprites.add(self._lidar)
self._sprites.add(self._mbot)
# Start
self._lcm_thread.start()
self._lidar.start()
self._running = True
def sample_usage2():
vertices = np.array([[0, -1, -1], [0, 1, -1], [0, 0, 1]])
polygons = np.array((0, 1, 2)).reshape(1, 3)
from lidar import Lidar
ray_origin, ray_direction = Lidar(position=(10, 0,
0)).create_rays(vertices)
point_cloud = ray_intersection(ray_origin, ray_direction, vertices,
polygons)
print(len(point_cloud))
from utilities.visualization import visualize_2d
visualize_2d(point_cloud)
visualize_2d(point_cloud, plane=(0, 2))
visualize_2d(point_cloud, plane=(1, 2))
def __init__(self):
"""Initialize the components of the car."""
rp.init_node("car", anonymous=True)
self.joy = Joy()
self.zed = Zed()
self.lidar = Lidar()
self.vesc = Vesc()
rp.Subscriber("vesc/joy", JoyMsg, self.joy_callback)
rp.Subscriber("zed/normal", Image, self.zed_callback)
rp.Subscriber("scan", LaserScan, self.lidar_callback)
self.serpentine = Serpentine("", False, -1, -2)
self.lineFollow = LineFollow()
self.laneCenter = LaneCenter((False, True), 0, False)
#self.laneCenter = LaneCenter()
#self.orbit = Orbit()
#self.parallel = Parallel()
#self.roundabout = Roundabout()
#self.serpentine = Serpentine(self.vesc)
"""
def create_actor(carla_actor, parent):
"""
Static factory method to create vehicle actors
:param carla_actor: carla sensor actor object
:type carla_actor: carla.Sensor
:param parent: the parent of the new traffic actor
:type parent: carla_ros_bridge.Parent
:return: the created sensor actor
:rtype: carla_ros_bridge.Sensor or derived type
"""
if carla_actor.type_id.startswith("sensor.camera"):
return Camera.create_actor(carla_actor=carla_actor, parent=parent)
if carla_actor.type_id.startswith("sensor.lidar"):
return Lidar(carla_actor=carla_actor, parent=parent)
else:
return Sensor(carla_actor=carla_actor, parent=parent)
def render_scene(n):
with File("dataset_{:>02}.hdf5".format(n), "w") as hdf:
print(n)
scene.place_object_randomly("car")
scene.place_object_randomly("car")
scene.place_object_randomly("car")
scene.place_object_randomly("box")
scene.place_object_randomly("box")
scene.place_object("ground", position=(0, 0, 0), angle=0)
scene_vertices, scene_polygons = scene.build_scene()
path = os.path.join(os.path.dirname(__file__), "{:>02}.png".format(n))
scene.visualize(scene_vertices, path)
point_cloud = Lidar(delta_azimuth=2 * np.pi / 4000,
delta_elevation=np.pi / 500,
position=(0, -40, 1)).sample_3d_model_gpu(
scene_vertices, scene_polygons)
hdf.create_dataset("point_cloud", data=point_cloud)
hdf.create_dataset("bounding_boxes", data=scene.get_bounding_boxes())
scene.clear()
def scanning(rawPoints):
range_finder = Lidar(
'/dev/ttyUSB0') # initializes serial connection with the lidar
nbr_tours = 0
nbr_pairs = 0
nbr_points = 0
distancesList = []
start_time = time.time()
iterator = range_finder.scan(
'express', max_buf_meas=False,
speed=450) # returns a yield containing each measure
try:
for measure in iterator:
#print("medindo...")
if time.time(
) - start_time > 1: # Given the time for the Lidar to "heat up"
dX = measure[0][3] * np.cos(-measure[0][2] * ANGLE_TO_RAD +
PI / 2)
dY = measure[0][3] * np.sin(-measure[0][2] * ANGLE_TO_RAD +
PI / 2)
distancesList.append([dX, dY])
nbr_pairs += 1
nbr_points += 1
if nbr_pairs == MIN_NEIGHBOORS:
rawPoints.put(distancesList[:])
del distancesList[:]
nbr_pairs = 0
if measure[0][0]:
print("Nombre de points: {}".format(nbr_points))
nbr_tours += 1
if len(distancesList) > 2:
rawPoints.put(distancesList[:])
rawPoints.put(0)
del distancesList[:]
nbr_pairs = 0
nbr_points = 0
except KeyboardInterrupt:
#print("Saindo...")
range_finder.stop_motor()
range_finder.reset()
rawPoints.put(None)
def sample_usage():
from data_loaders.load_3d_models import load_Porsche911
from lidar import Lidar
vertices, polygons = load_Porsche911()
ray_origin, ray_direction = Lidar(delta_azimuth=2 * np.pi / 1000,
delta_elevation=np.pi / 500,
position=(10, 0,
0)).create_rays(vertices)
print("Created {} rays.".format(len(ray_direction)))
point_cloud = ray_intersection(ray_origin, ray_direction, vertices,
polygons)
print(len(point_cloud))
from utilities.visualization import visualize_2d, visualize_3d
visualize_3d(point_cloud)
visualize_3d(vertices)
visualize_2d(point_cloud)
visualize_2d(vertices)
visualize_2d(point_cloud, plane=(0, 2))
visualize_2d(vertices, plane=(0, 2))
visualize_2d(point_cloud, plane=(1, 2))
visualize_2d(vertices, plane=(1, 2))
def sample_usage():
from lidar import Lidar
from data_loaders.load_3d_models import load_Porsche911
from data_loaders.create_test_data import create_box, create_rectangle
import pptk
import os
scene = Scene(spawn_area=Polygon(((-10, -10), (-10, 10), (10, 10), (10,
-10))))
scene.add_model_to_shelf(*load_Porsche911(), "car")
scene.add_model_to_shelf(*create_box(position=(0, 0, 2), size=(4, 6, 4)),
"box")
scene.add_model_to_shelf(
*create_rectangle(position=(0, 0, 0), size=(25, 25)), "ground")
scene.place_object_randomly("car")
scene.place_object_randomly("car")
scene.place_object_randomly("car")
scene.place_object_randomly("box")
scene.place_object_randomly("box")
scene.place_object("ground", position=(0, 0, 0), angle=0)
scene_vertices, scene_polygons = scene.build_scene()
path = os.path.join(os.path.dirname(__file__), "tmp", "scene_sample.png")
scene.visualize(scene_vertices, path=path)
# point_cloud = Lidar(delta_azimuth=2 * np.pi / 4000,
# delta_elevation=np.pi / 500,
# position=(0, -40, 1)).sample_3d_model(scene_vertices,
# scene_polygons,
# rays_per_cycle=400)
point_cloud = Lidar(delta_azimuth=2 * np.pi / 4000,
delta_elevation=np.pi / 1000,
position=(0, -40, 1)).sample_3d_model_gpu(
scene_vertices, scene_polygons)
v = pptk.viewer(point_cloud)
v.set(point_size=.01)
def scanning(my_q):
range_finder = Lidar(
'/dev/ttyUSB1') # initializes serial connection with the lidar
nbr_tours = 0
start_time = time.time()
iterator = range_finder.scan(
'express', max_buf_meas=False,
speed=350) # returns a yield containing each measure
try:
for measure in iterator:
#print("medindo...")
if time.time(
) - start_time > 1: # Given the time for the Lidar to "heat up"
my_q.put(measure)
if measure[0][0]:
nbr_tours += 1
my_q.put(0)
except KeyboardInterrupt:
#print("Saindo...")
range_finder.stop_motor()
range_finder.reset()
my_q.put(None)
ip = '192.168.50.' + str(i) print 'Connecting to', ip conn = rpyc.connect(ip, 18861) piglows1.append(conn) piglows.append(conn.root) for piglow in piglows: piglow.init() piglow.all_off() print clearString serialDevice = sys.argv[1] lid = Lidar(serialDevice) print 'Opened Lidar on serial port', serialDevice lid.laserOn() print 'Lidar laser is ON!' oldDistances = [] for i in range(0, numberSamples): oldDistances.append(4095) while True: distances = [] distances2 = [] for _ in range(0, 23): distances2.append([])
import sys
from service.ws_client import WebsocketService
sys.path.append('../')
# from lidar_mock import MockLidar as Lidar
from lidar import Lidar
if __name__ == "__main__":
connection = WebsocketService()
slam = Lidar(on_map_change=connection.send)
slam.run()
def __init__(self):
self.wind_vane = WindVane()
self.gps = GPS()
self.lidar = Lidar()
self.world_map = load_world_map()
def _create_actor(self, carla_actor): # pylint: disable=too-many-branches,too-many-statements
"""
create an actor
"""
parent = None
if carla_actor.parent:
if carla_actor.parent.id in self.actors:
parent = self.actors[carla_actor.parent.id]
else:
parent = self._create_actor(carla_actor.parent)
actor = None
pseudo_actors = []
#print("typeid")
#print(carla_actor.type_id)
if carla_actor.type_id.startswith('traffic'):
if carla_actor.type_id == "traffic.traffic_light":
actor = TrafficLight(carla_actor, parent, self.comm)
else:
actor = Traffic(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("vehicle"):
print(carla_actor.attributes.get('role_name'))
print(carla_actor.type_id)
print(carla_actor.id)
if carla_actor.attributes.get('role_name')\
in self.parameters['ego_vehicle']['role_name']:
actor = EgoVehicle(
carla_actor, parent, self.comm, self._ego_vehicle_control_applied_callback)
pseudo_actors.append(ObjectSensor(parent=actor,
communication=self.comm,
actor_list=self.actors,
filtered_id=carla_actor.id))
else:
actor = Vehicle(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("sensor"):
if carla_actor.type_id.startswith("sensor.camera"):
if carla_actor.type_id.startswith("sensor.camera.rgb"):
actor = RgbCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.depth"):
actor = DepthCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.camera.semantic_segmentation"):
actor = SemanticSegmentationCamera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
else:
actor = Camera(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.lidar"):
actor = Lidar(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.gnss"):
actor = Gnss(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.collision"):
actor = CollisionSensor(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("sensor.other.lane_invasion"):
actor = LaneInvasionSensor(
carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
else:
actor = Sensor(carla_actor, parent, self.comm, self.carla_settings.synchronous_mode)
elif carla_actor.type_id.startswith("spectator"):
actor = Spectator(carla_actor, parent, self.comm)
elif carla_actor.type_id.startswith("walker"):
actor = Walker(carla_actor, parent, self.comm)
else:
actor = Actor(carla_actor, parent, self.comm)
with self.update_lock:
self.actors[carla_actor.id] = actor
for pseudo_actor in pseudo_actors:
with self.update_lock:
self.pseudo_actors.append(pseudo_actor)
return actor
from gridmap import GridMap
from imu import IMU
from lidar import Lidar
if __name__ == "__main__":
lidar_data = Lidar("lidar")
imu_data = IMU("imu", "speed")
map = GridMap()
print(lidar_data)
print(imu_data)
print(map)
# lidarData.show_all()
#! /usr/bin/env python
import rospy
from lidar import Lidar
if __name__ == '__main__':
# Initial node
rospy.init_node('lidar', anonymous=True)
rospy.loginfo('Start the lidar node')
lidar = Lidar()
if lidar.setup():
lidar.spin()
def make_env(n_substeps=3,
horizon=250,
deterministic_mode=False,
n_agents=1,
env_no=1):
env = Base(n_agents=n_agents,
n_substeps=n_substeps,
horizon=horizon,
floor_size=10,
grid_size=50,
deterministic_mode=deterministic_mode,
env_no=env_no)
# Add Walls
#env.add_module(RandomWalls(grid_size=5, num_rooms=2, min_room_size=5, door_size=5, low_outside_walls=True, outside_wall_rgba="1 1 1 0.1"))
# Add Agents
first_agent_placement = uniform_placement
second_agent_placement = uniform_placement
agent_placement_fn = [first_agent_placement] + [second_agent_placement]
env.add_module(Agents(n_agents, placement_fn=agent_placement_fn))
# Add LidarSites
n_lidar_per_agent = 1
visualize_lidar = False
compress_lidar_scale = None
if visualize_lidar:
env.add_module(
LidarSites(n_agents=n_agents, n_lidar_per_agent=n_lidar_per_agent))
env.reset()
keys_self = ['agent_qpos_qvel']
keys_mask_self = [] #['mask_aa_obs']
keys_external = [] #['agent_qpos_qvel']
keys_mask_external = []
keys_copy = []
env = AddConstantObservationsWrapper(
env, new_obs={'agents_health': np.full((n_agents, 1), 100.0)})
keys_self += ['agents_health']
env = ProjectileWrapper(env)
#env = HealthWrapper(env)
env = SplitMultiAgentActions(env)
env = DiscretizeActionWrapper(env, 'action_movement')
env = AgentAgentObsMask2D(env)
if n_lidar_per_agent > 0:
env = Lidar(env,
n_lidar_per_agent=n_lidar_per_agent,
visualize_lidar=visualize_lidar,
compress_lidar_scale=compress_lidar_scale)
keys_copy += ['lidar']
keys_external += ['lidar']
env = SplitObservations(env,
keys_self + keys_mask_self,
keys_copy=keys_copy)
env = DiscardMujocoExceptionEpisodes(env)
env = SelectKeysWrapper(env,
keys_self=keys_self,
keys_external=keys_external,
keys_mask=keys_mask_self + keys_mask_external,
flatten=False)
return env
def __init__(self): self.sampler = GridSampler() self.recorder = Recorder() self.lidar = Lidar() self.tracer = Tracer(self.lidar, self.recorder)
self.playground = np.ones(self.SHAPE, dtype=np.uint8) * 255
for x, y in self.predicted_pedestrians_location:
cv2.circle(self.playground, (int(x), int(y)), 5, (0, 0, 255),
cv2.FILLED)
for x, y in self.true_pedestrians_location:
cv2.circle(self.playground, (x, y), 5, (0, 0, 0), cv2.FILLED)
vehicle_x, vehicle_y, _ = self.autonomous_vehicle.location
start_point = (int(vehicle_x) - 5, int(vehicle_y) - 5)
end_point = (int(vehicle_x) + 5, int(vehicle_y) + 5)
cv2.rectangle(self.playground, start_point, end_point, (0, 255, 0),
cv2.FILLED)
def test_show(self):
cv2.imshow("Screen", self.playground)
cv2.waitKey(0)
if __name__ == "__main__":
pedestrians = Pedestrians.init_random(30)
lidar = Lidar.init_from_pedestrians(pedestrians)
autonomous_vehicle = AutonomousVehicle(360, 640, 0, lidar, (360, 640),
(0, 0))
scene = Scene(autonomous_vehicle, pedestrians)
for i in range(30):
scene.next(i)
scene.render()
scene.test_show()
class Scene:
def __init__(self):
self.sampler = GridSampler()
self.recorder = Recorder()
self.lidar = Lidar()
self.tracer = Tracer(self.lidar, self.recorder)
def render(self):
samples = [sample for sample in self.sampler.getSample()]
# self.saveSample(samples)
weights = self.getWeights(samples)
num = len(samples)
k = 0
for sample, weight in zip(samples, weights):
if k % 10000 == 0:
print('%d / %d' % (k, num))
k += 1
ray = self.lidar.generateRay(sample)
# initialize the energy of the ray
ray.energy = self.lidar.pulseEnergy * weight
self.tracer.trace(ray, 0, 0)
# records = np.array(self.tracer.records)
# np.savetxt('records.txt', records)
self.recorder.save()
def getWeights(self, samples):
sampleRadii = np.array([np.linalg.norm(sample) for sample in samples])
weights = np.exp(-sampleRadii * sampleRadii * 0.5 / self.lidar.sigmaBeam2)
weights = weights / np.sum(weights)
return weights
def saveSample(self, sample):
npsample = np.array(sample)
np.savetxt('imPulseFile.txt', npsample)
def renderUsingSimulatedPhoton(self):
""" According to DART manual.
:return:
"""
minPerSubcenter = 5
samples = self.sampler.getSample()
weights = self.getWeights(samples)
minWeight = min(weights)
# get photon number per subcenter
subcenterPhotonNumbers = []
for weight in weights:
subcenterPhotonNumber = int(weight / minWeight * minPerSubcenter) # TODO little number may not be divisor
subcenterPhotonNumbers.append(subcenterPhotonNumber)
# be sure that subcenter with smallest weight has minimun photon
# and keep gaussian shape pulse
actualPhotonNumber = sum(subcenterPhotonNumbers)
#
energy = self.lidar.pulseEnergy / actualPhotonNumber
for sample, subcenterPhotonNumber in zip(samples, subcenterPhotonNumbers):
for i in range(subcenterPhotonNumber):
ray = self.lidar.generateRay(sample)
ray.energy = energy
self.tracer.trace(ray, 0, 0)
def renderUsingMultiLaunch(self):
n = 5
samples = [sample for sample in self.sampler.getSample()]
# self.saveSample(samples)
weights = self.getWeights(samples)
num = len(samples)
k = 0
for sample, weight in zip(samples, weights):
if k % 10000 == 0:
print('%d / %d' % (k, num))
k += 1
for i in range(n):
ray = self.lidar.generateRay(sample)
# initialize the energy of the ray
ray.energy = self.lidar.pulseEnergy * weight / n
self.tracer.trace(ray, 0, 0)
# records = np.array(self.tracer.records)
# np.savetxt('records.txt', records)
self.recorder.save()
import time
from lidar import Lidar
from queue import Queue
if __name__ == "__main__":
ld = Lidar("/dev/tty.SLAB_USBtoUART")
print(ld.getDeviceInfo())
print(ld.getDeviceHealth())
q = Queue()
ld.startScan(q)
start_time = time.time()
mode = 0
while True:
result = q.get()
if result['first_scan']:
if mode == 0:
mode = 1
elif mode == 1:
mode = 2
elif mode == 2:
ld.stopScan()
print(time.time() - start_time)
exit(0)
if mode == 2:
print(str(result) + ",")
class Car:
controlMode = [0, 0, 0, 0] # lineFollow, laneCenter, serpentine
utilMode = [0]
speed = 0
def __init__(self):
"""Initialize the components of the car."""
rp.init_node("car", anonymous=True)
self.joy = Joy()
self.zed = Zed()
self.lidar = Lidar()
self.vesc = Vesc()
rp.Subscriber("vesc/joy", JoyMsg, self.joy_callback)
rp.Subscriber("zed/normal", Image, self.zed_callback)
rp.Subscriber("scan", LaserScan, self.lidar_callback)
self.serpentine = Serpentine("", False, -1, -2)
self.lineFollow = LineFollow()
self.laneCenter = LaneCenter((False, True), 0, False)
#self.laneCenter = LaneCenter()
#self.orbit = Orbit()
#self.parallel = Parallel()
#self.roundabout = Roundabout()
#self.serpentine = Serpentine(self.vesc)
"""
lineFollow = LineFollow(vesc)
laneCenter = LaneCenter(vesc)
polebending = Polebending(vesc)
parallel = Parallel(vesc)
roundabout = Roundabout(vesc)"""
def controller(self, zed, lidar, vesc):
#try:
joyButtons = self.joy.getData().buttons
self.setMode(joyButtons)
if joyButtons[5]: # Autonomous Mode
self.runMode(zed, lidar, vesc)
else:
self.serpentine.resetValues()
#except:
#print "You've got an error! (Controller probably can't connect.)"
def setMode(self, buttons):
if buttons[0]: self.controlMode = [1, 0, 0, 0] # X |
elif buttons[1]: self.controlMode = [0, 1, 0, 0] # A |
elif buttons[2]: self.controlMode = [0, 0, 1, 0] # B |
elif buttons[3]: self.controlMode = [0, 0, 0, 1] # Y |
elif buttons[9]: self.utilMode = [1] # START | ENABLE DISPLAY MODE (Independent from other modes)
elif buttons[8]: # BACK | TURNS OFF ALL MODES
self.controlMode = [0, 0, 0, 0]
self.utilMode = [0]
cv2.destroyAllWindows()
self.speed = 0
if buttons[6]:
self.speed -= 0.05
print "speed %f" % self.speed
if buttons[7]:
self.speed += 0.05
print "speed %f" % self.speed
def runMode(self, zed, lidar, vesc, DISPLAY=False):
if self.utilMode[0]:
DISPLAY = True
if self.controlMode[0]: # X
self.lineFollow.run(zed, vesc, DISPLAY)
elif self.controlMode[1]: # A
self.laneCenter.run(zed, lidar, vesc, DISPLAY)
self.laneCenter.setSpeedMod(self.speed)
elif self.controlMode[2]: # B
self.serpentine.run(zed, lidar, vesc, DISPLAY)
elif self.controlMode[3]: # Y
pass
def joy_callback(self, data):
self.joy.setData(data)
def zed_callback(self, data):
self.zed.setImage(data)
self.controller(self.zed, self.lidar, self.vesc)
def lidar_callback(self, data):
self.lidar.setData(data)
from auto import autonomous_car
from servo import Servo
from machine import I2C, Pin, Neopixel
import time
from lidar import Lidar
from lights import LightBar
from vl53l0x import VL53L0X
import uasyncio as asyncio
lenkservo = Servo(26)
lenkservo.set(0)
lenkservo.offset(-.2) #adjust to make it straight
lidarservo = Servo(18, min_ms=0.6, max_ms=2.6, range_degrees=180)
i2c = I2C(sda=Pin(21), scl=Pin(22), freq=100000)
lidar = Lidar(lidarservo, VL53L0X(i2c))
kitt = LightBar()
car = autonomous_car(lidar, kitt, lenkservo, i2c)
loop = asyncio.get_event_loop()
try:
loop.run_forever()
except KeyboardInterrupt: #catch keybord interrupt
car.m0.brake()
car.m1.brake()
raise
except: #catch others
car.m0.brake()
car.m1.brake()
raise
#!/usr/bin/env python
# pylint: disable=C0103
import sys
import os
from lidar import Lidar
# Last part of the path is the name
dirname = sys.argv[1].rstrip('/')
filename = os.path.basename(dirname).lower()
if not os.path.isdir(dirname):
raise Exception('Argument must be a directory {}'.format(dirname))
# Assumes of the form LIDAR-DTM-2M-SD92
(_, model, resolution, base) = filename.split('-')
area = Lidar(model, resolution, base)
area.load(dirname)
# area.write_csv('output/' + filename + '.csv')
area.normalise()
area.write_png('output/' + filename + '.png')
t = route[0][0].transform
car.set_transform(t)
t.location.z += 25 # 15 meters above car
sp.set_transform(t)
car.apply_control(carla.VehicleControl(throttle=0, steer=0))
#Set controll parameters
K = np.array([.01, 0.15]).reshape((1, 2))
L = 3.5
#Wait for car to be ready
time.sleep(1)
#Import and create instances of modules
from lidar import Lidar
lidar = Lidar(world, car)
from object_detector import ObjectDetector
object_detector = ObjectDetector(lidar, 1)
from route_planner import RoutePlanner
route_planner = RoutePlanner([r[0] for r in route], .5)
lidar.start()
from controller import Controller
ctrl = Controller(K, L, world.debug)
previous_time = -1
while True:
tck = world.wait_for_tick(1)
class Gui:
def __init__(self, map_file, render_lidar, use_noise,
lidar_dist_measure_sigma, lidar_theta_step_sigma,
lidar_num_ranges_noise, odom_trans_sigma, odom_rot_sigma,
width, mbot_max_trans_speed, mbot_max_angular_speed,
real_time_factor):
# Model
self._map_file = map_file
self._use_noise = use_noise
self._lidar_dist_measure_sigma = lidar_dist_measure_sigma
self._lidar_theta_step_sigma = lidar_theta_step_sigma
self._lidar_num_ranges_noise = lidar_num_ranges_noise
self._odom_trans_sigma = odom_trans_sigma
self._odom_rot_sigma = odom_rot_sigma
self._mbot_max_trans_speed = mbot_max_trans_speed
self._mbot_max_angular_speed = mbot_max_angular_speed
self._map = None
self._mbot = None
self._lidar = None
self._odom_pose = geometry.Pose(0, 0, 0)
self._last_pose = geometry.Pose(0, 0, 0)
# Controller
self._lcm = lcm.LCM("udpm://239.255.76.67:7667?ttl=2")
# LCM update rate
self._lcm_handle_rate = 100
# LCM channel names
self._odometry_channel = 'ODOMETRY'
self._motor_command_channel = 'MBOT_MOTOR_COMMAND'
# Subscribe to lcm topics
self._lcm.subscribe(self._motor_command_channel,
self._motor_command_handler)
# LCM callback thread
self._lcm_thread = threading.Thread(target=self._handle_lcm)
self._real_time_factor = real_time_factor
# View
self._render_lidar = render_lidar
self._running = True
self._display_surf = None
self._width = width
self._sprites = pygame.sprite.RenderUpdates()
self._max_frame_rate = 50
self._space_converter = None
def on_init(self):
# Map
self._map = Map()
self._map.load_from_file(self._map_file)
self._space_converter = geometry.SpaceConverter(
self._width / (self._map.meters_per_cell * self._map.width),
(self._map._global_origin_x, self._map._global_origin_y))
# Mbot
self._mbot = Mbot(self._map,
max_trans_speed=self._mbot_max_trans_speed,
max_angular_speed=self._mbot_max_angular_speed,
real_time_factor=self._real_time_factor)
# Lidar
self._lidar = Lidar(lambda at_time: self._mbot.get_pose(at_time),
self._map,
self._space_converter,
use_noise=self._use_noise,
dist_measure_sigma=self._lidar_dist_measure_sigma,
theta_step_sigma=self._lidar_theta_step_sigma,
num_ranges_noise=self._lidar_num_ranges_noise,
real_time_factor=self._real_time_factor)
# Pygame
pygame.init()
height = self._space_converter.to_pixel(self._map.height *
self._map.meters_per_cell)
pygame.display.set_mode((self._width, height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self._display_surf = pygame.display.get_surface()
self._display_surf.blit(self._map.render(self._space_converter),
(0, 0))
pygame.display.flip()
# Add sprites
if self._render_lidar:
self._sprites.add(self._lidar)
self._sprites.add(self._mbot)
# Start
self._lcm_thread.start()
self._lidar.start()
self._running = True
""" Controller """
def on_event(self, event):
if event.type == pygame.QUIT:
self._running = False
def on_loop(self):
# Publish odometry
pose = self._mbot.get_current_pose()
dpose = pose - self._last_pose
if self._use_noise and self._mbot.moving:
trans = numpy.sqrt(dpose.x**2 + dpose.y**2) + numpy.random.normal(
0, self._odom_trans_sigma)
dpose.theta += numpy.random.normal(0, self._odom_rot_sigma)
dpose.x = trans * numpy.cos(self._odom_pose.theta + dpose.theta)
dpose.y = trans * numpy.sin(self._odom_pose.theta + dpose.theta)
self._odom_pose += dpose
msg = odometry_t()
msg.x = self._odom_pose.x
msg.y = self._odom_pose.y
msg.theta = self._odom_pose.theta
msg.utime = int(time.perf_counter() * 1e6)
self._lcm.publish(self._odometry_channel, msg.encode())
# Update
self._last_pose = pose
def on_execute(self):
if self.on_init() is False:
self._running = False
while self._running:
with Rate(self._max_frame_rate,
real_time_factor=self._real_time_factor):
for event in pygame.event.get():
self.on_event(event)
self.on_loop()
self.on_render()
self.on_cleanup()
""" Controller LCM """
def _handle_lcm(self):
try:
while True:
with Rate(self._lcm_handle_rate):
self._lcm.handle_timeout(1000.0 / self._lcm_handle_rate)
except KeyboardInterrupt:
print("lcm exit!")
sys.exit()
def _motor_command_handler(self, channel, data):
msg = mbot_motor_command_t.decode(data)
# Override utime with sim time
msg.utime = int(time.perf_counter() * 1e6)
self._mbot.add_motor_cmd(msg)
""" View """
def on_render(self):
self._sprites.clear(self._display_surf, self._map.image)
self._sprites.update(self._space_converter)
self._sprites.draw(self._display_surf)
pygame.display.flip()
def on_cleanup(self):
self._lidar.stop()
pygame.quit()