public void RosTimeDelay()
{
Clock clock = new Clock();
RosTime oneSecond = clock.CreateRosTime(1.0d);
RosTime delayedTime = oneSecond.Delay(1.0d);
Assert.That(delayedTime.Seconds, Is.EqualTo(2.0d));
delayedTime = oneSecond.Delay(3.4d);
Assert.That(delayedTime.Seconds, Is.EqualTo(4.4d));
}
new void FixedUpdate()
{
if (lastPublishTime.Delay(1.0f / PublisherFrequency).IsInThePast)
{
lastPublishTime = clock.Now;
Vector3 unityPosition = Quaternion.Inverse(ParentFrame.rotation) * (ChildFrame.position - ParentFrame.position);
message.pose.position.Unity2Ros(unityPosition);
//TODO(sam): Figure out why z is y in numpy quaternion
message.pose.orientation.Unity2Ros(Quaternion.Inverse(ParentFrame.rotation) * ChildFrame.rotation);
message.header.Update(clock);
publisher.Publish(message);
}
}
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();
}
new void FixedUpdate()
{
if (lastPublishTime.Delay(1.0f / PublisherFrequency).IsInThePast)
{
lastPublishTime = clock.Now;
RaycastHit hit;
if (Physics.Raycast(SensorLink.position, ray, out hit, Mathf.Infinity, LayerMaskDistanceSensor))
{
distance = hit.distance;
}
rangeMessage.header.Update(clock);
rangeMessage.range = distance;
publisher.Publish(rangeMessage);
}
}
