private void InitLaserScanMsg(sensor_msgs.msg.LaserScan laserScanMessage)
{
int numLines = (int)Mathf.Round(m_ApertureAngle / m_AngularResolution) + 1;
float timeIncrement = 0.0f;
if (m_UseTimeIncrement)
{
timeIncrement = 1 / ((float)m_ScanningFrequency * (float)numLines);
}
laserScanMessage.header.frame_id = m_ScanLink.name;
laserScanMessage.angle_min = Mathf.Deg2Rad * -m_ApertureAngle / 2;
laserScanMessage.angle_max = -laserScanMessage.angle_min;
laserScanMessage.angle_increment = Mathf.Deg2Rad * m_AngularResolution;
laserScanMessage.time_increment = timeIncrement;
laserScanMessage.range_max = m_RangeMaximum;
laserScanMessage.range_min = m_RangeMinimum;
List <float> ranges = new List <float>();
List <float> intensities = new List <float>();
for (int index = 0; index < m_NumLines; index++)
{
ranges.Add(0.0f);
intensities.Add(0.0f);
}
laserScanMessage.ranges = ranges;
laserScanMessage.intensities = intensities;
}
private sensor_msgs.msg.LaserScan CreateLaserScanMessage()
{
UpdateNumLines();
float timeIncrement = 0.0f;
if (UseTimeIncrement)
{
timeIncrement = 1.0f / (ScanningFrequency * numLines);
}
var msg = new sensor_msgs.msg.LaserScan
{
Angle_min = -Mathf.Deg2Rad * ApertureAngle / 2.0f,
Angle_max = Mathf.Deg2Rad * ApertureAngle / 2.0f,
Angle_increment = Mathf.Deg2Rad * AngularResolution,
Time_increment = timeIncrement,
Range_max = RangeMaximum,
Range_min = RangeMinimum,
Ranges = new float[numLines],
Intensities = new float[numLines],
};
msg.Header.Frame_id = ScanLinkName;
return(msg);
}
private void UpdateRanges(sensor_msgs.msg.LaserScan laserScanMessage)
{
List <float> ranges = new List <float>();
for (int index = 0; index < m_NumLines; index++)
{
ranges.Add(GetDistance(index, false));
}
laserScanMessage.ranges = ranges;
}
IEnumerator PublishScansIfOldEnough()
{
for (;;)
{
if (scanMsgQueue.Count > 0 && IsOldEnough(scanMsgQueue.Peek()))
{
lastSentScanMsg = scanMsgQueue.Dequeue();
scanPublisher.Publish(lastSentScanMsg);
}
yield return(new WaitForSeconds(0.1f / ScanningFrequency));
}
}
new void FixedUpdate()
{
if (m_LastPublishTime.Delay(1.0f / m_ScanningFrequency).IsInThePast)
{
m_LastPublishTime = m_Clock.Now;
sensor_msgs.msg.LaserScan laserScanMessage = new sensor_msgs.msg.LaserScan();
InitLaserScanMsg(laserScanMessage);
UptateLaserScanMessage(laserScanMessage);
m_LaserScanMessageQueue.Enqueue(laserScanMessage);
}
PublishScansOlderThan(m_PublisherDelay);
base.FixedUpdate();
}
protected override void StartRos()
{
shouldScan = false;
if (ScanLink == null)
{
ScanLink = transform;
}
if (ScanLinkName == "")
{
ScanLinkName = ScanLink.name;
}
scanMsgQueue = new Queue <sensor_msgs.msg.LaserScan>();
lastSentScanMsg = CreateLaserScanMessage();
scanPublisher = node.CreatePublisher <sensor_msgs.msg.LaserScan>(ScanTopic);
StartCoroutine("TriggerScan");
StartCoroutine("PublishScansIfOldEnough");
}
protected override void StartRos()
{
frameID = "base_footprint";
angleRes = 0.9f;
oneCycleNumOfReadings = Mathf.RoundToInt(360 / angleRes);
dist = new float[oneCycleNumOfReadings];
angle = new float[oneCycleNumOfReadings];
measurmentTime = 0.00025f;
//rotationFrequency = 1f / (((maxAngle - minAngle) / angleRes) * measurmentTime); // Not used anywhere
samplesPerPhysStep = Mathf.RoundToInt(Time.fixedDeltaTime / measurmentTime);
currentAngle = minAngle;
sensorRotator.localEulerAngles = new Vector3(0, currentAngle, 0);
azimuth = new float[samplesPerPhysStep];
intensities = new float[oneCycleNumOfReadings];
LaserScanPublisher = node.CreatePublisher <sensor_msgs.msg.LaserScan>(scanTopic);
message = new sensor_msgs.msg.LaserScan();
message.Ranges = new float[oneCycleNumOfReadings];
}
private bool IsOldEnough(sensor_msgs.msg.LaserScan msg)
{
return(msg.Header.IsOlderThan(PublisherDelay, clock));
}
private void UptateLaserScanMessage(sensor_msgs.msg.LaserScan laserScanMessage)
{
laserScanMessage.header.Update(m_Clock);
UpdateRanges(laserScanMessage);
}