void Update()
{
if (poseState == PoseStates.Initial)
{
CreatePedestrians();
poseState = PoseStates.Waiting;
}
if (poseState == PoseStates.Ready)
{
poseState = PoseStates.Waiting;
UpdatePedestrians();
}
}
private void HandlePosesUpdated(geometry_msgs.PoseArray poses)
{
if (rosReady)
{
if (firstSignal)
{
Debug.Log("First pose aquired");
poseState = PoseStates.Initial;
numPedestrians = poses.Count;
firstSignal = false;
}
else
{
poseState = PoseStates.Ready;
}
poseArray = poses;
}
}
// <summary>
// HandlePosesUpdated is a callback function from the ROS Subscriber that handles incoming messages.
// When a message is initially recieved, positional data about the pedestrians are updated, but the GameObjects
// are not updated, because you cannot make UI calls within a thread other than the main thread (Update)
// </summary>
private void HandlePosesUpdated(SyntheticPeds message)
{
if (rosReady) // don't start creating peds until mapping is finished and ROS connected
{
if (firstSignal)
{
firstSignal = false;
numPedestrians = message.poses.Count;
for (int i = 0; i < numPedestrians; i++) // Create new pedestrians and fill in data that only needs to be read once
{
pedestrians.Add(new Pedestrian(message.ids[i], message.radii[i]));
}
Debug.Log("First pose aquired");
}
else
{
for (int i = 0; i < numPedestrians; i++) // update values of all pedestrians
{
pedestrians[i].id = message.ids[i];
pedestrians[i].pose = message.poses[i];
pedestrians[i].velocity = message.velocities[i];
pedestrians[i].goalPosition = message.goal_positions[i];
pedestrians[i].prefSpeed = pedestrians[i].prefSpeed;
}
if (poseState == PoseStates.NotReady) // We do not want to create pedestrians in CreatePedestrians() until their vals have been initialized
{
poseState = PoseStates.Initial;
}
else if (pedestriansCreated)
{
poseState = PoseStates.Ready;
}
}
}
}