def __init__(self):
self.logger = Logger()
self.logger.write("Robot: initializing")
self.logger.display("Starting...")
ports = usb_probe.probe()
self.logger.write("Robot: found USB ports...")
for port in ports:
self.logger.write(" %s, %s" % (ports[port], port))
self.speedometer = Speedometer(ports['speedometer'], 9600, self.logger)
self.compasswitch = Compasswitch(ports['compasswitch'], 9600,
self.logger)
self.powersteering = PowerSteering(ports['chias'], 9600, self.logger,
self.speedometer, self.compasswitch)
self.gps = GPS(ports['gps'], 4800, self.logger)
self.camera = Camera(9788, self.logger)
def __init__(self):
""" Constructs the World """
self.sky = SkyDome("Resources/Models/skydome.egg")
self.sky.sky.setScale(Vec3(10, 10, 10))
self.setupPhysX()
self.setup3DAudio()
self.car = Car(self.physxScene, self.audio3d, "defender.xml")
self.car.setActiveAudioProfile('outside')
self.setupLight()
self.initTrack()
taskMgr.add(self.simulate, 'PhysX Simulation')
# To to use Steering Wheel change inputHandler to SteeringControl
self.inputHandler = KeyControl(self.car)
#self.inputHandler = SteeringControl( self.car )
self.cameraControl = CameraControl(self.car)
self.cameraControl.enableTowerCamera()
self.speedometer = Speedometer()
render.setShaderAuto()
self.accept('escape', sys.exit)
def __init__(self):
""" Constructs the World """
self.sky = SkyDome( "Resources/Models/skydome.egg" )
self.sky.sky.setScale( Vec3( 10,10,10))
self.setupPhysX()
self.setup3DAudio()
self.car = Car( self.physxScene, self.audio3d, "defender.xml" )
self.car.setActiveAudioProfile( 'outside' )
self.setupLight()
self.initTrack()
taskMgr.add(self.simulate, 'PhysX Simulation')
# To to use Steering Wheel change inputHandler to SteeringControl
self.inputHandler = KeyControl( self.car )
#self.inputHandler = SteeringControl( self.car )
self.cameraControl = CameraControl( self.car )
self.cameraControl.enableTowerCamera()
self.speedometer = Speedometer();
render.setShaderAuto()
self.accept( 'escape', sys.exit )
class World( DirectObject ):
""" Dynamic world """
def __init__(self):
""" Constructs the World """
self.sky = SkyDome( "Resources/Models/skydome.egg" )
self.sky.sky.setScale( Vec3( 10,10,10))
self.setupPhysX()
self.setup3DAudio()
self.car = Car( self.physxScene, self.audio3d, "defender.xml" )
self.car.setActiveAudioProfile( 'outside' )
self.setupLight()
self.initTrack()
taskMgr.add(self.simulate, 'PhysX Simulation')
# To to use Steering Wheel change inputHandler to SteeringControl
self.inputHandler = KeyControl( self.car )
#self.inputHandler = SteeringControl( self.car )
self.cameraControl = CameraControl( self.car )
self.cameraControl.enableTowerCamera()
self.speedometer = Speedometer();
render.setShaderAuto()
self.accept( 'escape', sys.exit )
# To turn on physx visual debugging, uncomment below
#self.enablePhysxDebug()
def initTrack(self):
""" Loads the track model and the collision model for it. """
kitchen = PhysxKitchen()
trackCollision = loader.loadModel( "Resources/Models/TrackCollision.egg" )
fenceCollision = loader.loadModel( "Resources/Models/FenceCollision.egg")
self.track = loader.loadModel( "Resources/Models/Track.egg" )
triMeshDesc = PhysxTriangleMeshDesc()
triMeshDesc.setFromNodePath( trackCollision )
triMesh = kitchen.cookTriangleMesh( triMeshDesc )
triMeshShapeDesc = PhysxTriangleMeshShapeDesc()
triMeshShapeDesc.setMesh( triMesh )
triMeshDesc2 = PhysxTriangleMeshDesc()
triMeshDesc2.setFromNodePath( fenceCollision )
triMesh2 = kitchen.cookTriangleMesh( triMeshDesc2 )
triMeshShapeDesc2 = PhysxTriangleMeshShapeDesc()
triMeshShapeDesc2.setMesh( triMesh2 )
actor = PhysxActorDesc()
actor.setName( 'trackcollision' )
actor.addShape( triMeshShapeDesc )
actor.addShape( triMeshShapeDesc2 )
self.physxtrack = self.physxScene.createActor( actor )
self.track.reparentTo( render )
loader.loadModel( "Resources/Models/Fence.egg" ).reparentTo( self.track )
loader.loadModel( "Resources/Models/Rocks.egg" ).reparentTo( self.track )
linfog = Fog( "Fog" )
linfog.setColor( Vec4( 0.8, 0.85, 0.8, 1 ) )
linfog.setExpDensity( 0.003 )
self.track.attachNewNode(linfog)
render.setFog(linfog)
def enablePhysxDebug(self):
""" Turns on physx visual debuggging """
self.debugNP = render.attachNewNode(self.physxScene.getDebugGeomNode())
self.debugNP.node().on()
self.debugNP.node().visualizeWorldAxes(True)
def setupPhysX(self):
""" Sets up the physx world """
self.physx = PhysxManager.getGlobalPtr()
sceneDesc = PhysxSceneDesc()
sceneDesc.setGravity(Vec3(0, 0, -9.81))
self.physxScene = self.physx.createScene(sceneDesc)
mGround = self.physxScene.getMaterial( 0 )
mGround.setRestitution(0.0)
mGround.setStaticFriction(0.8)
mGround.setDynamicFriction(0.2)
def setup3DAudio(self):
""" Initializes the 3D audio manager """
self.audio3d = Audio3DManager( base.sfxManagerList[0], base.cam )
def setupLight(self):
""" Sets up the scene lighting """
ambient_source = AmbientLight('ambient')
ambient_source.setColor(Vec4( 0.6, 0.65, 0.7, 1 ))
ambient = render.attachNewNode(ambient_source.upcastToPandaNode())
render.setLight( ambient )
sun = render.attachNewNode( DirectionalLight( 'sun' ) )
sun.node().setScene( render )
render.setLight( sun )
sun.reparentTo( self.car.chassisModel )
sun.setH( -60 )
sun.setP( -60 )
sun.setPos( 0, 0, 10 )
sun.node().getLens().setFov( 70 )
sun.node().getLens().setNearFar( 1, 20 )
sun.node().getLens().setFilmSize( 16, 16 )
sun.node().setColor( Vec4( 1, 0.96, 1, 1 ))
sun.node().setShadowCaster( True )
self.sun = sun
def simulate(self, task):
""" Simulation loop, called every frame """
dt = globalClock.getDt()
self.physxScene.simulate(dt)
self.physxScene.fetchResults()
self.car.simulate(dt)
self.cameraControl.simulate(dt)
self.sun.setH( render, -60 )
self.sun.setP( render, -60 )
self.speedometer.updateSpeedometer( self.car.speed )
self.inputHandler.simulate( dt )
return task.cont
class Robot:
def __init__(self):
self.logger = Logger()
self.logger.write("Robot: initializing")
self.logger.display("Starting...")
ports = usb_probe.probe()
self.logger.write("Robot: found USB ports...")
for port in ports:
self.logger.write(" %s, %s" % (ports[port], port))
self.speedometer = Speedometer(ports['speedometer'], 9600, self.logger)
self.compasswitch = Compasswitch(ports['compasswitch'], 9600,
self.logger)
self.powersteering = PowerSteering(ports['chias'], 9600, self.logger,
self.speedometer, self.compasswitch)
self.gps = GPS(ports['gps'], 4800, self.logger)
self.camera = Camera(9788, self.logger)
def initialize(self,
speedometer=True,
compasswitch=True,
powersteering=True,
gps=True,
camera=True):
self.logger.write("Robot: initializing")
self.logger.display("Initializing...")
if speedometer == True:
self.speedometer_thread = Thread(target=self.speedometer.run)
self.speedometer_thread.start()
if compasswitch == True:
self.compasswitch_thread = Thread(target=self.compasswitch.run)
self.compasswitch_thread.start()
if powersteering == True:
self.powersteering_thread = Thread(target=self.powersteering.run)
self.powersteering_thread.start()
if gps == True:
self.gps_thread = Thread(target=self.gps.run)
self.gps_thread.start()
if camera == True:
self.camera_thread = Thread(target=self.camera.run)
self.camera_thread.start()
def terminate(self):
self.logger.write("Robot: terminating")
self.logger.display("Terminating...")
self.speedometer.terminate()
self.compasswitch.terminate()
self.powersteering.terminate()
self.gps.terminate()
self.camera.terminate()
def wait_for_start_switch(self):
self.logger.write("Press start")
self.logger.display("Press start")
while self.compasswitch.start_switch == False:
pass
self.logger.write("Button Pressed")
self.logger.display("Button Pressed")
def wait_for_gps(self):
self.logger.display("Waiting for GPS")
self.logger.write("Waiting for GPS")
while self.gps.got_fix() == False:
pass
self.logger.display("Got GPS Fix")
self.logger.write("Got GPS Fix")
def set_power_and_steering(self, power_value, steer_value):
self.powersteering.set_power_and_steering(power_value, steer_value)
def set_speed_and_direction(self, speed, direction):
self.powersteering.set_speed_and_direction(speed, direction)
def drive_to_waypoint(self,
target_lat,
target_lon,
speed,
accuracy=GPS_ERROR_RADIUS):
self.logger.write(
"Called drive_to_waypoint: lat=%0.5f, lon=%0.5f, speed=%0.1f" %
(target_lat, target_lon, speed))
(distance,
bearing) = utils.get_distance_and_bearing(self.gps.get_latitude(),
self.gps.get_longitude(),
target_lat, target_lon)
while distance > accuracy:
self.logger.write(
"Drive_to_waypoint: distance=%0.2f, bearing=%0.1f, tgt_speed=%0.2f"
% (distance, bearing, speed))
self.logger.write(
"Drive_to_waypoint: actual_speed=%0.2f, power=%d" %
(self.speedometer.get_speed(), self.powersteering.get_power()))
self.logger.display("D2W %0.1f, %05.1f" % (distance, bearing))
self.logger.display(
"D2W %0.1f, %d" %
(self.speedometer.get_speed(), self.powersteering.get_power()))
self.powersteering.set_speed_and_direction(speed, bearing)
time.sleep(0.5)
(distance, bearing) = utils.get_distance_and_bearing(
self.gps.get_latitude(), self.gps.get_longitude(), target_lat,
target_lon)
self.logger.write("Drive_to_waypoint: arrived, distance = %o.2f" %
distance)
def drive_to_cone(self,
target_lat,
target_lon,
tgt_speed,
camera_speed=1.1,
gps_accuracy=GPS_ERROR_RADIUS,
timeout=3600):
self.logger.write(
"Called drive to cone: lat=%0.5f, lon=%0.5f, tgt_speed=%0.5f, camera_speed=%0.5f, gps_accuracy = %0.5f, timeout=%d"
% (target_lat, target_lon, tgt_speed, camera_speed, gps_accuracy,
timeout))
start_time = time.time()
camera_mode = False
while (time.time() - start_time < timeout):
# Get the robot pose
(distance, bearing) = utils.get_distance_and_bearing(
self.gps.get_latitude(), self.gps.get_longitude(), target_lat,
target_lon)
compass = self.compasswitch.get_heading()
speed = self.speedometer.get_speed()
(blob_location, blob_size) = self.camera.get_blob_info()
bump_switch = self.compasswitch.get_bump_switch()
self.logger.write(
"D2C: dst=%0.5f, head=%0.5f, compass=%0.5f, tgt_speed=%0.5f, speed=%0.5f, blob_loc=%d, blob_size=%d, bump=%s"
% (distance, bearing, compass, tgt_speed, speed, blob_location,
blob_size, bump_switch))
# GPS mode
if not camera_mode:
if not bump_switch:
if distance > gps_accuracy:
# continue to drive to waypoint
self.logger.write("D2C GPS mode")
self.logger.display("D2C %0.1f, %05.1f" %
(distance, bearing))
self.logger.display("D2C %0.1f, %d" %
(self.speedometer.get_speed(),
self.powersteering.get_power()))
self.powersteering.set_speed_and_direction(
tgt_speed, bearing)
else:
# set to camera mode
camera_mode = True
else:
# deal with collision
pass
# Camera mode
else:
if distance < gps_accuracy:
if bump_switch == False:
# Still looking for cone
self.logger.write("D2C: Camera mode")
if blob_size == 0:
blob_location = 32 #Drive Straight if no pixels
self.powersteering.set_steering(
int((blob_location) - 32) * 10)
self.powersteering.set_speed(camera_speed)
else:
# Hit the cone (presumably!)
self.powersteering.set_power_and_steering(0, 0)
self.logger.write("D2C: Hit cone!!!")
return
else:
# set back to GPS mode
self.logger.write(
"D2C: left camera mode - GPS distance too great")
camera_mode = False
time.sleep(0.1)
else:
self.logger.write("D2C: timed out")
def backup_to_compass(self,
target_heading,
speed=-1,
steer=400,
accuracy=15,
timeout=10):
self.logger.write(
"Back up to Compass: target = %d , power = %d , steer = %d , accuracy = %d , timeout = %d"
% (target_heading, speed, steer, accuracy, timeout))
delta_angle = target_heading - self.compasswitch.get_heading()
if delta_angle > 180:
delta_angle = int(delta_angle + 360)
elif delta_angle < -180:
delta_angle = int(delta_angle - 360)
else:
delta_angle = int(delta_angle)
if delta_angle > 0:
steering = -steer
elif delta_angle < 0:
steering = steer
else:
return
start_time = time.time()
self.powersteering.set_steering(steering)
self.powersteering.set_speed(speed)
while abs(target_heading - self.compasswitch.get_heading()) > accuracy:
self.logger.display(
"B2C: tgt=%d cur=%d" %
(target_heading, self.compasswitch.get_heading()))
if time.time() - start_time > timeout:
self.logger.write("B2C - Timed Out")
break
self.powersteering.set_power_and_steering(0, 0)
def backup_to_waypoint(self, target_lat, target_lon):
(distance,
bearing) = utils.get_distance_and_bearing(self.gps.get_latitude(),
self.gps.get_longitude(),
target_lat, target_lon)
self.backup_to_compass(bearing)
def main():
speedometer = Speedometer()
client = Client()
client.manage_client()
client.send_requests(speedometer)
class World(DirectObject):
""" Dynamic world """
def __init__(self):
""" Constructs the World """
self.sky = SkyDome("Resources/Models/skydome.egg")
self.sky.sky.setScale(Vec3(10, 10, 10))
self.setupPhysX()
self.setup3DAudio()
self.car = Car(self.physxScene, self.audio3d, "defender.xml")
self.car.setActiveAudioProfile('outside')
self.setupLight()
self.initTrack()
taskMgr.add(self.simulate, 'PhysX Simulation')
# To to use Steering Wheel change inputHandler to SteeringControl
self.inputHandler = KeyControl(self.car)
#self.inputHandler = SteeringControl( self.car )
self.cameraControl = CameraControl(self.car)
self.cameraControl.enableTowerCamera()
self.speedometer = Speedometer()
render.setShaderAuto()
self.accept('escape', sys.exit)
# To turn on physx visual debugging, uncomment below
#self.enablePhysxDebug()
def initTrack(self):
""" Loads the track model and the collision model for it. """
kitchen = PhysxKitchen()
trackCollision = loader.loadModel(
"Resources/Models/TrackCollision.egg")
fenceCollision = loader.loadModel(
"Resources/Models/FenceCollision.egg")
self.track = loader.loadModel("Resources/Models/Track.egg")
triMeshDesc = PhysxTriangleMeshDesc()
triMeshDesc.setFromNodePath(trackCollision)
triMesh = kitchen.cookTriangleMesh(triMeshDesc)
triMeshShapeDesc = PhysxTriangleMeshShapeDesc()
triMeshShapeDesc.setMesh(triMesh)
triMeshDesc2 = PhysxTriangleMeshDesc()
triMeshDesc2.setFromNodePath(fenceCollision)
triMesh2 = kitchen.cookTriangleMesh(triMeshDesc2)
triMeshShapeDesc2 = PhysxTriangleMeshShapeDesc()
triMeshShapeDesc2.setMesh(triMesh2)
actor = PhysxActorDesc()
actor.setName('trackcollision')
actor.addShape(triMeshShapeDesc)
actor.addShape(triMeshShapeDesc2)
self.physxtrack = self.physxScene.createActor(actor)
self.track.reparentTo(render)
loader.loadModel("Resources/Models/Fence.egg").reparentTo(self.track)
loader.loadModel("Resources/Models/Rocks.egg").reparentTo(self.track)
linfog = Fog("Fog")
linfog.setColor(Vec4(0.8, 0.85, 0.8, 1))
linfog.setExpDensity(0.003)
self.track.attachNewNode(linfog)
render.setFog(linfog)
def enablePhysxDebug(self):
""" Turns on physx visual debuggging """
self.debugNP = render.attachNewNode(self.physxScene.getDebugGeomNode())
self.debugNP.node().on()
self.debugNP.node().visualizeWorldAxes(True)
def setupPhysX(self):
""" Sets up the physx world """
self.physx = PhysxManager.getGlobalPtr()
sceneDesc = PhysxSceneDesc()
sceneDesc.setGravity(Vec3(0, 0, -9.81))
self.physxScene = self.physx.createScene(sceneDesc)
mGround = self.physxScene.getMaterial(0)
mGround.setRestitution(0.0)
mGround.setStaticFriction(0.8)
mGround.setDynamicFriction(0.2)
def setup3DAudio(self):
""" Initializes the 3D audio manager """
self.audio3d = Audio3DManager(base.sfxManagerList[0], base.cam)
def setupLight(self):
""" Sets up the scene lighting """
ambient_source = AmbientLight('ambient')
ambient_source.setColor(Vec4(0.6, 0.65, 0.7, 1))
ambient = render.attachNewNode(ambient_source.upcastToPandaNode())
render.setLight(ambient)
sun = render.attachNewNode(DirectionalLight('sun'))
sun.node().setScene(render)
render.setLight(sun)
sun.reparentTo(self.car.chassisModel)
sun.setH(-60)
sun.setP(-60)
sun.setPos(0, 0, 10)
sun.node().getLens().setFov(70)
sun.node().getLens().setNearFar(1, 20)
sun.node().getLens().setFilmSize(16, 16)
sun.node().setColor(Vec4(1, 0.96, 1, 1))
sun.node().setShadowCaster(True)
self.sun = sun
def simulate(self, task):
""" Simulation loop, called every frame """
dt = globalClock.getDt()
self.physxScene.simulate(dt)
self.physxScene.fetchResults()
self.car.simulate(dt)
self.cameraControl.simulate(dt)
self.sun.setH(render, -60)
self.sun.setP(render, -60)
self.speedometer.updateSpeedometer(self.car.speed)
self.inputHandler.simulate(dt)
return task.cont
def __init__(self):
self.cmdPub = rospy.Publisher('/clearpath/robots/default/cmd_vel', Twist)
self.estPub = rospy.Publisher('state_est', StateEstimate)
self.spd = Speedometer(self._velocityCb)
rospy.Subscriber('indoor_pos', ips_msg, self._poseCb)
self.running = False
self.ts = 0.05 # Sample time [s]
self.length = 0.265 # Robot length [m]
# Velocity control state:
# Proportional control constants
self.kp = 143.9
self.ki = 857.8
self.integrator = 0.0
self.lastErr = 0.0 # (needed for trapezoidal integrator)
# Reference and control signals:
self.velRef = None # [m/s]
self.velCtrl = 0.0 # [-100,100]
# Record values for analysis
self.stateRecord = []
self.msmtRecord = []
self.outputRecord = []
# Proportional control constant for steering angle.
self.kSteer = 0.5
# Next waypoint
self.xRef = None
self.yRef = None
self.hRef = None
# Steering reference and control signals:
self.steerRef = None # [-0.4,0.4] [rad]
self.steerCtrl = None # [-100,100]
self.setSteeringAngle(0.0) # initialize sensibly
# EKF State:
# Latest measurements. Set to None when consumed for EKF estimate.
self.lastPose = None
self.lastVel = None
# Process covariance:
self.Q = np.matrix([[0.0020, 0, 0, 0],
[0, 0.15, 0, 0],
[0, 0, 0.07, 0],
[0, 0, 0, 0.07]])
# Measurement covariance:
self.R = np.matrix([[0.0020, 0, 0, 0],
[0, 0.0001, 0, 0],
[0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]])
# Zero initial state
self.stateEst = np.matrix(np.zeros((4,1)))
self.velOutDelay = [0, 0, 0, 0]
# ... with low confidence
self.P = np.matrix(np.ones((4,4)))
# Velocity model params:
Km = 0.003267973309
am = 6.779750386209
self.velNum = (Km*am*self.ts)/(am*self.ts + 2)
self.velDen = (am*self.ts-2)/(am*self.ts+2)
def detect_on_video(video_path,
out_path,
detector,
tracker,
coef_file='coef.json',
mask_file='mask.png',
to_mp4=False,
class_names=None):
if class_names is None:
class_names = [
'car', 'minibus', 'trolleybus', 'tram', 'truck', 'bus',
'middle_bus', 'ambulance', 'fire_truck', 'middle_truck', 'tractor',
'uncategorized', 'van', 'person'
]
# Создаём считыватель исходного видео
istream = cv2.VideoCapture(video_path)
# Получаем данные о исходном видео
w = int(istream.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(istream.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = int(istream.get(cv2.CAP_PROP_FPS))
frame_count = int(istream.get(cv2.CAP_PROP_FRAME_COUNT))
# Создаём писателя для результирующего видео
if to_mp4:
fourcc = cv2.VideoWriter_fourcc(*'XVID')
else:
fourcc = cv2.VideoWriter_fourcc(*'VP80')
writer = cv2.VideoWriter(out_path, fourcc, fps, (w, h), True)
# Создаём экземпляр класса Masker для выделения проезжей части на каждом кадре
masker = Masker(mask_file, (w, h))
# Создаём трекер для отслеживания автомобилей на разных кадрах
tracker = Sort()
speedometer = Speedometer(coef_file, fps=fps, size=(w, h))
# Обрабатываем видео покадрово
for frame_id in tqdm(range(frame_count)):
# Считываем кадр, создаём кадр для видео с результатом
ret, frame = istream.read()
if not ret:
break
else:
out_frame = frame
# Выделяем проезжую часть
masked_frame = masker.apply(frame)
# Распознаём объекты на кадре
outputs = detector(masked_frame)
# Получаем bbox (рамки) и вероятность принадлежности классу для каждого найденного объекта
boxes = outputs['instances'].pred_boxes.tensor.to("cpu").numpy()
scores = outputs['instances'].scores.to("cpu").numpy()
classes = outputs['instances'].pred_classes.to("cpu").numpy()
# Обновляем трекер, получаем track_id (id объекта на предыдущих кадрах) для каждого найденного объекта
for_tracker = np.concatenate([boxes, scores[:, None]], axis=1)
dets, associaties = tracker.update(for_tracker, make_associaties=True)
speeds = spmeter.update(dets)
for i in range(scores.shape[0]):
box = boxes[i].astype(np.int16)
track_id = associaties[i]
if track_id == 0: continue
speed = spmeter.ms_to_kmh(speeds[track_id])
class_id = classes[i]
class_label = class_names[class_id]
draw_box(out_frame, box)
draw_label(out_frame, track_id, box[:2], size=2, shadow=True)
draw_label(out_frame,
round(speed, 2), (box[0], box[1] + 10),
color=(255, 100, 100),
shadow=True)
draw_label(out_frame,
class_label, (box[0], box[1] + 25),
color=(100, 255, 100),
shadow=True)
# Добавляем кадр с разметкой к результирующему видео
writer.write(out_frame)
# Сохраняем видео с результатом
writer.release()
return True
