//////////////////////////////////////////////////////////////////////////////////// /// /// \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); }