public void Update()
{
var points = lidar.Scan();
var pcd = new Messages.Sensor.PointCloud2();
// create a byte array and copy the floats into it...
var byteArray = new byte[points.Length * 4];
Buffer.BlockCopy(points, 0, byteArray, 0, byteArray.Length);
var point_x = new PointField {
datatype = PointField.FLOAT32, name = "x", offset = 0, count = 1
};
var point_y = new PointField {
datatype = PointField.FLOAT32, name = "y", offset = 4, count = 1
};
var point_z = new PointField {
datatype = PointField.FLOAT32, name = "z", offset = 8, count = 1
};
pcd.header.frame_id = "M8_Fix_Reference";
pcd.data = byteArray;
pcd.fields = new PointField[] { point_x, point_y, point_z };
pcd.is_dense = true;
pcd.is_bigendian = false;
pcd.width = (uint)(int)byteArray.Length / 12;
pcd.height = 1;
pcd.point_step = 12;
pcd.row_step = pcd.point_step * pcd.width;
Publish(pcd);
}
IEnumerator Process()
{
while (true)
{
if (lidar.IsInitialized())
{
if (Time.time >= elapsedTime)
{
elapsedTime = Time.time + 1.0f / rate;
lidar.Scan();
PublishVelodyneData();
}
}
yield return(null);
}
}
// Update belief state with new particles
void UpdateBelief()
{
// Get observation from Lidar and action(displacement) of the agent
float[] agent_observation = _lidar_script.GetDistanceObservation();
Vector3 action = GetAction();
// Create _weights for each sampled particle
_weights = new float[_beliefStates.Length];
Vector3[] sampledPositions = new Vector3[_beliefStates.Length];
bool has_high_confidence = false;
// Sample from belief states
for (int i = 0; i < _beliefStates.Length; i++)
{
int randomParticleIndex = Random.Range(0, _beliefStates.Length);
GameObject randomParticle = _beliefStates[randomParticleIndex];
// Update the sampled particle's position by taking the action performed by the agent
Vector3 newParticlePosition = randomParticle.transform.position + action;
// sampledPositions[i] = randomParticle.transform.position;
sampledPositions[i] = newParticlePosition;
float[] particle_observation = _lidar_script.Scan(newParticlePosition + new Vector3(0, 0, 2));
int numRayMatches = 0;
int numRayObserved_agent = 0;
int numRayObserved_particle = 0;
// Compare particle observation with agent observation
for (int j = 0; j < agent_observation.Length; j++)
{
// If the observation is positive and within threshold then increment match counts
if ((agent_observation[j] > 0 && particle_observation[j] > 0 &&
Mathf.Abs(agent_observation[j] - particle_observation[j]) < observation_threshold))
{
numRayMatches++;
}
// Count how many rays are observed by the agent
if (agent_observation[j] != -1)
{
numRayObserved_agent++;
}
// Count how many rays are observed by the particle
if (particle_observation[j] != -1)
{
numRayObserved_particle++;
}
}
// Heavily pentalize particle weight if the particle observed more than the agent
if (numRayObserved_particle > numRayObserved_agent)
{
numRayObserved_agent = 2 * Mathf.Max(numRayObserved_agent, numRayObserved_particle);
}
// Weight is the percentage of rays that matched between agent and particle observations
_weights[i] = (float)(numRayMatches + 1) / (numRayObserved_agent + 1);
if (_weights[i] > upper_confidence_threshold)
{
has_high_confidence = true;
}
}
// If there's at least one particle with high confidence or all particles are low confidence, keep noise level normal. Otherwise increase noise.
// Noise is increased when there are observations but none of the particles match the observations.
float multiplier = has_high_confidence ? 1 : noise_multiplier;
// Resample using the _weights and update the belief
for (int i = 0; i < _beliefStates.Length; i++)
{
int randomIndex = WeightedRandomIndex(ref _weights);
Vector3 noise = new Vector3(SampleNormal(NOISE_MEAN, multiplier * NOISE_STD), SampleNormal(NOISE_MEAN, multiplier * NOISE_STD), 0);
_beliefStates[i].transform.position = sampledPositions[randomIndex] + noise;
}
//Update beliefs with information from other agents
if (evalAdvanced)
{
MultiAgentUpdate();
}
}
