Hi,
I had raised the issue #601 in relation to _sensor import with 9.0.0 and
was asked to install version 9.0.1. I tried the sensor simulation demos
with 9.0.1 and ran into issues. I re-built some of the pipeline to better
reflect functionality of 9.0.1 in my own code, which has been attached with
this email. I am running into an issue with using LIDAR simulation here as
the LIDAR array length is 0x1 at every polling interval. Could anyone help
me debug this?
Warm Regards,
Srivatsank P
--
You received this message because you are subscribed to the Google Groups
"ProjectChrono" group.
To unsubscribe from this group and stop receiving emails from it, send an email
to [email protected].
To view this discussion visit
https://groups.google.com/d/msgid/projectchrono/4f0e56e9-b10a-479d-9147-cad94ac46680n%40googlegroups.com.
import pychrono as chrono
import pychrono.irrlicht as irr
import pychrono.vehicle as veh
import pychrono.sensor as sens
import math
import os
import errno
import time
def main():
# -----------------
# Create the system
# -----------------
mphysicalSystem = chrono.ChSystemNSC()
# ----------------------------------
# add a mesh to be sensed by a lidar
# ----------------------------------
vehmesh = chrono.ChTriangleMeshConnected()
vehmesh.LoadWavefrontMesh(chrono.GetChronoDataFile(
"vehicle/hmmwv/hmmwv_chassis.obj"), False, True)
trimesh_shape = chrono.ChVisualShapeTriangleMesh()
trimesh_shape.SetMesh(vehmesh)
trimesh_shape.SetName("HMMWV Chassis Mesh")
trimesh_shape.SetMutable(False)
# Add Terrain mesh
terrainmesh = chrono.ChTriangleMeshConnected()
terrainmesh.LoadWavefrontMesh(chrono.GetChronoDataFile(
"vehicle/terrain/meshes/test.obj"), False, True)
terrainmesh_shape = chrono.ChVisualShapeTriangleMesh()
terrainmesh_shape.SetMesh(terrainmesh)
terrainmesh_shape.SetName("Terrain Mesh")
terrainmesh_shape.SetMutable(False)
mesh_body = chrono.ChBody()
mesh_body.SetPos(chrono.ChVector3d(0, 0, 0))
mesh_body.AddVisualShape(trimesh_shape)
mesh_body.AddVisualShape(terrainmesh_shape)
mesh_body.SetFixed(True)
mphysicalSystem.Add(mesh_body)
# -----------------------
# Create a sensor manager
# -----------------------
manager = sens.ChSensorManager(mphysicalSystem)
# ------------------------------------------------
# Create a lidar and add it to the sensor manager
# ------------------------------------------------
offset_pose = chrono.ChFramed(
chrono.ChVector3d(-8, 0, 1), chrono.QuatFromAngleAxis(0, chrono.ChVector3d(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
sens.LidarBeamShape_RECTANGULAR,
sample_radius, # sample radius
divergence_angle, # divergence angle
divergence_angle, # divergence angle
return_mode # return mode for the lidar
)
lidar.SetName("Lidar Sensor")
lidar.SetLag(lag)
lidar.SetCollectionWindow(collection_time)
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.ChVector3d(-orbit_radius * math.cos(ch_time * orbit_rate), -
orbit_radius * math.sin(ch_time * orbit_rate), 1),
chrono.QuatFromAngleAxis(ch_time * orbit_rate, chrono.ChVector3d(0, 0, 1))))
xyzi_buffer = lidar.GetMostRecentXYZIBuffer()
print(lidar.GetMostRecentXYZIBuffer().GetXYZIData())
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))
# # 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)
# -----------------
# Lidar parameters
# -----------------
# Noise model attached to the sensor
# TODO: Noise models haven't been implemented in python
# noise_model="CONST_NORMAL_XYZI" # Gaussian noise with constant mean and standard deviation
noise_model = "NONE" # No noise model
# Lidar return mode
return_mode = sens.LidarReturnMode_STRONGEST_RETURN
# return_mode = sens.MEAN_RETURN
# return_mode = sens.FIRST_RETURN
# return_mode = sens.LAST_RETURN
# Update rate in Hz
update_rate = 10.0
# Number of horizontal and vertical samples
horizontal_samples = 4500
vertical_samples = 32
# Horizontal and vertical field of view (radians)
horizontal_fov = 2 * chrono.CH_PI # 360 degrees
max_vert_angle = chrono.CH_PI / 12
min_vert_angle = -chrono.CH_PI / 6
# Lag time
lag = 0
# Collection window for the lidar
collection_time = 1. / update_rate # typically 1/update rate
# Radius of samples to use, 1->1 sample,2->9 samples, 3->25 samples...
sample_radius = 2
# 3mm radius (as cited by velodyne)
divergence_angle = 0.003
# ---------------------
# Simulation parameters
# ---------------------
# Simulation step size
step_size = 1e-3
# Simulation end time
end_time = 20
# Save camera images
save = False
# Render camera images
vis = True
# Output directory
out_dir = "SENSOR_OUTPUT/"
# The path to the Chrono data directory containing various assets (meshes, textures, data files)
# is automatically set, relative to the default location of this demo.
# If running from a different directory, you must change the path to the data directory with:
# chrono.SetChronoDataPath('path/to/data')
main()
