////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Generates fake waypoints to simulate control unit for testing.
///
////////////////////////////////////////////////////////////////////////////////////
void RobotSim::UseFakeWaypoints()
{
JAUS::Address controllerID(42, 1, 1);
mLocalWaypointDriverComponent.AccessControlService()->SetControllerID(controllerID);
mLocalWaypointDriverComponent.ManagementService()->SetStatus(JAUS::Management::Status::Ready);
JAUS::SetElement command(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
command.SetRequestID(1);
JAUS::Point3D::List localWaypoints;
// Add more waypoints for testing as needed.
localWaypoints.push_back(JAUS::Point3D(10, 0, 0));
localWaypoints.push_back(JAUS::Point3D(10, 10, 0));
localWaypoints.push_back(JAUS::Point3D(0, 10, 0));
localWaypoints.push_back(JAUS::Point3D(0, 0, 0));
for(unsigned int i = 0; i < (unsigned int)localWaypoints.size(); i++)
{
JAUS::Element e;
// Set the element ID.
e.mID = i + 1;
// If this is the last element (and we are not looping) set the
// next ID to 0, otherwise the value of the next element ID.
if(i < (unsigned int)localWaypoints.size() - 1)
{
e.mNextID = e.mID + 1;
}
// Set previous element.
e.mPrevID = i;
// Populate the element message which is the command to be executed.
JAUS::SetLocalWaypoint* message = new JAUS::SetLocalWaypoint();
message->SetX(localWaypoints[i].mX);
message->SetY(localWaypoints[i].mY);
// Save the pointer
e.mpElement = message;
// Push completed element onto the list.
command.GetElementList()->push_back(e);
}
// Set the command.
mLocalWaypointDriverComponent.TransportService()->Receive(&command);
// Now tell it to start executing.
JAUS::ExecuteList executeCommand(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
executeCommand.SetElementUID(1); // Start at beginning.
executeCommand.SetSpeed(2.0); // Maximum speed.
mLocalWaypointDriverComponent.TransportService()->Receive(&executeCommand);
}
