def __init__(self, system: 'WAChronoSystem', filename: str = None):
if missing_chrono_sensor:
sens_import_error('WAChronoSensorManager.__init__')
_check_type(system, WAChronoSystem, 'system',
'WAChronoSensorManager.__init__') # noqa
super().__init__(system)
self._manager = sens.ChSensorManager(system._system)
if filename is not None:
load_chrono_sensor_scene_from_json(self, filename)
def reset(self):
n = 2 * np.random.randint(0, 4)
b1 = 0
b2 = 0
r1 = n
r2 = n
r3 = n
r4 = n
r5 = n
t1 = 0
t2 = 0
t3 = 0
c = 0
self.assets = AssetList(b1, b2, r1, r2, r3, r4, r5, t1, t2, t3, c)
# Create systems
self.system = chrono.ChSystemNSC()
self.system.Set_G_acc(chrono.ChVectorD(0, 0, -9.81))
self.system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
self.system.SetSolverMaxIterations(150)
self.system.SetMaxPenetrationRecoverySpeed(4.0)
# Create the terrain
rigid_terrain = True
self.terrain = veh.RigidTerrain(self.system)
if rigid_terrain:
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1),
self.terrain_length * 1.5,
self.terrain_width * 1.5)
else:
self.bitmap_file = os.path.dirname(
os.path.realpath(__file__)) + "/../utils/height_map.bmp"
self.bitmap_file_backup = os.path.dirname(
os.path.realpath(__file__)) + "/../utils/height_map_backup.bmp"
shape = (252, 252)
generate_random_bitmap(shape=shape,
resolutions=[(2, 2)],
mappings=[(-1.5, 1.5)],
file_name=self.bitmap_file)
try:
patch = self.terrain.AddPatch(
chrono.CSYSNORM, # position
self.bitmap_file, # heightmap file (.bmp)
"test", # mesh name
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height) # hMax
except Exception:
print('Corrupt Bitmap File')
patch = self.terrain.AddPatch(
chrono.CSYSNORM, # position
self.bitmap_file_backup, # heightmap file (.bmp)
"test", # mesh name
self.terrain_length * 1.5, # sizeX
self.terrain_width * 1.5, # sizeY
self.min_terrain_height, # hMin
self.max_terrain_height) # hMax
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
visual_asset.SetStatic(True)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
visual_asset.material_list.append(vis_mat)
theta = random.random() * 2 * np.pi
x, y = self.terrain_length * 0.5 * np.cos(
theta), self.terrain_width * 0.5 * np.sin(theta)
z = self.terrain.GetHeight(chrono.ChVectorD(x, y, 0)) + 0.25
ang = np.pi + theta
self.initLoc = chrono.ChVectorD(x, y, z)
self.initRot = chrono.Q_from_AngZ(ang)
self.vehicle = veh.Gator(self.system)
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.m_inputs = veh.Inputs()
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
if self.play_mode:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_MESH)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
else:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetTireVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
# self.chassis_body.GetCollisionModel().ClearModel()
# size = chrono.ChVectorD(3,2,0.2)
# self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x, 0.5 * size.y, 0.5 * size.z)
# self.chassis_body.GetCollisionModel().BuildModel()
self.chassis_collision_box = chrono.ChVectorD(3, 2, 0.2)
# Driver
self.driver = veh.ChDriver(self.vehicle.GetVehicle())
# create goal
# pi/4 ang displ
delta_theta = (random.random() - 0.5) * 1.0 * np.pi
gx, gy = self.terrain_length * 0.5 * np.cos(
theta + np.pi +
delta_theta), self.terrain_width * 0.5 * np.sin(theta + np.pi +
delta_theta)
self.goal = chrono.ChVectorD(
gx, gy,
self.terrain.GetHeight(chrono.ChVectorD(gx, gy, 0)) + 1.0)
i = 0
while (self.goal - self.initLoc).Length() < 15:
gx = random.random() * self.terrain_length - self.terrain_length / 2
gy = random.random() * self.terrain_width - self.terrain_width / 2
self.goal = chrono.ChVectorD(gx, gy, self.max_terrain_height + 1)
if i > 100:
print('Break')
break
i += 1
# self.goal = chrono.ChVectorD(75, 0, 0)
# Origin in Madison WI
self.origin = chrono.ChVectorD(43.073268, -89.400636, 260.0)
self.goal_coord = chrono.ChVectorD(self.goal)
sens.Cartesian2GPS(self.goal_coord, self.origin)
self.goal_sphere = chrono.ChBodyEasySphere(.55, 1000, True, False)
self.goal_sphere.SetBodyFixed(True)
sphere_asset = self.goal_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)
visual_asset.SetStatic(True)
self.goal_sphere.SetPos(self.goal)
if self.play_mode:
self.system.Add(self.goal_sphere)
# create obstacles
# start = t.time()
self.assets.Clear()
self.assets.RandomlyPositionAssets(self.system,
self.initLoc,
self.goal,
self.terrain,
self.terrain_length * 1.5,
self.terrain_width * 1.5,
should_scale=False)
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = 0.75
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = .5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.SteeringDelta = (self.timestep / steering_time)
self.ThrottleDelta = (self.timestep / throttle_time)
self.BrakingDelta = (self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
# Let's not, for the moment, give a different scenario during test
"""
if self.play_mode:
self.manager.scene.GetBackground().has_texture = True;
self.manager.scene.GetBackground().env_tex = "sensor/textures/qwantani_8k.hdr"
self.manager.scene.GetBackground().has_changed = True;
"""
# -----------------------------------------------------
# Create a self.camera and add it to the sensor manager
# -----------------------------------------------------
#chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875), chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
20, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(.65, 0, .75),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
6 # supersampling factor
)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
if self.play_mode:
self.camera.PushFilter(
sens.ChFilterVisualize(self.camera_width, self.camera_height,
"RGB Camera"))
self.manager.AddSensor(self.camera)
# -----------------------------------------------------
# Create a self.gps and add it to the sensor manager
# -----------------------------------------------------
gps_noise_none = sens.ChGPSNoiseNone()
self.gps = sens.ChGPSSensor(
self.chassis_body, 15,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.gps.SetName("GPS Sensor")
self.gps.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.gps)
# have to reconstruct scene because sensor loads in meshes separately (ask Asher)
# start = t.time()
if self.assets.GetNum() > 0:
# self.assets.TransformAgain()
# start = t.time()
for asset in self.assets.assets:
if len(asset.frames) > 0:
self.manager.AddInstancedStaticSceneMeshes(
asset.frames, asset.mesh.shape)
# self.manager.ReconstructScenes()
# self.manager.AddInstancedStaticSceneMeshes(self.assets.frames, self.assets.shapes)
# self.manager.Update()
# print('Reconstruction :: ', t.time() - start)
self.old_dist = (self.goal - self.initLoc).Length()
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
self.dist0 = (self.goal - self.chassis_body.GetPos()).Length()
if self.play_mode:
self.render()
# print(self.get_ob()[1])
return self.get_ob()
def reset(self):
x_half_length = 90
y_half_length = 40
self.path = BezierPath(x_half_length, y_half_length, 0.5,
self.interval)
pos, rot = self.path.getPosRot(self.path.current_t - self.interval)
self.initLoc = chrono.ChVectorD(pos)
self.initRot = chrono.ChQuaternionD(rot)
self.vehicle = veh.HMMWV_Reduced()
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.surf_material = chrono.ChMaterialSurfaceNSC()
self.vehicle.SetChassisCollisionType(
veh.ChassisCollisionType_PRIMITIVES)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
self.vehicle.SetDriveType(veh.DrivelineType_AWD)
# self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
if self.play_mode == True:
# self.vehicle.SetChassisVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
else:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
self.chassis_body.GetCollisionModel().ClearModel()
size = chrono.ChVectorD(3, 2, 0.2)
self.chassis_body.GetCollisionModel().AddBox(self.surf_material,
0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
self.m_inputs = veh.Inputs()
self.system = self.vehicle.GetVehicle().GetSystem()
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 5000.0)
# Driver
#self.driver = veh.ChDriver(self.vehicle.GetVehicle())
self.terrain = veh.RigidTerrain(self.system)
patch_mat = chrono.ChMaterialSurfaceNSC()
patch_mat.SetFriction(0.9)
patch_mat.SetRestitution(0.01)
patch = self.terrain.AddPatch(patch_mat, chrono.ChVectorD(0, 0, 0),
chrono.ChVectorD(0, 0, 1),
self.terrainLength * 1.5,
self.terrainWidth * 1.5)
patch.SetTexture(veh.GetDataFile("terrain/textures/grass.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
self.groundBody = patch.GetGroundBody()
ground_asset = self.groundBody.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(veh.GetDataFile("terrain/textures/grass.jpg"))
visual_asset.material_list.append(vis_mat)
self.leaders.addLeaders(self.system, self.path)
self.leader_box = self.leaders.getBBox()
self.lead_pos = (self.chassis_body.GetPos() -
self.leaders[0].GetPos()).Length()
# Add obstacles:
self.obstacles = []
self.placeObstacle(8)
# for leader in self.leaders:
# ------------------------------------------------
# Create a self.camera and add it to the sensor manager
# ------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
10, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
6)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# -----------------------------------------------------
# Create a self.gps and add it to the sensor manager
# -----------------------------------------------------
gps_noise_none = sens.ChGPSNoiseNone()
self.AgentGPS = sens.ChGPSSensor(
self.chassis_body, 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.AgentGPS.SetName("AgentGPS Sensor")
self.AgentGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.AgentGPS)
### Target GPS
self.TargetGPS = sens.ChGPSSensor(
self.leaders[0], 10,
chrono.ChFrameD(
chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
self.origin, gps_noise_none)
self.TargetGPS.SetName("TargetGPS Sensor")
self.TargetGPS.PushFilter(sens.ChFilterGPSAccess())
self.manager.AddSensor(self.TargetGPS)
self.step_number = 0
self.num_frames = 0
self.num_updates = 0
self.c_f = 0
self.old_ac = [0, 0]
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
def reset(self):
self.generate_track()
self.vehicle = veh.Sedan()
self.vehicle.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_RIGID)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.system = self.vehicle.GetSystem()
self.chassis_body = self.vehicle.GetChassisBody()
# Create contact model
self.chassis_body.GetCollisionModel().ClearModel()
size = chrono.ChVectorD(3, 2, 0.2)
self.chassis_body.GetCollisionModel().AddBox(0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
# Driver
self.driver = Driver(self.vehicle.GetVehicle())
# Throttle controller
#self.throttle_controller = PIDThrottleController()
#self.throttle_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
#self.throttle_controller.SetTargetSpeed(speed=self.target_speed)
# Rigid terrain
self.terrain = veh.RigidTerrain(self.system)
patch = self.terrain.AddPatch(
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, self.terrainHeight - 5),
chrono.QUNIT),
chrono.ChVectorD(self.terrainLength, self.terrainWidth, 10))
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
visual_asset.material_list.append(vis_mat)
# create barriers
self.barriers = []
self.DrawBarriers(self.track.left.points)
self.DrawBarriers(self.track.right.points)
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = 0.5
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = 0.5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.driver.SetSteeringDelta(self.timestep / steering_time)
self.driver.SetThrottleDelta(self.timestep / throttle_time)
self.driver.SetBrakingDelta(self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 500.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 500.0)
# -----------------------------------------------------
# Create a self.camera and add it to the sensor manager
# -----------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
50, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(1.5, 0, .875),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
(self.camera_height / self.camera_width) * chrono.CH_C_PI /
3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.FilterList().append(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
self.old_dist = self.track.center.calcDistance(
self.chassis_body.GetPos())
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
app.AssetBindAll()
app.AssetUpdateAll()
# Number of simulation steps between miscellaneous events
render_steps = math.ceil(render_step_size / step_size)
control_steps = math.ceil(control_step_size / step_size)
# Initialize simulation frame counter s
# realtime_timer = chrono.ChRealtimeStepTimer()
step_number = 0
render_frame = 0
manager = sens.ChSensorManager(my_rccar.GetSystem())
manager.scene.AddPointLight(chrono.ChVectorF(100,100,100),chrono.ChVectorF(1,1,1),500.0)
manager.scene.AddPointLight(chrono.ChVectorF(-100,100,100),chrono.ChVectorF(1,1,1),500.0)
manager.scene.GetBackground().has_texture = True;
manager.scene.GetBackground().env_tex = "sensor/textures/cloud_layers_8k.hdr";
manager.scene.GetBackground().has_changed = True;
#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
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)
myapplication.AssetBindAll()
# ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
# that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
myapplication.AssetUpdateAll()
# ---------------------------------------------------------------------
#
# Run the simulation
#
myapplication.SetTimestep(0.005)
manager = sens.ChSensorManager(mysystem)
imu = sens.ChIMUSensor(mbody2, 2, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0)))
imu.SetName("IMU")
SHIB = sens.SensorHostIMUBuffer()
fl = imu.FilterList()
print(type(fl))
FiA = sens.ChFilterIMUAccess()
print(type(FiA))
#imu->PushFilter(chrono_types::make_shared<ChFilterIMUAccess>())
imu.FilterList().append(FiA)
def reset(self):
del self.manager
material = chrono.ChMaterialSurfaceNSC()
self.vehicle = veh.HMMWV_Reduced()
self.vehicle.SetContactMethod(chrono.ChContactMethod_NSC)
self.vehicle.SetChassisCollisionType(
veh.ChassisCollisionType_PRIMITIVES)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetPowertrainType(veh.PowertrainModelType_SHAFTS)
self.vehicle.SetDriveType(veh.DrivelineType_AWD)
#self.vehicle.SetSteeringType(veh.SteeringType_PITMAN_ARM)
self.vehicle.SetTireType(veh.TireModelType_TMEASY)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
#self.vehicle.SetStepsize(self.timestep)
if self.play_mode == True:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_MESH)
self.vehicle.SetWheelVisualizationType(veh.VisualizationType_MESH)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_MESH)
else:
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
self.chassis_body.GetCollisionModel().ClearModel()
size = chrono.ChVectorD(3, 2, 0.2)
self.chassis_body.GetCollisionModel().AddBox(material, 0.5 * size.x,
0.5 * size.y,
0.5 * size.z)
self.chassis_body.GetCollisionModel().BuildModel()
# Driver
self.driver = veh.ChDriver(self.vehicle.GetVehicle())
# Rigid terrain
self.system = self.vehicle.GetSystem()
self.terrain = veh.RigidTerrain(self.system)
patch = self.terrain.AddPatch(
material, chrono.ChVectorD(0, 0, self.terrainHeight - .5),
chrono.VECT_Z, self.terrainLength, self.terrainWidth, 10)
material.SetFriction(0.9)
material.SetRestitution(0.01)
#patch.SetContactMaterialProperties(2e7, 0.3)
patch.SetTexture(veh.GetDataFile("terrain/textures/tile4.jpg"), 200,
200)
patch.SetColor(chrono.ChColor(0.8, 0.8, 0.5))
self.terrain.Initialize()
ground_body = patch.GetGroundBody()
ground_asset = ground_body.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(ground_asset)
vis_mat = chrono.ChVisualMaterial()
vis_mat.SetKdTexture(chrono.GetChronoDataFile("concrete.jpg"))
visual_asset.material_list.append(vis_mat)
# create obstacles
self.boxes = []
for i in range(3):
box = chrono.ChBodyEasyBox(2, 2, 10, 1000, True, True)
box.SetPos(
chrono.ChVectorD(25 + 25 * i,
(np.random.rand(1)[0] - 0.5) * 10, .05))
box.SetBodyFixed(True)
box_asset = box.GetAssets()[0]
visual_asset = chrono.CastToChVisualization(box_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)
visual_asset.SetStatic(True)
self.boxes.append(box)
self.system.Add(box)
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = 1.0
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = .5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.SteeringDelta = (self.timestep / steering_time)
self.ThrottleDelta = (self.timestep / throttle_time)
self.BrakingDelta = (self.timestep / braking_time)
self.manager = sens.ChSensorManager(self.system)
self.manager.scene.AddPointLight(chrono.ChVectorF(100, 100, 100),
chrono.ChVectorF(1, 1, 1), 4000.0)
self.manager.scene.AddPointLight(chrono.ChVectorF(-100, -100, 100),
chrono.ChVectorF(1, 1, 1), 4000.0)
# ------------------------------------------------
# Create a self.camera and add it to the sensor manager
# ------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
50, # scanning rate in Hz
chrono.ChFrameD(chrono.ChVectorD(1.5, 0, .875)),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 3, # horizontal field of view
#(self.camera_height / self.camera_width) * chrono.CH_C_PI / 3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.PushFilter(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
self.d_old = np.linalg.norm(self.Xtarg + self.Ytarg)
self.step_number = 0
self.c_f = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
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
def reset(self):
self.generate_track(starting_index=350, z=.40)
self.vehicle = veh.RCCar()
self.vehicle.SetContactMethod(chrono.ChMaterialSurface.NSC)
self.vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE)
self.vehicle.SetChassisFixed(False)
self.vehicle.SetInitPosition(
chrono.ChCoordsysD(self.initLoc, self.initRot))
self.vehicle.SetTireType(veh.TireModelType_RIGID)
self.vehicle.SetTireStepSize(self.timestep)
self.vehicle.Initialize()
self.vehicle.SetChassisVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetWheelVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSuspensionVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetSteeringVisualizationType(
veh.VisualizationType_PRIMITIVES)
self.vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES)
self.chassis_body = self.vehicle.GetChassisBody()
self.system = self.vehicle.GetVehicle().GetSystem()
# Driver
self.driver = Driver(self.vehicle.GetVehicle())
# Set the time response for steering and throttle inputs.
# NOTE: this is not exact, since we do not render quite at the specified FPS.
steering_time = .65
# time to go from 0 to +1 (or from 0 to -1)
throttle_time = .5
# time to go from 0 to +1
braking_time = 0.3
# time to go from 0 to +1
self.driver.SetSteeringDelta(self.timestep / steering_time)
self.driver.SetThrottleDelta(self.timestep / throttle_time)
self.driver.SetBrakingDelta(self.timestep / braking_time)
# Longitudinal controller (throttle and braking)
self.long_controller = PIDLongitudinalController(
self.vehicle, self.driver)
self.long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
self.long_controller.SetTargetSpeed(speed=self.target_speed)
# Mesh hallway
y_max = 5.65
x_max = 23
offset = chrono.ChVectorD(-x_max / 2, -y_max / 2, .21)
offsetF = chrono.ChVectorF(offset.x, offset.y, offset.z)
self.terrain = veh.RigidTerrain(self.system)
coord_sys = chrono.ChCoordsysD(offset,
chrono.ChQuaternionD(1, 0, 0, 0))
patch = self.terrain.AddPatch(
coord_sys,
chrono.GetChronoDataFile("sensor/textures/hallway_contact.obj"),
"mesh", 0.01, False)
vis_mesh = chrono.ChTriangleMeshConnected()
vis_mesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("sensor/textures/hallway.obj"), True,
True)
trimesh_shape = chrono.ChTriangleMeshShape()
trimesh_shape.SetMesh(vis_mesh)
trimesh_shape.SetName("mesh_name")
trimesh_shape.SetStatic(True)
patch.GetGroundBody().AddAsset(trimesh_shape)
patch.SetContactFrictionCoefficient(0.9)
patch.SetContactRestitutionCoefficient(0.01)
patch.SetContactMaterialProperties(2e7, 0.3)
self.terrain.Initialize()
f = 0
start_light = 0
end_light = 8
self.manager = sens.ChSensorManager(self.system)
for i in range(8):
f += 3
if i < start_light or i > end_light:
continue
self.manager.scene.AddPointLight(
chrono.ChVectorF(f, 1.25, 2.3) + offsetF,
chrono.ChVectorF(1, 1, 1), 5)
self.manager.scene.AddPointLight(
chrono.ChVectorF(f, 3.75, 2.3) + offsetF,
chrono.ChVectorF(1, 1, 1), 5)
# -----------------------------------------------------
# Create a self.camera and add it to the sensor manager
# -----------------------------------------------------
self.camera = sens.ChCameraSensor(
self.chassis_body, # body camera is attached to
50, # scanning rate in Hz
chrono.ChFrameD(
chrono.ChVectorD(-.075, 0, .15),
chrono.Q_from_AngAxis(0, chrono.ChVectorD(0, 1, 0))),
# offset pose
self.camera_width, # number of horizontal samples
self.camera_height, # number of vertical channels
chrono.CH_C_PI / 2, # horizontal field of view
(self.camera_height / self.camera_width) * chrono.CH_C_PI /
3. # vertical field of view
)
self.camera.SetName("Camera Sensor")
self.camera.FilterList().append(sens.ChFilterRGBA8Access())
self.manager.AddSensor(self.camera)
# create obstacles
self.cones = []
self.DrawCones(self.track.left.points, 'green', z=.31)
self.DrawCones(self.track.right.points, 'red', z=.31)
self.old_dist = self.track.center.calcDistance(
self.chassis_body.GetPos())
self.step_number = 0
self.isdone = False
self.render_setup = False
if self.play_mode:
self.render()
return self.get_ob()
def main():
chrono.SetChronoDataPath("../../../data/")
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# ----------------------------------
# add a mesh to be sensed by a lidar
# ----------------------------------
mmesh = chrono.ChTriangleMeshConnected()
mmesh.LoadWavefrontMesh(
chrono.GetChronoDataFile("vehicle/hmmwv/hmmwv_chassis.obj"), False,
True)
mmesh.Transform(chrono.ChVectorD(0, 0, 0),
chrono.ChMatrix33D(2)) # scale to a different size
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)
manager.SetKeyframeSizeFromTimeStep(.001, 1 / collection_time)
# ------------------------------------------------
# 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(
mesh_body, # body lidar is attached to
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, # vertical field of view
min_vert_angle,
100.0, #max lidar range
sample_radius, # sample radius
divergence_angle, # divergence angle
return_mode, # return mode for the lidar
lens_model # method/model to use for generating data
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lag)
lidar.SetCollectionWindow(collection_time)
# -----------------------------------------------------------------
# Create a filter graph for post-processing the data from the lidar
# -----------------------------------------------------------------
if noise_model == "CONST_NORMAL_XYZI":
lidar.PushFilter(sens.ChFilterLidarNoiseXYZI(0.01, 0.001, 0.001, 0.01))
elif noise_model == "NONE":
# Don't add any noise models
pass
if vis:
# Visualize 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)
# ---------------
# Simulate system
# ---------------
orbit_radius = 5
orbit_rate = 0.2
ch_time = 0.0
render_time = 0
t1 = time.time()
while (ch_time < end_time):
lidar.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 XYZI buffer from lidar
xyzi_buffer = lidar.GetMostRecentXYZIBuffer()
if xyzi_buffer.HasData():
xyzi_data = xyzi_buffer.GetXYZIData()
print('XYZI buffer recieved from lidar. Lidar resolution: {0}x{1}'\
.format(xyzi_buffer.Width, xyzi_buffer.Height))
print('Max Value: {0}'.format(np.max(xyzi_data)))
# 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)