/*
*createSpot function
*
*p : a pointer to an array of strings represinting a spot
*
*this function will take an array of strings represinting a spot
*and return a struct spot
*/
struct spot *createSpot(char **p)
{
struct spot *s_s;
char *temp;
char **params;
int now;
int n = 0;
s_s = malloc(sizeof(struct spot));
assert(s_s != 0);
now = numberOfWords(p[n]);
assert(now == 2);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
s_s -> spot_id = createId(params[1]);
freeCharArray(params, now);
n++;
temp = "spot_width";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
if(compTwoStrings(params[0], temp)){
assert(now == 2);
s_s -> spot_width = atoi(params[1]);
n++;
}
else
s_s -> spot_width = -1;
freeCharArray(params, now);
temp = "checkpoint";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
if(compTwoStrings(params[0], temp)){
assert(now == 3);
s_s -> check_waypoint = createCheckpoint(p[n]);
n++;
}
else
s_s -> check_waypoint = 0;
freeCharArray(params, now);
s_s -> waypoint1= createWaypoint(p[n++]);
s_s -> waypoint2= createWaypoint(p[n]);
return s_s;
}
void ParseScenario::handleXmlStartElement()
{
// New waypoint definition
if (xmlReader.name() == "waypoint")
{
createWaypoint();
}
// New agents to add to scenario
else if (xmlReader.name() == "agent")
{
createAgents();
}
// Add waypoint that was defined earlier by "createWaypoint"
// to all agents
else if (xmlReader.name() == "addwaypoint")
{
// Get waypoint by id
QString id = readString("id");
addWaypointToCurrentAgents(id);
}
else
{
// nop, unknown, ignore
}
}
/*
*createLane function
*
*p : a pointer to an array of strings represinting a lane
*
*this function will take an array of strings represinting a lane
*and return a struct lane
*/
struct lane *createLane(char **p)
{
struct lane *l;
char *temp;
char **params;
int now;
int n = 0;
int i;
int numOfCkPts;
int numOfSps;
int numOfEts;
l = malloc(sizeof(struct lane));
assert( l != 0);
now = numberOfWords(p[n]);
assert(now == 2);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
l -> lane_id = createId(params[1]);
freeCharArray(params, now);
n++;
now = numberOfWords(p[n]);
assert(now == 2);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
l -> number_of_waypoints = atoi(params[1]);
freeCharArray(params, now);
n++;
temp = "lane_width";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
if(compTwoStrings(params[0], temp)){
assert(now == 2);
l -> lane_width = atoi(params[1]);
n++;
}
else
l -> lane_width = -1;
freeCharArray(params, now);
temp = "left_boundary";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
if(compTwoStrings(params[0], temp)){
assert(now == 2);
l -> left_boundary = malloc(MAX_STRING_LENGTH * sizeof(char));
stringCopy(params[1],l -> left_boundary);
n++;
}
else
l -> left_boundary = 0;
freeCharArray(params, now);
temp = "right_boundary";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
if(compTwoStrings(params[0], temp)){
assert(now == 2);
l -> right_boundary = malloc(MAX_STRING_LENGTH * sizeof(char));
stringCopy(params[1],l -> right_boundary);
n++;
}
else
l -> right_boundary = 0;
freeCharArray(params, now);
numOfCkPts = 0;
temp = "checkpoint";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
while(compTwoStrings(params[0], temp)){
assert(now == 3);
freeCharArray(params, now);
numOfCkPts++;
now = numberOfWords(p[n + numOfCkPts]);
params = divideLine(p[n + numOfCkPts], now, MAX_STRING_LENGTH);
assert(params != 0);
}
freeCharArray(params, now);
l -> number_of_checkpoints = numOfCkPts;
if(numOfCkPts > 0)
l -> checkpoints_list = malloc(numOfCkPts * sizeof(struct checkpoint));
else
l -> checkpoints_list = 0;
for(i = 0; i < numOfCkPts; i++){
l -> checkpoints_list[i] = createCheckpoint(p[n]);
n++;
}
numOfSps = 0;
temp = "stop";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
while(compTwoStrings(params[0], temp)){
assert(now == 2);
freeCharArray(params, now);
numOfSps++;
now = numberOfWords(p[n + numOfSps]);
params = divideLine(p[n + numOfSps], now, MAX_STRING_LENGTH);
assert(params != 0);
}
freeCharArray(params, now);
l -> number_of_stops = numOfSps;
if(numOfSps > 0)
l -> stops_list = malloc(numOfSps * sizeof(struct stop));
else
l -> stops_list = 0;
for(i = 0; i < numOfSps; i++){
l -> stops_list[i] = createStop(p[n]);
n++;
}
numOfEts = 0;
temp = "exit";
now = numberOfWords(p[n]);
params = divideLine(p[n], now, MAX_STRING_LENGTH);
assert(params != 0);
while(compTwoStrings(params[0], temp)){
assert(now == 3);
freeCharArray(params, now);
numOfEts++;
now = numberOfWords(p[n + numOfEts]);
params = divideLine(p[n + numOfEts], now, MAX_STRING_LENGTH);
assert(params != 0);
}
freeCharArray(params, now);
l -> number_of_exits = numOfEts;
if(numOfEts > 0)
l -> exits_list = malloc(numOfEts * sizeof(struct stop));
else
l -> exits_list = 0;
for(i = 0; i < numOfEts; i++){
l -> exits_list[i] = createExit(p[n]);
n++;
}
l -> waypoints_list = malloc(
(l -> number_of_waypoints) *
sizeof(struct waypoint));
assert((l -> waypoints_list) != 0);
for(i = 0; i < l -> number_of_waypoints; i++){
l -> waypoints_list[i] = createWaypoint(p[n]);
n++;
}
return l;
}
// update waypoint and path actions UAV objects
//
// waypoints are unique and each waypoint has an entry in the UAV waypoint list
//
// a path action can be referenced by multiple waypoints
// waypoints reference path action by their index in the UAV path action list
// the compression of path actions happens here.
// (compression consists in keeping only one instance of similar path actions)
//
// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
bool ModelUavoProxy::modelToObjects()
{
qDebug() << "ModelUAVProxy::modelToObjects";
// track number of path actions
int actionCount = 0;
// iterate over waypoints
int waypointCount = myModel->rowCount();
for (int i = 0; i < waypointCount; ++i) {
// Path Actions
// create action to use as a search criteria
// this object does not need to be deleted as it will either be added to the managed list or deleted later
PathAction *action = new PathAction;
// get model data
PathAction::DataFields actionData = action->getData();
modelToPathAction(i, actionData);
// see if that path action has already been added in this run
PathAction *foundAction = findPathAction(actionData, actionCount);
// TODO this test needs a consistency check as it is unsafe.
// if the find method is buggy and returns false positives then the flight plan sent to the uav is broken!
// the find method should do a "binary" compare instead of a field by field compare
// if a field is added everywhere and not in the compare method then you can start having false positives
if (!foundAction) {
// create or reuse an action instance
action = createPathAction(actionCount, action);
actionCount++;
// update UAVObject
action->setData(actionData);
} else {
action->deleteLater();
action = foundAction;
qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
}
// Waypoints
// create or reuse a waypoint instance
Waypoint *waypoint = createWaypoint(i, NULL);
Q_ASSERT(waypoint);
// get model data
Waypoint::DataFields waypointData = waypoint->getData();
modelToWaypoint(i, waypointData);
// connect waypoint to path action
waypointData.Action = action->getInstID();
// update UAVObject
waypoint->setData(waypointData);
}
// Put "safe" values in unused waypoint and path action objects
if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
// TODO
}
}
if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
// TODO
}
}
// Update PathPlan
PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
PathPlan::DataFields pathPlanData = pathPlan->getData();
pathPlanData.WaypointCount = waypointCount;
pathPlanData.PathActionCount = actionCount;
pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
pathPlan->setData(pathPlanData);
return true;
}
