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