def render(self, mode='human'): if not (self.play_mode == True): raise Exception('Please set play_mode=True to render') if not self.render_setup: vis = True save = False birds_eye = False third_person = True width = 1280 height = 720 if birds_eye: body = chrono.ChBodyAuxRef() body.SetBodyFixed(True) self.system.AddBody(body) vis_camera = sens.ChCameraSensor( body, # body camera is attached to 20, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(0, 0, 200), chrono.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))), # offset pose width, # number of horizontal samples height, # number of vertical channels chrono.CH_C_PI / 3 # horizontal field of view ) vis_camera.SetName("Birds Eye Camera Sensor") if vis: vis_camera.PushFilter(sens.ChFilterVisualize(width, height, "Visualization Camera")) if save: vis_camera.PushFilter(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) if third_person: vis_camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 20, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(-8, 0, 3), chrono.Q_from_AngAxis(chrono.CH_C_PI / 20, chrono.ChVectorD(0, 1, 0))), # offset pose width, # number of horizontal samples height, # number of vertical channels chrono.CH_C_PI / 3 # horizontal field of view ) vis_camera.SetName("Follow Camera Sensor") if vis: vis_camera.PushFilter(sens.ChFilterVisualize(width, height, "Visualization Camera")) if save: vis_camera.PushFilter(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) # ----------------------------------------------------------------- # Create a filter graph for post-processing the data from the lidar # ----------------------------------------------------------------- # self.camera.PushFilter(sens.ChFilterVisualize("RGB Camera")) # vis_camera.PushFilter(sens.ChFilterVisualize("Visualization Camera")) self.render_setup = True if (mode == 'rgb_array'): return self.get_ob()
def render(self, mode='human'): if not (self.play_mode == True): raise Exception('Please set play_mode=True to render') if not self.render_setup: if False: vis_camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 30, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(0, 0, 25), chrono.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0))), # offset pose 1280, # number of horizontal samples 720, # number of vertical channels chrono.CH_C_PI / 3, # horizontal field of view (720 / 1280) * chrono.CH_C_PI / 3. # vertical field of view ) vis_camera.SetName("Birds Eye Camera Sensor") # self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera")) if False: self.camera.FilterList().append(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) if True: vis_camera = sens.ChCameraSensor( self.chassis_body, # body camera is attached to 30, # scanning rate in Hz chrono.ChFrameD( chrono.ChVectorD(-8, 0, 3), chrono.Q_from_AngAxis(chrono.CH_C_PI / 20, chrono.ChVectorD(0, 1, 0))), # offset pose 1280, # number of horizontal samples 720, # number of vertical channels chrono.CH_C_PI / 3, # horizontal field of view (720 / 1280) * chrono.CH_C_PI / 3. # vertical field of view ) vis_camera.SetName("Follow Camera Sensor") # self.camera.FilterList().append(sens.ChFilterVisualize(self.camera_width, self.camera_height, "RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize(1280, 720, "Visualization Camera")) if True: vis_camera.FilterList().append(sens.ChFilterSave()) # self.camera.FilterList().append(sens.ChFilterSave()) self.manager.AddSensor(vis_camera) # ----------------------------------------------------------------- # Create a filter graph for post-processing the data from the lidar # ----------------------------------------------------------------- # self.camera.FilterList().append(sens.ChFilterVisualize("RGB Camera")) # vis_camera.FilterList().append(sens.ChFilterVisualize("Visualization Camera")) self.render_setup = True if (mode == 'rgb_array'): return self.get_ob()
def __init__(self, system: 'WAChronoSystem', vehicle: 'WAChronoVehicle', vehicle_inputs: 'WAVehicleInputs', environment: 'WAEnvironment' = None, opponents: list = [], record: bool = False, record_folder: str = "OUTPUT/"): self._render_steps = int( ceil(system.render_step_size / system.step_size)) self._system = system self._vehicle_inputs = vehicle_inputs self._record = record self._record_folder = record_folder self._manager = WAChronoSensorManager(system) self._manager._manager.scene.AddPointLight( chrono.ChVectorF(0, 0, 100), chrono.ChVectorF(2, 2, 2), 5000) update_rate = 30. offset_pose = chrono.ChFrameD(chrono.ChVectorD(-8, 0, 2)) image_width = 1280 image_height = 720 fov = 1.408 cam = sens.ChCameraSensor( vehicle._vehicle.GetChassisBody( ), # body camera is attached to update_rate, # update rate in Hz offset_pose, # offset pose image_width, # image width image_height, # image height fov # camera's horizontal field of view ) cam.PushFilter(sens.ChFilterVisualize(image_width, image_height)) if self._record: cam.PushFilter(sens.ChFilterSave(self._record_folder)) self._manager._manager.AddSensor(cam) if environment is not None: self.visualize(environment.get_assets())
#field of view: fov = 1.408 camera_3rd = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 60, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(-2, 0, 1), chrono.Q_from_AngAxis(.3, chrono.ChVectorD(0, 1, 0))), # offset pose 1920*2, # number of horizontal samples 1080*2, # number of vertical channels chrono.CH_C_PI / 4 # horizontal field of view # vertical field of view ) camera_3rd.SetName("Camera Sensor") if(visualize): camera_3rd.PushFilter(sens.ChFilterVisualize(1280,720,"Third Person Camera")) if(save_data): camera_3rd.PushFilter(sens.ChFilterSave(1280,720,"output/iros/third_person_camera/")) manager.AddSensor(camera_3rd) camera_front = sens.ChCameraSensor( my_rccar.GetChassisBody(), # body lidar is attached to 60, # scanning rate in Hz chrono.ChFrameD(chrono.ChVectorD(0, 0, .2), chrono.Q_from_AngAxis(0, chrono.ChVectorD(1, 0, 0))), # offset pose 210*2, # number of horizontal samples 160*2, # number of vertical channels fov # camera's horizontal field of view ) camera_front.SetName("Camera Sensor") camera_front.PushFilter(sens.ChFilterImgAlias(2)) if(visualize):
def main(): # ----------------- # Create the system # ----------------- mphysicalSystem = chrono.ChSystemNSC() mphysicalSystem.Set_G_acc(chrono.ChVectorD(0, 0, -9.81)) # ---------------------------------------- # add a floor, box and sphere to the scene # ---------------------------------------- phys_mat = chrono.ChMaterialSurfaceNSC() phys_mat.SetFriction(0.5) phys_mat.SetDampingF(0.00000) phys_mat.SetCompliance(1e-9) phys_mat.SetComplianceT(1e-9) floor = chrono.ChBodyEasyBox(10, 10, 1, 1000, True, True, phys_mat) floor.SetPos(chrono.ChVectorD(0, 0, -1)) floor.SetBodyFixed(True) mphysicalSystem.Add(floor) box = chrono.ChBodyEasyBox(1, 1, 1, 1000, True, True, phys_mat) box.SetPos(chrono.ChVectorD(0, 0, 5)) box.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0))) mphysicalSystem.Add(box) sphere = chrono.ChBodyEasySphere(.5, 1000, True, True, phys_mat) sphere.SetPos(chrono.ChVectorD(0, 0, 8)) sphere.SetRot(chrono.Q_from_AngAxis(.2, chrono.ChVectorD(1, 0, 0))) mphysicalSystem.Add(sphere) sphere_asset = sphere.GetAssets()[0] visual_asset = chrono.CastToChVisualization(sphere_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0)) vis_mat.SetDiffuseColor(chrono.ChVectorF(.2, .2, .9)) vis_mat.SetSpecularColor(chrono.ChVectorF(.9, .9, .9)) visual_asset.material_list.append(vis_mat) # ----------------------- # Create a sensor manager # ----------------------- manager = sens.ChSensorManager(mphysicalSystem) manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100), chrono.ChVectorF(1, 1, 1), 1000.0) manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100), chrono.ChVectorF(1, 1, 1), 1000.0) # ------------------------------------------------ # Create a camera and add it to the sensor manager # ------------------------------------------------ offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) cam = sens.ChCameraSensor( floor, # body camera is attached to cam_update_rate, # update rate in Hz offset_pose, # offset pose image_width, # number of horizontal samples image_height, # number of vertical channels cam_fov # vertical field of view ) cam.SetName("Camera Sensor") cam.SetLag(cam_lag) cam.SetCollectionWindow(cam_collection_time) # ------------------------------------------------------------------ # Create a filter graph for post-processing the data from the camera # ------------------------------------------------------------------ # Visualizes the image if vis: cam.PushFilter( sens.ChFilterVisualize(image_width, image_height, "RGB Image")) # Save the current image to a png file at the specified path if (save): cam.PushFilter(sens.ChFilterSave(out_dir + "/rgb/")) # Provides the host access to this RGBA8 buffer cam.PushFilter(sens.ChFilterRGBA8Access()) # Filter the sensor to grayscale cam.PushFilter(sens.ChFilterGrayscale()) # Render the buffer again to see the new grayscaled image if (vis): cam.PushFilter( sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2), "Grayscale Image")) # Save the grayscaled image at the specified path if (save): cam.PushFilter(sens.ChFilterSave(out_dir + "/gray/")) # Access the grayscaled buffer as R8 pixels cam.PushFilter(sens.ChFilterR8Access()) # Add a camera to a sensor manager manager.AddSensor(cam) # ------------------------------------------------ # Create a lidar and add it to the sensor manager # ------------------------------------------------ offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) lidar = sens.ChLidarSensor( floor, # body lidar is attached to lidar_update_rate, # scanning rate in Hz offset_pose, # offset pose horizontal_samples, # number of horizontal samples vertical_samples, # number of vertical channels horizontal_fov, # horizontal field of view max_vert_angle, min_vert_angle, # vertical field of view 100 #max lidar range ) lidar.SetName("Lidar Sensor") lidar.SetLag(lidar_lag) lidar.SetCollectionWindow(lidar_collection_time) # ----------------------------------------------------------------- # Create a filter graph for post-processing the data from the lidar # ----------------------------------------------------------------- if vis: # Randers the raw lidar data lidar.PushFilter( sens.ChFilterVisualize(horizontal_samples, vertical_samples, "Raw Lidar Depth Data")) # Provides the host access to the Depth,Intensity data lidar.PushFilter(sens.ChFilterDIAccess()) # Convert Depth,Intensity data to XYZI point cloud data lidar.PushFilter(sens.ChFilterPCfromDepth()) if vis: # Visualize the point cloud lidar.PushFilter( sens.ChFilterVisualizePointCloud(640, 480, 1.0, "Lidar Point Cloud")) # Provides the host access to the XYZI data lidar.PushFilter(sens.ChFilterXYZIAccess()) # Add the lidar to the sensor manager manager.AddSensor(lidar) # ---------------------------------------------- # Create an IMU sensor and add it to the manager # ---------------------------------------------- offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) imu = sens.ChIMUSensor( box, # body imu is attached to imu_update_rate, # update rate in Hz offset_pose, # offset pose imu_noise_none, # noise model ) imu.SetName("IMU Sensor") imu.SetLag(imu_lag) imu.SetCollectionWindow(imu_collection_time) # Provides the host access to the imu data imu.PushFilter(sens.ChFilterIMUAccess()) # Add the imu to the sensor manager manager.AddSensor(imu) # ---------------------------------------------- # Create an GPS sensor and add it to the manager # ---------------------------------------------- offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) gps = sens.ChGPSSensor( box, # body imu is attached to gps_update_rate, # update rate in Hz offset_pose, # offset pose gps_reference, gps_noise_none # noise model ) gps.SetName("GPS Sensor") gps.SetLag(gps_lag) gps.SetCollectionWindow(gps_collection_time) # Provides the host access to the gps data gps.PushFilter(sens.ChFilterGPSAccess()) # Add the gps to the sensor manager manager.AddSensor(gps) # --------------- # Simulate system # --------------- t1 = time.time() ch_time = 0 while (ch_time < end_time): # Access the sensor data camera_data_RGBA8 = cam.GetMostRecentRGBA8Buffer() camera_data_R8 = cam.GetMostRecentR8Buffer() lidar_data_DI = lidar.GetMostRecentDIBuffer() lidar_data_XYZI = lidar.GetMostRecentXYZIBuffer() gps_data = gps.GetMostRecentGPSBuffer() imu_data = imu.GetMostRecentIMUBuffer() # Check data is present # If so, print out the max value if camera_data_RGBA8.HasData(): print("Camera RGBA8:", camera_data_RGBA8.GetRGBA8Data().shape, "max:", np.max(camera_data_RGBA8.GetRGBA8Data())) if camera_data_R8.HasData(): print("Camera R8:", camera_data_R8.GetChar8Data().shape, "max:", np.max(camera_data_R8.GetChar8Data())) if lidar_data_DI.HasData(): print("Lidar DI:", lidar_data_DI.GetDIData().shape, "max:", np.max(lidar_data_DI.GetDIData())) if lidar_data_XYZI.HasData(): print("Lidar XYZI:", lidar_data_XYZI.GetXYZIData().shape, "max:", np.max(lidar_data_XYZI.GetXYZIData())) if gps_data.HasData(): print("GPS:", gps_data.GetGPSData().shape, "max:", np.max(gps_data.GetGPSData())) if imu_data.HasData(): print("IMU:", imu_data.GetIMUData().shape, "max:", np.max(imu_data.GetIMUData())) # Update sensor manager # Will render/save/filter automatically manager.Update() # Perform step of dynamics mphysicalSystem.DoStepDynamics(step_size) # Get the current time of the simulation ch_time = mphysicalSystem.GetChTime() print("Sim time:", end_time, "Wall time:", time.time() - t1)
def main(): # ----------------- # Create the system # ----------------- mphysicalSystem = chrono.ChSystemNSC() # ----------------------------------- # add a mesh to be sensed by a camera # ----------------------------------- mmesh = chrono.ChTriangleMeshConnected() mmesh.LoadWavefrontMesh( chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False, True) # scale to a different size mmesh.Transform(chrono.ChVectorD(0, 0, 0), chrono.ChMatrix33D(2)) trimesh_shape = chrono.ChTriangleMeshShape() trimesh_shape.SetMesh(mmesh) trimesh_shape.SetName("HMMWV Chassis Mesh") trimesh_shape.SetStatic(True) mesh_body = chrono.ChBody() mesh_body.SetPos(chrono.ChVectorD(0, 0, 0)) mesh_body.AddAsset(trimesh_shape) mesh_body.SetBodyFixed(True) mphysicalSystem.Add(mesh_body) # ----------------------- # Create a sensor manager # ----------------------- manager = sens.ChSensorManager(mphysicalSystem) intensity = 1.0 manager.scene.AddPointLight( chrono.ChVectorF(2, 2.5, 100), chrono.ChVectorF(intensity, intensity, intensity), 500.0) manager.scene.AddPointLight( chrono.ChVectorF(9, 2.5, 100), chrono.ChVectorF(intensity, intensity, intensity), 500.0) manager.scene.AddPointLight( chrono.ChVectorF(16, 2.5, 100), chrono.ChVectorF(intensity, intensity, intensity), 500.0) manager.scene.AddPointLight( chrono.ChVectorF(23, 2.5, 100), chrono.ChVectorF(intensity, intensity, intensity), 500.0) # manager.SetKeyframeSizeFromTimeStep(.001,1/exposure_time) # ------------------------------------------------ # Create a camera and add it to the sensor manager # ------------------------------------------------ offset_pose = chrono.ChFrameD( chrono.ChVectorD(-5, 0, 2), chrono.Q_from_AngAxis(2, chrono.ChVectorD(0, 1, 0))) cam = sens.ChCameraSensor( mesh_body, # body camera is attached to update_rate, # update rate in Hz offset_pose, # offset pose image_width, # image width image_height, # image height fov # camera's horizontal field of view ) cam.SetName("Camera Sensor") cam.SetLag(lag) cam.SetCollectionWindow(exposure_time) # ------------------------------------------------------------------ # Create a filter graph for post-processing the data from the camera # ------------------------------------------------------------------ if noise_model == "CONST_NORMAL": cam.PushFilter(sens.ChFilterCameraNoiseConstNormal(0.0, 0.02)) elif noise_model == "PIXEL_DEPENDENT": cam.PushFilter(sens.ChFilterCameraNoisePixDep(0, 0.02, 0.03)) elif noise_model == "NONE": # Don't add any noise models pass # Renders the image at current point in the filter graph if vis: cam.PushFilter( sens.ChFilterVisualize(image_width, image_height, "Before Grayscale Filter")) # Provides the host access to this RGBA8 buffer cam.PushFilter(sens.ChFilterRGBA8Access()) # Save the current image to a png file at the specified path if save: cam.PushFilter(sens.ChFilterSave(out_dir + "rgb/")) # Filter the sensor to grayscale cam.PushFilter(sens.ChFilterGrayscale()) # Render the buffer again to see the new grayscaled image if vis: cam.PushFilter( sens.ChFilterVisualize(int(image_width / 2), int(image_height / 2), "Grayscale Image")) # Save the grayscaled image at the specified path if save: cam.PushFilter(sens.ChFilterSave(out_dir + "gray/")) # Resizes the image to the provided width and height cam.PushFilter( sens.ChFilterImageResize(int(image_width / 2), int(image_height / 2))) # Access the grayscaled buffer as R8 pixels cam.PushFilter(sens.ChFilterR8Access()) # add sensor to manager manager.AddSensor(cam) # --------------- # Simulate system # --------------- orbit_radius = 10 orbit_rate = 0.5 ch_time = 0.0 t1 = time.time() while (ch_time < end_time): cam.SetOffsetPose( chrono.ChFrameD( chrono.ChVectorD( -orbit_radius * math.cos(ch_time * orbit_rate), -orbit_radius * math.sin(ch_time * orbit_rate), 1), chrono.Q_from_AngAxis(ch_time * orbit_rate, chrono.ChVectorD(0, 0, 1)))) # Access the RGBA8 buffer from the camera rgba8_buffer = cam.GetMostRecentRGBA8Buffer() if (rgba8_buffer.HasData()): rgba8_data = rgba8_buffer.GetRGBA8Data() print('RGBA8 buffer recieved from cam. Camera resolution: {0}x{1}'\ .format(rgba8_buffer.Width, rgba8_buffer.Height)) print('First Pixel: {0}'.format(rgba8_data[0, 0, :])) # Update sensor manager # Will render/save/filter automatically manager.Update() # Perform step of dynamics mphysicalSystem.DoStepDynamics(step_size) # Get the current time of the simulation ch_time = mphysicalSystem.GetChTime() print("Sim time:", end_time, "Wall time:", time.time() - t1)
def main(): print("Copyright (c) 2017 projectchrono.org" + "\n\n") # Create systems # Create the HMMWV vehicle, set parameters, and initialize my_hmmwv = veh.HMMWV_Full() my_hmmwv.SetContactMethod(contact_method) my_hmmwv.SetChassisCollisionType(chassis_collision_type) my_hmmwv.SetChassisFixed(False) my_hmmwv.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) my_hmmwv.SetPowertrainType(powertrain_model) my_hmmwv.SetDriveType(drive_type) my_hmmwv.SetSteeringType(steering_type) my_hmmwv.SetTireType(tire_model) my_hmmwv.SetTireStepSize(tire_step_size) my_hmmwv.Initialize() my_hmmwv.SetChassisVisualizationType(chassis_vis_type) my_hmmwv.SetSuspensionVisualizationType(suspension_vis_type) my_hmmwv.SetSteeringVisualizationType(steering_vis_type) my_hmmwv.SetWheelVisualizationType(wheel_vis_type) my_hmmwv.SetTireVisualizationType(tire_vis_type) # Create the terrain terrain = veh.RigidTerrain(my_hmmwv.GetSystem()) if (contact_method == chrono.ChContactMethod_NSC): patch_mat = chrono.ChMaterialSurfaceNSC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) elif (contact_method == chrono.ChContactMethod_SMC): patch_mat = chrono.ChMaterialSurfaceSMC() patch_mat.SetFriction(0.9) patch_mat.SetRestitution(0.01) patch_mat.SetYoungModulus(2e7) patch = terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(0, 0, 1), terrainLength, terrainWidth) patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200, 200) patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5)) terrain.Initialize() # Create the vehicle Irrlicht interface app = veh.ChWheeledVehicleIrrApp(my_hmmwv.GetVehicle(), 'HMMWV', irr.dimension2du(1000, 800)) app.SetSkyBox() app.AddTypicalLights(irr.vector3df(30, -30, 100), irr.vector3df(30, 50, 100), 250, 130) app.AddTypicalLogo(chrono.GetChronoDataFile('logo_pychrono_alpha.png')) app.SetChaseCamera(trackPoint, 6.0, 0.5) app.SetTimestep(step_size) app.AssetBindAll() app.AssetUpdateAll() # Initialize output try: os.mkdir(out_dir) except: print("Error creating directory ") # Set up vehicle output my_hmmwv.GetVehicle().SetChassisOutput(True) my_hmmwv.GetVehicle().SetSuspensionOutput(0, True) my_hmmwv.GetVehicle().SetSteeringOutput(0, True) my_hmmwv.GetVehicle().SetOutput(veh.ChVehicleOutput.ASCII, out_dir, "output", 0.1) # Generate JSON information with available output channels my_hmmwv.GetVehicle().ExportComponentList(out_dir + "/component_list.json") # Create the interactive driver system driver = veh.ChIrrGuiDriver(app) # Set the time response for steering and throttle keyboard inputs. steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1) throttle_time = 1.0 # time to go from 0 to +1 braking_time = 0.3 # time to go from 0 to +1 driver.SetSteeringDelta(render_step_size / steering_time) driver.SetThrottleDelta(render_step_size / throttle_time) driver.SetBrakingDelta(render_step_size / braking_time) driver.Initialize() # Simulation loop # Number of simulation steps between miscellaneous events render_steps = m.ceil(render_step_size / step_size) debug_steps = m.ceil(debug_step_size / step_size) # Initialize simulation frame counter and simulation time step_number = 0 render_frame = 0 if (contact_vis): app.SetSymbolscale(1e-4) # app.SetContactsDrawMode(irr.eCh_ContactsDrawMode::CONTACT_FORCES); # --------------------------------------------- # Create a sensor manager and add a point light # --------------------------------------------- manager = sens.ChSensorManager(my_hmmwv.GetSystem()) manager.scene.AddPointLight(chrono.ChVectorF(0, 0, 100), chrono.ChVectorF(2, 2, 2), 5000) manager.SetKeyframeSizeFromTimeStep(.001, 1 / 5) # ------------------------------------------------ # Create a camera and add it to the sensor manager # ------------------------------------------------ fov = 1.408 lag = 0 update_rate = 5 exposure_time = 1 / update_rate offset_pose = chrono.ChFrameD(chrono.ChVectorD(-5, 0, 2)) cam = sens.ChCameraSensor( my_hmmwv.GetChassisBody(), # body camera is attached to update_rate, # update rate in Hz offset_pose, # offset pose image_width, # image width image_height, # image height fov # camera's horizontal field of view ) cam.SetName("Camera Sensor") # cam.SetLag(0); # cam.SetCollectionWindow(0); # Visualizes the image if vis: cam.PushFilter( sens.ChFilterVisualize(image_width, image_height, "HMMWV Camera")) # Save the current image to a png file at the specified path if save: cam.PushFilter(sens.ChFilterSave(out_dir + "cam/")) # Add a camera to a sensor manager manager.AddSensor(cam) # ---------------------------------------------- # Create an IMU sensor and add it to the manager # ---------------------------------------------- offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) imu = sens.ChIMUSensor( my_hmmwv.GetChassisBody(), # body imu is attached to imu_update_rate, # update rate in Hz offset_pose, # offset pose imu_noise_none # noise model ) imu.SetName("IMU Sensor") imu.SetLag(imu_lag) imu.SetCollectionWindow(imu_collection_time) # Provides the host access to the imu data imu.PushFilter(sens.ChFilterIMUAccess()) # Add the imu to the sensor manager manager.AddSensor(imu) # ---------------------------------------------- # Create an GPS sensor and add it to the manager # ---------------------------------------------- offset_pose = chrono.ChFrameD( chrono.ChVectorD(-8, 0, 1), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))) gps = sens.ChGPSSensor( my_hmmwv.GetChassisBody(), # body imu is attached to gps_update_rate, # update rate in Hz offset_pose, # offset pose gps_reference, gps_noise_none # noise model ) gps.SetName("GPS Sensor") gps.SetLag(gps_lag) gps.SetCollectionWindow(gps_collection_time) # Provides the host access to the gps data gps.PushFilter(sens.ChFilterGPSAccess()) # Add the gps to the sensor manager manager.AddSensor(gps) realtime_timer = chrono.ChRealtimeStepTimer() while (app.GetDevice().run()): time = my_hmmwv.GetSystem().GetChTime() #End simulation if (time >= t_end): break if (step_number % render_steps == 0): app.BeginScene(True, True, irr.SColor(255, 140, 161, 192)) app.DrawAll() app.EndScene() #Debug logging if (debug_output and step_number % debug_steps == 0): print("\n\n============ System Information ============\n") print("Time = " << time << "\n\n") #my_hmmwv.DebugLog(OUT_SPRINGS | OUT_SHOCKS | OUT_CONSTRAINTS) marker_driver = my_hmmwv.GetChassis().GetMarkers()[0].GetAbsCoord( ).pos marker_com = my_hmmwv.GetChassis().GetMarkers()[1].GetAbsCoord( ).pos print("Markers\n") print(" Driver loc: ", marker_driver.x, " ", marker_driver.y, " ", marker_driver.z) print(" Chassis COM loc: ", marker_com.x, " ", marker_com.y, " ", marker_com.z) # Get driver inputs driver_inputs = driver.GetInputs() # Update modules (process inputs from other modules) driver.Synchronize(time) terrain.Synchronize(time) my_hmmwv.Synchronize(time, driver_inputs, terrain) app.Synchronize(driver.GetInputModeAsString(), driver_inputs) # Advance simulation for one timestep for all modules driver.Advance(step_size) terrain.Advance(step_size) my_hmmwv.Advance(step_size) app.Advance(step_size) # Update sensor manager # Will render/save/filter automatically manager.Update() # Increment frame number step_number += 1 # Spin in place for real time to catch up realtime_timer.Spin(step_size) return 0