private void InitializeMessage()
{
laserScanReader = LaserScannerObject.GetComponent <LaserScanReader>();
scanPeriod = (float)laserScanReader.samples / (float)laserScanReader.update_rate;
message = new Messages.Sensor.LaserScan
{
header = new Messages.Standard.Header {
frame_id = FrameId
},
angle_min = laserScanReader.angle_min,
angle_max = laserScanReader.angle_max,
angle_increment = laserScanReader.angle_increment,
time_increment = laserScanReader.time_increment,
range_min = laserScanReader.range_min,
range_max = laserScanReader.range_max,
ranges = laserScanReader.ranges,
intensities = laserScanReader.intensities
};
}
void Update()
{
if (!lidar)
{
lidar = Globals.Instance.CurrentCar.lidar;
balanceHits();
}
//Reached updatefrequency threshhold, read new value
if (Time.time - stime > 1.0f / updateFrequency)
{
balanceHits();
for (int i = 0; i < lidar.samples; i++)
{
float range = lidar.ranges[i];
if (range >= lidar.range_min && range <= lidar.range_max)
{
float angle = lidar.angle_min + lidar.angle_increment * i;
Vector3 pos = new Vector3(range * Mathf.Cos(angle), range * Mathf.Sin(angle), 0.0f);
if (!relativePosition)
{
pos += new Vector3(lidar.transform.position.x, lidar.transform.position.z, 0.0f); //x and y of transform are different than on HUD
}
hits[i].anchoredPosition3D = toCanvasSpace(pos);
if (xAxis.isValid(pos.x) && yAxis.isValid(pos.y))
{
hits[i].gameObject.SetActive(true);
}
else
{
hits[i].gameObject.SetActive(false);
}
}
else
{
hits[i].gameObject.SetActive(false);
}
}
stime = Time.time;
}
}
public override void set_reference(Transform o)
{
reference = o;
ls_pub = o.GetComponent <RosSharp.RosBridgeClient.LaserScanPublisher>();
ls_reader = o.GetComponent <RosSharp.RosBridgeClient.LaserScanReader>();
}
