373 lines
9.4 KiB
C#
Executable File
373 lines
9.4 KiB
C#
Executable File
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using System.IO;
|
|
using System.Threading;
|
|
using System;
|
|
using UnityEngine.UI;
|
|
|
|
[Serializable]
|
|
public class MetaJson
|
|
{
|
|
public string[] inputs;
|
|
public string[] types;
|
|
|
|
public void Init(string[] _inputs, string[] _types)
|
|
{
|
|
inputs = _inputs;
|
|
types = _types;
|
|
}
|
|
}
|
|
|
|
[Serializable]
|
|
public class DonkeyRecord
|
|
{
|
|
public string cam_image_array;
|
|
public float user_throttle;
|
|
public float user_angle;
|
|
public string user_mode;
|
|
public int track_lap;
|
|
public int track_loc;
|
|
|
|
public void Init(string image_name, float throttle, float angle, string mode, int lap, int loc)
|
|
{
|
|
cam_image_array = image_name;
|
|
user_throttle = throttle;
|
|
user_angle = angle;
|
|
user_mode = mode;
|
|
track_lap = lap;
|
|
track_loc = loc;
|
|
}
|
|
|
|
public string AsString()
|
|
{
|
|
string json = JsonUtility.ToJson(this);
|
|
|
|
//Can't name the variable names with a slash, so replace on output
|
|
json = json.Replace("cam_image", "cam/image");
|
|
json = json.Replace("user_throttle", "user/throttle");
|
|
json = json.Replace("user_angle", "user/angle");
|
|
json = json.Replace("user_mode", "user/mode");
|
|
json = json.Replace("track_lap", "track/lap");
|
|
json = json.Replace("track_lap", "track/lap");
|
|
json = json.Replace("track_loc", "track/loc");
|
|
|
|
return json;
|
|
}
|
|
}
|
|
public class Logger : MonoBehaviour {
|
|
|
|
public GameObject carObj;
|
|
public ICar car;
|
|
public CameraSensor camSensor;
|
|
public CameraSensor optionlB_CamSensor;
|
|
public Lidar lidar;
|
|
|
|
//what's the current frame index
|
|
public int frameCounter = 0;
|
|
|
|
//which lap
|
|
public int lapCounter = 0;
|
|
|
|
//is there an upper bound on the number of frames to log
|
|
public int maxFramesToLog = 14000;
|
|
|
|
//should we log when we are enabled
|
|
public bool bDoLog = true;
|
|
|
|
public int limitFPS = 30;
|
|
|
|
float timeSinceLastCapture = 0.0f;
|
|
|
|
//We can output our logs in the style that matched the output from the shark robot car platform - github/tawnkramer/shark
|
|
public bool SharkStyle = false;
|
|
|
|
//We can output our logs in the style that matched the output from the udacity simulator
|
|
public bool UdacityStyle = false;
|
|
|
|
//We can output our logs in the style that matched the output from the donkey robot car platform - donkeycar.com
|
|
public bool DonkeyStyle = false;
|
|
|
|
//Tub style as prefered by Donkey2
|
|
public bool DonkeyStyle2 = true;
|
|
|
|
public Text logDisplay;
|
|
|
|
string outputFilename = "log_car_controls.txt";
|
|
private StreamWriter writer;
|
|
|
|
class ImageSaveJob {
|
|
public string filename;
|
|
public byte[] bytes;
|
|
}
|
|
|
|
List<ImageSaveJob> imagesToSave;
|
|
|
|
Thread thread;
|
|
|
|
string GetLogPath()
|
|
{
|
|
if(GlobalState.log_path != "default")
|
|
return GlobalState.log_path + "/";
|
|
|
|
return Application.dataPath + "/../log/";
|
|
}
|
|
|
|
void Awake()
|
|
{
|
|
car = carObj.GetComponent<ICar>();
|
|
|
|
if(bDoLog && car != null)
|
|
{
|
|
if(UdacityStyle)
|
|
{
|
|
outputFilename = "driving_log.csv";
|
|
}
|
|
|
|
string filename = GetLogPath() + outputFilename;
|
|
|
|
writer = new StreamWriter(filename);
|
|
|
|
Debug.Log("Opening file for log at: " + filename);
|
|
|
|
if(UdacityStyle)
|
|
{
|
|
writer.WriteLine("center,left,right,steering,throttle,brake,speed");
|
|
}
|
|
|
|
if(DonkeyStyle2)
|
|
{
|
|
MetaJson mjson = new MetaJson();
|
|
string[] inputs = {"cam/image_array", "user/angle", "user/throttle", "user/mode", "track/lap", "track/loc"};
|
|
string[] types = {"image_array", "float", "float", "str", "int", "int"};
|
|
mjson.Init(inputs, types);
|
|
string json = JsonUtility.ToJson(mjson);
|
|
var f = File.CreateText(GetLogPath() + "meta.json");
|
|
f.Write(json);
|
|
f.Close();
|
|
}
|
|
}
|
|
|
|
Canvas canvas = GameObject.FindObjectOfType<Canvas>();
|
|
GameObject go = CarSpawner.getChildGameObject(canvas.gameObject, "LogCount");
|
|
if (go != null)
|
|
logDisplay = go.GetComponent<Text>();
|
|
|
|
imagesToSave = new List<ImageSaveJob>();
|
|
|
|
thread = new Thread(SaverThread);
|
|
thread.Start();
|
|
}
|
|
|
|
// 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},{7}", image_filename,
|
|
"none", "none", steering.ToString(),
|
|
car.GetThrottle().ToString(), "0", "0", lapCounter));
|
|
}
|
|
else if(DonkeyStyle || SharkStyle)
|
|
{
|
|
|
|
}
|
|
else if(DonkeyStyle2)
|
|
{
|
|
DonkeyRecord mjson = new DonkeyRecord();
|
|
float steering = car.GetSteering() / car.GetMaxSteering();
|
|
float throttle = car.GetThrottle();
|
|
int loc = LocationMarker.GetNearestLocMarker(carObj.transform.position);
|
|
|
|
//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", lapCounter, loc);
|
|
|
|
string json = mjson.AsString();
|
|
string filename = string.Format("record_{0}.json", frameCounter);
|
|
var f = File.CreateText(GetLogPath() + 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 && lidar.gameObject.activeInHierarchy)
|
|
{
|
|
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 && optionlB_CamSensor.gameObject.activeInHierarchy)
|
|
{
|
|
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;
|
|
}
|
|
|
|
string GetUdacityStyleImageFilename()
|
|
{
|
|
return GetLogPath() + string.Format("IMG/center_{0,8:D8}.jpg", frameCounter);
|
|
}
|
|
|
|
string GetDonkeyStyleImageFilename()
|
|
{
|
|
float steering = car.GetSteering() / 25.0f;
|
|
float throttle = car.GetThrottle();
|
|
return GetLogPath() + string.Format("frame_{0,6:D6}_ttl_{1}_agl_{2}_mil_0.0.jpg",
|
|
frameCounter, throttle, steering);
|
|
}
|
|
|
|
string GetSharkStyleImageFilename()
|
|
{
|
|
int steering = (int)(car.GetSteering() / 25.0f * 32768.0f);
|
|
int throttle = (int)(car.GetThrottle() * 32768.0f);
|
|
return GetLogPath() + string.Format("frame_{0,6:D6}_st_{1}_th_{2}.jpg",
|
|
frameCounter, steering, throttle);
|
|
}
|
|
|
|
string GetDonkey2StyleImageFilename()
|
|
{
|
|
return GetLogPath() + string.Format("{0}_cam-image_array_.jpg", frameCounter);
|
|
}
|
|
|
|
//Save the camera sensor to an image. Use the suffix to distinguish between cameras.
|
|
void SaveCamSensor(CameraSensor cs, string prefix, string suffix)
|
|
{
|
|
if (cs != null)
|
|
{
|
|
Texture2D image = cs.GetImage();
|
|
|
|
ImageSaveJob ij = new ImageSaveJob();
|
|
|
|
if(UdacityStyle)
|
|
{
|
|
ij.filename = GetUdacityStyleImageFilename();
|
|
|
|
ij.bytes = image.EncodeToJPG();
|
|
}
|
|
else if (DonkeyStyle)
|
|
{
|
|
ij.filename = GetDonkeyStyleImageFilename();
|
|
|
|
ij.bytes = image.EncodeToJPG();
|
|
}
|
|
else if (DonkeyStyle2)
|
|
{
|
|
ij.filename = GetDonkey2StyleImageFilename();
|
|
|
|
ij.bytes = image.EncodeToJPG();
|
|
}
|
|
else if(SharkStyle)
|
|
{
|
|
ij.filename = GetSharkStyleImageFilename();
|
|
|
|
ij.bytes = image.EncodeToJPG();
|
|
}
|
|
else
|
|
{
|
|
ij.filename = GetLogPath() + string.Format("{0}_{1,8:D8}{2}.png", prefix, frameCounter, suffix);
|
|
|
|
ij.bytes = image.EncodeToPNG();
|
|
}
|
|
|
|
lock (this)
|
|
{
|
|
imagesToSave.Add(ij);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void SaverThread()
|
|
{
|
|
while(true)
|
|
{
|
|
int count = 0;
|
|
|
|
lock(this)
|
|
{
|
|
count = imagesToSave.Count;
|
|
}
|
|
|
|
if(count > 0)
|
|
{
|
|
ImageSaveJob ij = imagesToSave[0];
|
|
|
|
//Debug.Log("saving: " + ij.filename);
|
|
|
|
File.WriteAllBytes(ij.filename, ij.bytes);
|
|
|
|
lock(this)
|
|
{
|
|
imagesToSave.RemoveAt(0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Shutdown()
|
|
{
|
|
if(writer != null)
|
|
{
|
|
writer.Close();
|
|
writer = null;
|
|
}
|
|
|
|
if(thread != null)
|
|
{
|
|
thread.Abort();
|
|
thread = null;
|
|
}
|
|
|
|
bDoLog = false;
|
|
}
|
|
|
|
void OnDestroy()
|
|
{
|
|
Shutdown();
|
|
}
|
|
} |