private void SetLidar(Lidar lidar)
{
if (_selectedLidar != lidar)
{
if (_selectedLidar != null)
{
_selectedLidar.NewMeasure -= lidar_NewMeasure;
if (_selectedLidar == AllDevices.LidarGround)
{
_selectedLidar.StopLoopMeasure();
}
}
_selectedLidar = lidar;
if (_selectedLidar != null)
{
_selectedLidar.NewMeasure += lidar_NewMeasure;
if (_selectedLidar == AllDevices.LidarGround)
{
_selectedLidar.StartLoopMeasure();
}
}
_measureObjects = null;
_measureBoard = null;
picWorld.Invalidate();
}
}
private void switchEnable_ValueChanged(object sender, bool value)
{
if (_selectedLidar != null)
{
_selectedLidar.FrequencyChange -= lidar_FrequencyChange;
_selectedLidar.NewMeasure -= lidar_NewMeasure;
_selectedLidar.StartLoopMeasure();
}
if (value)
{
if ((String)cboLidar.Text == "Ground")
{
_selectedLidar = AllDevices.LidarGround;
}
else if ((String)cboLidar.Text == "Avoid")
{
_selectedLidar = AllDevices.LidarAvoid;
}
else
{
_selectedLidar = null;
}
if (_selectedLidar != null)
{
_selectedLidar.FrequencyChange += lidar_FrequencyChange;
_selectedLidar.NewMeasure += lidar_NewMeasure;
_selectedLidar.StartLoopMeasure();
}
}
}
public void Start()
{
broadcast = IPAddress.Parse(IP);
ep = new IPEndPoint(broadcast, 2368);
s = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
lidarGO = gameObject.GetComponent <Lidar>();
}
protected override void Start()
{
base.Start();
lidar = gameObject.GetComponent <Lidar>();
InitializeMessage();
StartCoroutine("Process");
}
private void PanelLidar_Load(object sender, EventArgs e)
{
if (!Execution.DesignMode)
{
btnTrap.Focus();
_running = false;
_selectedLidar = null;
}
}
private void Update()
{
/* Refresh the datas of the robot */
datas.RefreshDatas(Controller.GetMotorsDatas(), Lidar.GetMeasures(), Controller.GetSensorValues());
if (Server.isActive)
{
_targetPositions = TargetPositions.ReadValues(Server.targetPositions);
}
status = datas.ToJson();
}
// Use this for initialization
void Start()
{
lidar = GetComponentInChildren <Lidar>();
tmp_gizmo_drawer = new List <Node>();
k = 1.0f + k;
gravity = Physics.gravity.y;
local_goal = transform.position;
rrt = new RRT(goal, 1.0f);
rrt.forward = transform.forward;
rrt.build_rrt(transform.position, 1000);
global_path = AStar(goal);
}
public void InstantiateSimulatedEquipment(VehicleSprite vehicle, Lidar distances)
{
_wheel = new SimulatedWheel(vehicle);
_encoderLeft = new SimulatedEncoderSensor(vehicle, leftWeelNotRight: true);
_encoderRight = new SimulatedEncoderSensor(vehicle, leftWeelNotRight: false);
float ultrasoundOffsetFwd = vehicle.Texture.Width * vehicle.TextureScale / 2 / 100;
float ultrasoundOffsetSide = vehicle.Texture.Height * vehicle.TextureScale / 2 / 100;
_ultrasound = new SimulatedUltrasoundSensor(distances, ultrasoundOffsetFwd, ultrasoundOffsetSide);
_lidar = new SimulatedLidarPacketTransmitter(distances);
}
private async void Lidar_PropertyChanged(object sender, System.ComponentModel.PropertyChangedEventArgs e)
{
switch (e.PropertyName)
{
case nameof(Lidar.RunCollector) when Lidar.RunCollector == false:
CalculateHorizontalPoints = false;
break;
case nameof(Lidar.RaiseNotificationForSelective) when Lidar.RaiseNotificationForSelective == false:
AutoCalculateDirections = false;
AutoCalculateLargestDistance = false;
break;
}
if (e.PropertyName != nameof(Lidar.LastUpdate))
{
return;
}
if (AutoCalculateDirections)
{
Task.Run(() =>
{
float fwd = Lidar.Fwd;
float left = Lidar.Left;
float right = Lidar.Right;
});
}
if (AutoCalculateLargestDistance)
{
HorizontalPoint point = Lidar.LargestDistance;
}
if (CalculateHorizontalPoints)
{
float fromAngle = CenterForAnglesInRange - BeamOpeningForAnglesInRange / 2;
float toAngle = CenterForAnglesInRange + BeamOpeningForAnglesInRange / 2;
if (fromAngle < 0)
{
fromAngle += 360;
}
if (toAngle > 360)
{
fromAngle -= 360;
}
SelectedAngleRange = $"From {fromAngle} to {toAngle}";
HorizontalPointsInRange = await Task.Run(() => Lidar.GetHorizontalPointsInRange(fromAngle, toAngle, Lidar.Config.DefaultVerticalAngle));
}
}
public PagePandaLidar()
{
InitializeComponent();
_measureObjects = null;
_selectedLidar = null;
picWorld.Dimensions.SetZoomFactor(5);
_enableGroup = false;
_enableBoard = true;
_showPoints = true;
_showLines = false;
_running = false;
}
private void WalkOnChild(Transform transform)
{
foreach (Transform t in transform)
{
GameObject go = t.gameObject;
kinematics = GetComponentOrDefault(go, kinematics);
RobotLidar = GetComponentOrDefault(go, RobotLidar);
manualController = GetComponentOrDefault(go, manualController);
RobotCamera = GetComponentOrDefault(go, RobotCamera, "Camera", false);
FirstPersonCamera = GetGameObjectOrDefault(go, FirstPersonCamera, "FirstPersonCamera");
ThirdPersonCamera = GetGameObjectOrDefault(go, ThirdPersonCamera, "ThirdPersonCamera");
TopCamera = GetGameObjectOrDefault(go, TopCamera, "TopCamera");
WalkOnChild(t);
}
}
public override void OnInspectorGUI()
{
_lidar = this.target as Lidar;
base.OnInspectorGUI();
EditorGUILayout.BeginHorizontal();
GUILayout.Label("Range: ");
_lidar.Range = float.Parse(GUILayout.TextField(_lidar.Range.ToString()));
EditorGUILayout.EndHorizontal();
_lidar.IsDebug = EditorGUILayout.Toggle("Debug: ", _lidar.IsDebug);
if (_lidar.IsDebug)
{
GUILayout.BeginHorizontal();
GUILayout.Label("Color: ");
_lidar.ColorDebug = EditorGUILayout.ColorField(_lidar.ColorDebug);
GUILayout.EndHorizontal();
}
}
//UDP_Host networkHost;
public IO_Manager(MainWindow form)
{
//use parent variable to change Main Form
parent = form;
//setup host
//networkHost = new UDP_Host();
//networkHost.RegisterID(SampleMessage, "POST");
//networkHost.StartServer();
//Create Thread to get data
ThreadStart thr = new ThreadStart(this.Process);
oThread = new Thread(thr);
try
{
Arduino = new SerialPort("COM5", 9600);
Arduino.DataReceived += Arduino_DataReceived;
Arduino.Open();
}
catch { }
//send function to lidar constructor to assign function to delegate
l = new Lidar("COM6", 115200, this.DisplayLidarData);
if (l.isOpen())
{
parent.setLidarStatus("Close", "COM5", 115200);
}
else
{
parent.setLidarStatus("Open", "COM5", 115200);
}
//Create GPS object
//gpsUnit = new GPS("COM7", 9600);
controller = new Manual();
oThread.Start();
}
void OnPostRender()
{
if (GlobalState.drawLidar == true)
{
if (car == null)
{
return;
}
if (lidar == null)
{
lidar = car.GetComponentInChildren <Lidar>();
return;
}
else if (lidar.enabled == false)
{
return;
}
Draw();
}
}
/// <summary>
/// Configures the point for plotting
/// </summary>
/// <param name="ellipse"></param>
/// <param name="point"></param>
private void ConfigureEllipse(ref Ellipse ellipse, Point point)
{
var polar = Lidar.ConvertToPolar(point);
// Set the color based on how close the point is to the robot
var ellipseColor = Colors.White;
if (controller.Analysis.IsInStopZone(polar.Angle, polar.Distance))
{
ellipseColor = Colors.Red;
}
else if (controller.Analysis.IsInAdjustZone(polar.Angle, polar.Distance))
{
ellipseColor = Colors.Yellow;
}
ellipse.Width = ellipse.Height = 25;
ellipse.Fill = new SolidColorBrush(ellipseColor);
ellipse.Name = "point";
}
public void AddSensorToAgent(string typeSensor, string nom, AAgent agent)
{
GameObject sensor = new GameObject();
sensor.transform.parent = agent.transform;
sensor.transform.position = agent.transform.position;
if (typeSensor == "Lidar")
{
Lidar lidar = Lidar.CreateComponent(sensor, nom);
agent.Sensors.Add(lidar);
agent.AddLidarListner(lidar);
}
if (typeSensor == "RFID")
{
RFID rfid = RFID.CreateComponent(sensor, nom);
rfid.RfidTag = RFID_Tags.Agent;
agent.Sensors.Add(rfid);
agent.AddRFIDListner(rfid);
}
}
void Start()
{
if (kinematics == null)
{
kinematics = GetComponent <DifferentialKinematics>();
}
if (WorldCameraSelector == null)
{
WorldCameraSelector = GetComponent <CameraSelector>();
}
if (RobotCamera == null)
{
RobotCamera = Camera.main;
}
if (RobotLidar == null)
{
RobotLidar = GetComponent <Lidar>();
}
receiveBytes = new byte[921600];
Connected = false;
ClientName = "";
GetCameraImage();
IPAddress ipAddress = IPAddress.Parse("127.0.0.1");
tcpServer = new TcpListener(ipAddress, Port);
tcpServer.Server.SendTimeout = 1000;
tcpServer.Server.ReceiveTimeout = 1000;
tcpServer.Start();
running = true;
thread = new Thread(new ThreadStart(ServerThread));
thread.Start();
}
public PageLidar()
{
InitializeComponent();
_lastMeasure = null;
_selectedLidar = null;
}
public void AddLidarListner(Lidar lidar)
{
lidar.OnNearWallDetected += HandleOnNearWallDetected;
lidar.OnNearWallEscaped += HandleOnNearWallEscaped;
lidar.OnNoWallInFront += HandleOnNoWallInFront;
}
private void Start()
{
map = GetComponentInParent <MiniMap>();
myRectTransform = GetComponent <RectTransform>();
lidar = GetComponent <Lidar>();
}
// Start is called before the first frame update
void Start()
{
// Wait for evaluation metrix to initialize first
Evaluation evalScript = GameObject.Find("Evaluation").GetComponent <Evaluation>();
string evalType = evalScript.GetEvalType();
Debug.Log(evalType);
if (evalType == "advanced")
{
evalAdvanced = true;
}
else if (evalType == "advanced_triangulate")
{
evalAdvanced = true;
evalTriangulate = true;
}
// Define the world boundary based on camera projection
X_LENGTH = 2 * main_camera.orthographicSize * Screen.width / Screen.height;
Y_LENGTH = 2 * main_camera.orthographicSize;
// Number of particles is the amount of particles that can cover the map
NUM_PARTICLES = (int)X_LENGTH * (int)Y_LENGTH;
// Set random seed
Random.InitState(SEED);
// Set initial position
_agentPosition = transform.position;
// Get Lidar script
_lidar_script = GetComponent <Lidar>();
_lidar_precision_radians = _lidar_script.GetLidarPrecisionRadians();
// Initialize particles
_beliefStates = new GameObject[NUM_PARTICLES];
_weights = new float[NUM_PARTICLES];
_particleParent = new GameObject();
_particleParent.name = gameObject.name + "_particleParent";
for (int i = 0; i < (int)X_LENGTH; i++)
{
for (int j = 0; j < (int)Y_LENGTH; j++)
{
float x = i - (int)X_LENGTH / 2;
float y = j - (int)Y_LENGTH / 2;
GameObject particle = Instantiate(particlePrefab, new Vector3(x, y, -2), Quaternion.identity);
particle.transform.SetParent(_particleParent.transform);
_beliefStates[i + j * (int)X_LENGTH] = particle;
_weights[i + j * (int)X_LENGTH] = 0.01f;
}
}
// Create belief position
_beliefPosition = Instantiate(beliefPositionPrefab, new Vector3(0, 0, -2), Quaternion.identity);
_beliefPosition.GetComponent <Renderer>().materials = GetComponent <Renderer>().materials;
// Find other agents for cooperative localization
_agents = GameObject.FindGameObjectsWithTag("agents");
}
public SimulatedLidarPacketTransmitter(Lidar distances)
{
_distances = distances;
}
public SimulatedUltrasoundSensor(Lidar distances, float sensorOffsetFwd, float sensorOffsetSide)
{
_distances = distances;
_sensorOffsetFwd = sensorOffsetFwd;
_sensorOffsetSide = sensorOffsetSide;
}
void OnEnable()
{
_lidar = (Lidar)target;
}
