void OnDrawGizmosSelected()
{
Gizmos.color = Color.red;
LidarPointArray arr = GetOutput();
if (arr == null)
{
return;
}
Vector3 pos = Vector3.zero;
foreach (LidarPoint p in arr.points)
{
if (p == null)
{
continue;
}
pos.x = p.x;
pos.y = p.y;
pos.z = p.z;
Gizmos.DrawSphere(pos, gizmoSize);
}
}
void OnDrawGizmosSelected()
{
Gizmos.color = Color.red;
LidarPointArray arr = GetOutput();
if (arr == null)
{
return;
}
Vector3 pos = Vector3.zero;
foreach (LidarPoint p in arr.points)
{
if (p == null)
{
continue;
}
pos.x = p.x;
pos.y = p.y;
pos.z = p.z;
//make points global space for drawing
pos = transform.position + pos;
Gizmos.DrawSphere(pos, gizmoSize);
}
}
public JSONObject GetOutputAsJson()
{
LidarPointArray points = GetOutput();
JSONObject json = JSONObject.Create();
foreach (LidarPoint p in points.points)
{
JSONObject vec = JSONObject.Create();
try
{
vec.AddField("x", p.x);
vec.AddField("y", p.y);
vec.AddField("z", p.z);
vec.AddField("rx", p.rx);
vec.AddField("ry", p.ry);
json.Add(vec);
}
catch
{
// just ignore points that don't resolve.
}
}
return(json);
}
// Update is called once per frame
void Update()
{
if (!bDoLog)
{
return;
}
timeSinceLastCapture += Time.deltaTime;
if (timeSinceLastCapture < 1.0f / limitFPS)
{
return;
}
timeSinceLastCapture -= (1.0f / limitFPS);
string activity = car.GetActivity();
if (lidar != null)
{
LidarPointArray pa = lidar.GetOutput();
if (pa != null)
{
string json = JsonUtility.ToJson(pa);
var filename = string.Format("/../log/lidar_{0}_{1}.txt", frameCounter.ToString(), activity);
var f = File.CreateText(Application.dataPath + filename);
f.Write(json);
f.Close();
}
}
if (optionlB_CamSensor != null)
{
SaveCamSensor(camSensor, activity, "_a");
SaveCamSensor(optionlB_CamSensor, activity, "_b");
}
else
{
SaveCamSensor(camSensor, activity, "");
}
if (maxFramesToLog != -1 && frameCounter >= maxFramesToLog)
{
Shutdown();
this.gameObject.SetActive(false);
}
frameCounter = frameCounter + 1;
}
void Awake()
{
pointArr = new LidarPointArray();
pointArr.Init(360 / degPerSweepInc * numSweepsLevels);
int v = 0;
foreach (string layerName in layerMaskNames)
{
int layer = LayerMask.NameToLayer(layerName);
v |= 1 << layer;
}
collMask |= ~v;
}
// Will be called after all regular rendering is done
public void OnRenderObject()
{
if (DisplayDebugInScene == false)
{
return;
}
CreateLineMaterial();
// Apply the line material
lineMaterial.SetPass(0);
GL.PushMatrix();
// Set transformation matrix for drawing to
// match our transform
//GL.MultMatrix (transform.localToWorldMatrix);
GL.MultMatrix(Matrix4x4.identity);
// Draw lines
GL.Begin(GL.LINES);
LidarPointArray arr = GetOutput();
if (arr == null)
{
return;
}
foreach (LidarPoint p in arr.points)
{
if (p == null)
{
continue;
}
//red
GL.Color(new Color(1, 0, 0, 0.8F));
GL.Vertex3(p.x, p.y, p.z);
GL.Vertex3(p.x, p.y + gizmoSize, p.z);
}
GL.End();
GL.PopMatrix();
}
// Update is called once per frame
void Update()
{
string activity = car.GetActivity();
if (writer != null)
{
writer.WriteLine(string.Format("{0},{1},{2},{3}", frameCounter.ToString(), activity, car.GetSteering().ToString(), car.GetThrottle().ToString()));
}
if (lidar != null)
{
LidarPointArray pa = lidar.GetOutput();
if (pa != null)
{
string json = JsonUtility.ToJson(pa);
var filename = string.Format("/../log/lidar_{0}_{1}.txt", frameCounter.ToString(), activity);
var f = File.CreateText(Application.dataPath + filename);
f.Write(json);
f.Close();
}
}
if (optionlB_CamSensor != null)
{
SaveCamSensor(camSensor, activity, "_a");
SaveCamSensor(optionlB_CamSensor, activity, "_b");
}
else
{
SaveCamSensor(camSensor, activity, "");
}
if (maxFramesToLog != -1 && frameCounter >= maxFramesToLog)
{
Shutdown();
this.gameObject.SetActive(false);
}
frameCounter = frameCounter + 1;
}
public void SetConfig(float offset_x, float offset_y, float offset_z, float rot_x,
int _degPerSweepInc, float _degAngDown, float _degAngDelta, float _maxRange, float _noise, int _numSweepsLevels)
{
degPerSweepInc = _degPerSweepInc;
degAngDown = _degAngDown;
degAngDelta = _degAngDelta;
maxRange = _maxRange;
noise = _noise;
numSweepsLevels = _numSweepsLevels;
if (offset_x != 0.0f || offset_y != 0.0f || offset_z != 0.0f)
{
transform.localPosition = new Vector3(offset_x, offset_y, offset_z);
}
if (rot_x != 0.0f)
{
transform.localEulerAngles = new Vector3(rot_x, 0.0f, 0.0f);
}
pointArr = new LidarPointArray();
pointArr.Init(360 / degPerSweepInc * numSweepsLevels);
}
void OnDrawGizmosSelected()
{
Gizmos.color = Color.red;
pointArr = GetOutput(); // not really efficient but won't be called in the app, just for visualization purpose
Vector3 pos = Vector3.zero;
foreach (LidarPoint p in pointArr.points)
{
if (p == null)
{
continue;
}
pos.x = p.x;
pos.y = p.y;
pos.z = p.z;
//make points global space for drawing
pos += transform.position;
Gizmos.DrawSphere(pos, gizmoSize);
}
}
// Update is called once per frame
void Update()
{
if (!bDoLog)
{
return;
}
timeSinceLastCapture += Time.deltaTime;
if (timeSinceLastCapture < 1.0f / limitFPS)
{
return;
}
timeSinceLastCapture -= (1.0f / limitFPS);
string activity = car.GetActivity();
if (writer != null)
{
if (UdacityStyle)
{
string image_filename = GetUdacityStyleImageFilename();
float steering = car.GetSteering() / car.GetMaxSteering();
writer.WriteLine(string.Format("{0},{1},{2},{3},{4},{5},{6}", image_filename, "none", "none", steering.ToString(), car.GetThrottle().ToString(), "0", "0"));
}
else if (DonkeyStyle || SharkStyle)
{
}
else if (DonkeyStyle2)
{
DonkeyRecord mjson = new DonkeyRecord();
float steering = car.GetSteering() / car.GetMaxSteering();
float throttle = car.GetThrottle();
//training code like steering clamped between -1, 1
steering = Mathf.Clamp(steering, -1.0f, 1.0f);
mjson.Init(string.Format("{0}_cam-image_array_.jpg", frameCounter),
throttle, steering, "user");
string json = mjson.AsString();
string filename = string.Format("record_{0}.json", frameCounter);
var f = File.CreateText(Application.dataPath + "/../log/" + filename);
f.Write(json);
f.Close();
}
else
{
writer.WriteLine(string.Format("{0},{1},{2},{3}", frameCounter.ToString(), activity, car.GetSteering().ToString(), car.GetThrottle().ToString()));
}
}
if (lidar != null)
{
LidarPointArray pa = lidar.GetOutput();
if (pa != null)
{
string json = JsonUtility.ToJson(pa);
var filename = string.Format("/../log/lidar_{0}_{1}.txt", frameCounter.ToString(), activity);
var f = File.CreateText(Application.dataPath + filename);
f.Write(json);
f.Close();
}
}
if (optionlB_CamSensor != null)
{
SaveCamSensor(camSensor, activity, "_a");
SaveCamSensor(optionlB_CamSensor, activity, "_b");
}
else
{
SaveCamSensor(camSensor, activity, "");
}
if (maxFramesToLog != -1 && frameCounter >= maxFramesToLog)
{
Shutdown();
this.gameObject.SetActive(false);
}
frameCounter = frameCounter + 1;
if (logDisplay != null)
{
logDisplay.text = "Log:" + frameCounter;
}
}
public LidarPointArray GetOutput()
{
pointArr = new LidarPointArray();
pointArr.Init(360 / degPerSweepInc * numSweepsLevels);
pointArr.lidarPos = new V3(transform.position);
pointArr.lidarOrientation = new V3(transform.rotation.eulerAngles);
Ray ray = new Ray();
ray.origin = this.transform.position;
ray.direction = this.transform.forward;
//start out pointing a bit down.
Quaternion rotDown = Quaternion.AngleAxis(degAngDown, transform.right);
ray.direction = rotDown * ray.direction;
int numSweep = 360 / degPerSweepInc;
Quaternion rotSide = Quaternion.AngleAxis(degPerSweepInc, transform.up);
Quaternion rotUp = Quaternion.AngleAxis(degAngDelta, transform.right);
RaycastHit hit;
//Sample the output texture to create rays.
int iP = 0;
float rx = 0.0f;
float ry = 0.0f;
for (int iS = 0; iS < numSweepsLevels; iS++)
{
for (int iA = 0; iA < numSweep; iA++)
{
if (Physics.Raycast(ray, out hit, maxRange, collMask))
{
//sample that ray at the depth given by the pixel.
Vector3 pos = ray.GetPoint(hit.distance);
//make points relative to my pos.
pos = pos - transform.position;
//shouldn't hit this unless user is messing around in the interface with things running.
if (iP >= pointArr.points.Length)
{
break;
}
float noiseX = Mathf.PerlinNoise(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f));
float noiseY = Mathf.PerlinNoise(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f));
float noiseZ = Mathf.PerlinNoise(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f));
pos.x += noise * noiseX;
pos.y += noise * noiseY;
pos.z += noise * noiseZ;
//set iPoint
pointArr.points[iP] = new LidarPoint(pos, rx, ry);
ray.direction = rotSide * ray.direction;
iP++;
}
rx += degPerSweepInc;
}
ray.direction = rotUp * ray.direction;
ry += degAngDelta;
rx = 0.0f;
}
return(pointArr);
}
public LidarPointArray GetOutput()
{
int numSweep = (int)(360 / degPerSweepInc);
pointArr = new LidarPointArray();
pointArr.Init(numSweep * numSweepsLevels);
Ray ray = new Ray(transform.position, transform.forward);
RaycastHit hit;
// Vertical rotation
Quaternion rotUp = Quaternion.AngleAxis(degAngDelta, transform.right);
// Horizontal rotation
Quaternion rotSide = Quaternion.AngleAxis(degPerSweepInc, transform.up);
//Sample the output texture to create rays.
int iP = 0;
float rx = 0.0f;
float ry = 0.0f;
for (int iS = 0; iS < numSweepsLevels; iS++)
{
// reset the orientation of the ray
Quaternion rotDown = Quaternion.AngleAxis(degAngDown + iS * degAngDelta, transform.right);
ray.direction = rotDown * transform.forward;
rx = 0.0f;
for (int iA = 0; iA < numSweep; iA++)
{
if (Physics.Raycast(ray, out hit, maxRange, collMask))
{
//sample that ray at the depth given by the pixel.
Vector3 pos = hit.point - transform.position;
float distance = hit.distance;
//shouldn't hit this unless user is messing around in the interface with things running.
if (iP >= pointArr.points.Length)
{
break;
}
// add some noise to the point position
float noiseX = Mathf.PerlinNoise(UnityEngine.Random.Range(-1f, 1f), UnityEngine.Random.Range(-1f, 1f));
float noiseY = Mathf.PerlinNoise(UnityEngine.Random.Range(-1f, 1f), UnityEngine.Random.Range(-1f, 1f));
float noiseZ = Mathf.PerlinNoise(UnityEngine.Random.Range(-1f, 1f), UnityEngine.Random.Range(-1f, 1f));
pos.x += noise * noiseX;
pos.y += noise * noiseY;
pos.z += noise * noiseZ;
//set iPoint
pointArr.points[iP] = new LidarPoint(pos, distance, rx, ry);
iP++;
}
ray.direction = rotSide * ray.direction;
rx += degPerSweepInc;
}
ray.direction = rotUp * ray.direction;
ry += degAngDelta;
}
return(pointArr);
}
void Awake()
{
pointArr = new LidarPointArray();
pointArr.Init(360 / degPerSweepInc * numSweepsLevels);
}
// Update is called once per frame
void Update()
{
if (!bDoLog)
{
return;
}
timeSinceLastCapture += Time.deltaTime;
if (timeSinceLastCapture < 1.0f / limitFPS)
{
return;
}
timeSinceLastCapture -= (1.0f / limitFPS);
string activity = car.GetActivity();
if (writer != null)
{
if (PynqStyle)
{
string image_filename = GetPynqStyleImageFilename();
float steering = car.GetSteering() / car.GetMaxSteering();
float throttle = car.GetThrottle();
float speed = car.GetVelocity().magnitude / car.GetMaxSpeed();
//writer.WriteLine(string.Format("{0},{1},{2}", image_filename,steering.ToString(),throttle.ToString()));
writer.WriteLine(string.Format("{0},{1},{2}", image_filename, steering.ToString(), speed.ToString()));
}
}
if (lidar != null)
{
LidarPointArray pa = lidar.GetOutput();
if (pa != null)
{
string json = JsonUtility.ToJson(pa);
var filename = string.Format("lidar_{0}_{1}.txt", frameCounter.ToString(), activity);
var f = File.CreateText(GetLogPath() + filename);
f.Write(json);
f.Close();
}
}
if (optionlB_CamSensor != null)
{
SaveCamSensor(camSensor, activity, "_a");
SaveCamSensor(optionlB_CamSensor, activity, "_b");
}
else
{
SaveCamSensor(camSensor, activity, "");
}
if (maxFramesToLog != -1 && frameCounter >= maxFramesToLog)
{
Shutdown();
this.gameObject.SetActive(false);
}
if (storeNum != null)
{
storeNum.text = string.Format("Now/Total: {0}/{1}", frameCounter, maxFramesToLog);
}
frameCounter = frameCounter + 1;
if (logDisplay != null)
{
logDisplay.text = "Log:" + frameCounter;
}
}
