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(); } }