public static void visualizeLane(lane laneViz)
{
LaneArray wpLaneArray2 = new LaneArray();
lane[] laneArraViz2 = new lane[1];
laneArraViz2[0] = laneViz;
wpLaneArray2.lane = laneArraViz2;
rosSocket.Publish(publish_laneArray, wpLaneArray2);
}
public static lane[] laneMsgDataSplit(float[] xs, float[] ys, float[] zs, float[] vels, float[] yaws, int seqCounter, int countOfWp)
{
lane lane1 = new lane();
lane lane2 = new lane();
waypoint[] wps1 = new waypoint[countOfWp / 2];
waypoint[] wps2 = new waypoint[countOfWp / 2];
lane1.header.frame_id = "/map";
lane2.header.frame_id = "/map";
for (int j = 0; j < countOfWp; j++)
{
waypoint wp = new waypoint();
wp.pose.pose.position.x = xs[j];
wp.pose.pose.position.y = ys[j];
wp.pose.pose.position.z = zs[j];
if (isTimeNegative)
{
wp.twist.twist.linear.x = 0;
}
{
wp.twist.twist.linear.x = vels[j];
}
wp.twist.twist.linear.y = 0f;
wp.twist.twist.linear.z = 0f;
Quaternion quaternionYaw = new Quaternion();
GeometryQuaternion rot = new GeometryQuaternion();
quaternionYaw = Quaternion.CreateFromYawPitchRoll(yaws[j], 0, 0);
rot.w = quaternionYaw.W;
rot.x = quaternionYaw.X;
rot.y = quaternionYaw.Y;
rot.z = quaternionYaw.Z;
wp.pose.pose.orientation = rot;
if (j < (countOfWp / 2))
{
wps1[j] = wp;
}
else
{
wps2[j - (countOfWp / 2)] = wp;
}
}
lane1.header.seq = seqCounter;
seqCounter = seqCounter + 1;
lane2.header.seq = seqCounter;
Console.WriteLine("Seq {0}", lane2.header.seq);
lane1.waypoints = wps1;
lane2.waypoints = wps2;
lane[] laneArra = new lane[2];
laneArra[0] = lane1;
laneArra[1] = lane2;
return(laneArra);
}
private void SpawnUnit(bool left)
{
float random = UnityEngine.Random.Range(0, 3);
var spawnedEnemy = Instantiate(_enemyPrefab, new Vector3(transform.position.x, transform.position.y, random), Quaternion.identity);
spawnedEnemy.GetComponent <EnemyBase>()._left = left;
spawnedEnemy.GetComponent <EnemyBase>().gate = gate;
_lane = (lane)random;
spawnedEnemy.tag = Enum.GetName(typeof(lane), _lane);
}
public static int calculateTimeTravelingBetweenWaypoints(lane laneIn, waypoint currenWaypoint)
{
double timeToTravel = 0;
for (int i = 0; i < laneIn.waypoints.Length - 1; i++)
{
if (i == 0)
{
timeToTravel = timeToTravel + calculateTravelingTimeToNextWaypoint(currenWaypoint, laneIn.waypoints[i], laneIn.waypoints[i].twist.twist.linear.x);
}
else
{
timeToTravel = timeToTravel + calculateTravelingTimeToNextWaypoint(laneIn.waypoints[i], laneIn.waypoints[i + 1], laneIn.waypoints[i].twist.twist.linear.x);
}
}
return((int)timeToTravel);
}
public static lane laneMsgData(float[] xs, float[] ys, float[] zs, float[] vels, float[] yaws, int seqCounter, int countOfWp)
{
lane lane = new lane();
waypoint[] wps = new waypoint[countOfWp];
lane.header.frame_id = "/map";
for (int j = 0; j < countOfWp; j++)
{
waypoint wp = new waypoint();
wp.pose.pose.position.x = xs[j];
wp.pose.pose.position.y = ys[j];
wp.pose.pose.position.z = zs[j];
wp.twist.twist.linear.x = vels[j];
wp.twist.twist.linear.y = 0f;
wp.twist.twist.linear.z = 0f;
Quaternion quaternionYaw = new Quaternion();
GeometryQuaternion rot = new GeometryQuaternion();
quaternionYaw = Quaternion.CreateFromYawPitchRoll(yaws[j], 0, 0);
rot.w = quaternionYaw.W;
rot.x = quaternionYaw.X;
rot.y = quaternionYaw.Y;
rot.z = quaternionYaw.Z;
wp.pose.pose.orientation = rot;
wps[j] = wp;
}
lane.header.seq = seqCounter;
Console.WriteLine("Seq {0}", lane.header.seq);
lane.waypoints = wps;
return(lane);
}
/*
* ChangeLanes will:
* 1. Check to see if left and/or right lanes are open for changing (i.e.
* 2.
* 2.
*/
void RandomlyChangeLanes()
{
List <bool> lanesClearList = lanesClear();
bool leftClear = lanesClearList[0];
bool rightClear = lanesClearList[1];
//Debug.Log ("left: " + lanesClearList[0] + ", right: " + lanesClearList[1]);
Vector3 pos = transform.position;
switch (aLane)
{
case lane.inside: //lane before is inside, can only change to middle (this is allowed to happen ANYTIME, unlike middle and outside
if (leftClear)
{
if (((pos.x == 5.5 || pos.x == 31.5) && pos.z > 3.5 && pos.z < 16.5) ||
((pos.z == 3.5 || pos.z == 17.5) && pos.x > 6.5 && pos.x < 30.5))
{
aLane = lane.middle;
transform.Translate(transform.right * -1, Space.World); //does actual shift
//Debug.Log ("Inside works: before = " + pos + ", after" + transform.position);
}
//else Debug.Log ("inside fail: " + pos.x + ", " + pos.z);
}
//Debug.Log("insidefail: " + pos.x + ", " + pos.z);
break;
case lane.middle: //lane before is middle
if (((pos.x == 4.5 || pos.x == 32.5) && pos.z > 3.5 && pos.z < 16.5) ||
((pos.z == 2.5 || pos.z == 18.5) && pos.x > 6.5 && pos.x < 30.5)) //this code checks to make sure
{
if (leftClear && !rightClear) //go to the left!
{
aLane = lane.outside;
transform.Translate(-1 * transform.right, Space.World);
}
else if (!leftClear && rightClear) //go to the right!
{
aLane = lane.inside;
transform.Translate(transform.right, Space.World);
}
else if (leftClear && rightClear) //randomly pick whether to go to the left or right
{
int rand = Random.Range(0, 2); //if rand is 0 go left, else go right
if (rand == 0)
{
aLane = lane.outside;
transform.Translate(-1 * transform.right, Space.World);
}
else
{
aLane = lane.inside;
transform.Translate(transform.right, Space.World);
}
}
else if (!leftClear && !rightClear) //do nothing, can't change lanes because another zombie is blocking!
{
//Debug.Log ("Cant change lanes, target lane(s) occupied by zombies!");
}
//Debug.Log ("Mid works: before = " + pos + ", after" + transform.position + " and lane is: " + aLane);
}
//else Debug.Log ("middle fail: " + pos.x + ", " + pos.z);
break;
case lane.outside: //lane before is outside, so can only go to the middle
//Debug.Log ("test outside: pos.x = " + pos.x + ", pos.z = " + pos.z);
if (((pos.x == 3.5 || pos.x == 33.5) && pos.z > 3.5 && pos.z < 16.5) ||
((pos.z == 1.5 || pos.z == 19.5) && pos.x > 6.5 && pos.x < 30.5)) //this code checks to make sure
//if((((Mathf.Abs(pos.z - 3.5f) < .01) || (Mathf.Abs(pos.z - 33.5f) < .01)) && pos.z > 3.5 && pos.z < 16.5)
//|| (((Mathf.Abs(pos.z - 1.5f) < .01) || (Mathf.Abs(pos.z - 19.5f) < .01)) && pos.x > 6.5 && pos.x < 30.5)) //this code checks to make sure
//if((Mathf.Abs(pos.z - 1.5f) < .01) && (pos.x > 6.5f) && (pos.x < 30.5f) )
{
if (rightClear)
{
aLane = lane.middle;
transform.Translate(transform.right, Space.World); //does actual shift
//Debug.Log ("Outside works: before = " + pos + ", after" + transform.position);
}
//else Debug.Log ("right clear is false");
}
// else Debug.Log ("outside fail: pos.x is: " + pos.x + ", pos.z is: " + pos.z);
break;
}
}
public CompoundTerm DoAction(CompoundTerm action)
{
switch (action.Name)
{
case ("Tests"): return(null);
case ("StartPosition"):
Console.WriteLine("Väärtused ");
//Publish first waypoint so the car could start moving with second waypoint
/* float[] xs1 = Array.ConvertAll(((string)action[0]).Split(','), float.Parse);
* float[] ys1 = Array.ConvertAll(((string)action[1]).Split(','), float.Parse);
* float[] zs1 = Array.ConvertAll(((string)action[2]).Split(','), float.Parse);
* float[] vels1 = Array.ConvertAll(((string)action[3]).Split(','), float.Parse);
* float[] yaws1 = Array.ConvertAll(((string)action[4]).Split(','), float.Parse);
*
* lane laneToPub4 = laneMsgData(xs1, ys1, zs1, vels1, yaws1, seqCounter, (int)action[5]);
* Console.WriteLine("Start");
* seqCounter = seqCounter + 1;
* /* LaneArray wpLaneArray = new LaneArray();
* visualizeLane(laneToPub4); */
// rosSocket.Publish(publish_waypoint, laneToPub4);
return(null);
case ("MoveCar"):
float[] xs = Array.ConvertAll(((string)action[0]).Split(','), float.Parse);
float[] ys = Array.ConvertAll(((string)action[1]).Split(','), float.Parse);
float[] zs = Array.ConvertAll(((string)action[2]).Split(','), float.Parse);
float[] vels = Array.ConvertAll(((string)action[3]).Split(','), float.Parse);
float[] yaws = Array.ConvertAll(((string)action[4]).Split(','), float.Parse);
float[] txs = Array.ConvertAll(((string)action[6]).Split(','), float.Parse);
float[] tys = Array.ConvertAll(((string)action[7]).Split(','), float.Parse);
float[] tzs = Array.ConvertAll(((string)action[8]).Split(','), float.Parse);
float[] tvels = Array.ConvertAll(((string)action[9]).Split(','), float.Parse);
float[] tyaws = Array.ConvertAll(((string)action[10]).Split(','), float.Parse);
Console.WriteLine("Crash6");
lane lane = laneMsgData(xs, ys, zs, vels, yaws, seqCounter, (int)action[5]);
lane tempLane = laneMsgData(txs, tys, tzs, tvels, tyaws, seqCounter, (int)action[5]);
// visualizeLane(lane[1]);
int countOfWps = (int)action[5] - 1;
//Calculate the time that will take to travel from last passedwaypoint to next waypoint
Console.WriteLine("Crash8");
arrivalTime = calculateTimeTravelingBetweenWaypoints(lane, currentPosition);
// arrivalTime = (int)calculateTravelingTimeToNextWaypoint(currentPosition, lane.waypoints[(int)action[5] / 2 - 1], lane[0].waypoints[(int)action[5] / 2 - 1].twist.twist.linear.x);
nextWaypoint = lane.waypoints[countOfWps];
nextTempWaypoint = tempLane.waypoints[countOfWps];
Console.WriteLine("Crash 5");
if (yaws[(countOfWps / 2)] > -0.5 || yaws[(countOfWps / 2)] > 0.5)
{
}
else
{
wpToReachList.Add(nextWaypoint);
Console.WriteLine("Publish ");
rosSocket.Publish(publish_tempWaypoint, tempLane);
rosSocket.Publish(publish_waypoint, lane);
}
//lanelist.Add(lane[0]);
//Maybe it can be done NModel.Terms.Action.Create("timeoutAction", arrivalTime.ToString ());
return(null);
case ("didReacheWaypoint_Start"):
if (arrivalTime == 0)
{
arrivalTime = 10000;
}
Console.WriteLine("Wait for event");
Console.WriteLine("Arrival time {0} {1}", arrivalTime, nextWaypoint.pose.pose.position.x);
someEvent += new MyDelegate(Result);
bool result = wait.WaitOne(arrivalTime * 4);
wait.Reset();
Console.WriteLine("Finished");
if (result)
{
return(NModel.Terms.Action.Create("didReacheWaypoint_Finish"));
}
else
{
return(NModel.Terms.Action.Create("didNotReachWaypoint", nextWaypoint.pose.pose.position.x, nextWaypoint.pose.pose.position.y, nextWaypoint.pose.pose.position.z));
}
// return NModel.Terms.Action.Create("didNotReachWaypoint", nextWaypoint.pose.pose.position.x, nextWaypoint.pose.pose.position.y, nextWaypoint.pose.pose.position.z);
// return NModel.Terms.Action.Create("didNotReachWaypoint", nextWaypoint.pose.pose.position.x, nextWaypoint.pose.pose.position.y, nextWaypoint.pose.pose.position.z);
//return t.Result;
case ("Goal"):
//return NModel.Terms.Action.Create("Goal_Finish");
return(null);
default: throw new Exception("Unexpected action " + action);
}
}
//Toimmingud
public CompoundTerm DoAction(CompoundTerm action)
{
switch (action.Name)
{
case ("Tests"): return(null);
case ("StartPosition"):
Console.WriteLine("Väärtused ");
//Algpunkti publitseerimine
float[] xs1 = Array.ConvertAll(((string)action[0]).Split(','), float.Parse);
float[] ys1 = Array.ConvertAll(((string)action[1]).Split(','), float.Parse);
float[] zs1 = Array.ConvertAll(((string)action[2]).Split(','), float.Parse);
float[] vels1 = Array.ConvertAll(((string)action[3]).Split(','), float.Parse);
float[] yaws1 = Array.ConvertAll(((string)action[4]).Split(','), float.Parse);
lane laneToPub4 = laneMsgData(xs1, ys1, zs1, vels1, yaws1, seqCounter, (int)action[5]);
Console.WriteLine("Start");
seqCounter = seqCounter + 1;
rosSocket.Publish(publish_waypoint, laneToPub4);
return(null);
case ("MoveCar"):
//Auto liigutamine määratud teekonnapunktide abil
float[] xs = Array.ConvertAll(((string)action[0]).Split(','), float.Parse);
float[] ys = Array.ConvertAll(((string)action[1]).Split(','), float.Parse);
float[] zs = Array.ConvertAll(((string)action[2]).Split(','), float.Parse);
float[] vels = Array.ConvertAll(((string)action[3]).Split(','), float.Parse);
float[] yaws = Array.ConvertAll(((string)action[4]).Split(','), float.Parse);
lane lane = laneMsgData(xs, ys, zs, vels, yaws, seqCounter, (int)action[5]);
visualizeLane(lane);
int countOfWps = (int)action[5] - 1;
arrivalTime = calculateTimeTravelingBetweenWaypoints(lane, currentPosition);
nextWaypoint = lane.waypoints[countOfWps];
wpToReachList.Add(nextWaypoint);
Console.WriteLine("Publish ");
rosSocket.Publish(publish_waypoint, lane);
return(null);
case ("didReacheWaypoint_Start"):
//Teekonnapunkti jõudmise kontrollimine
if (arrivalTime <= 0)
{
arrivalTime = 10000;
}
else
{
arrivalTime = arrivalTime * konstant;
}
Console.WriteLine("Wait for event");
Console.WriteLine("Arrival time {0} {1}", arrivalTime, nextWaypoint.pose.pose.position.x);
Console.WriteLine("Velocity {0} ", nextWaypoint.twist.twist.linear.x);
someEvent += new CarArrivedDelegate(Result);
bool result = wait.WaitOne(arrivalTime * konstant);
wait.Reset();
Console.WriteLine("Finished");
if (result)
{
return(NModel.Terms.Action.Create("didReacheWaypoint_Finish"));
}
else
{
return(NModel.Terms.Action.Create("didNotReachWaypoint", nextWaypoint.pose.pose.position.x, nextWaypoint.pose.pose.position.y, nextWaypoint.pose.pose.position.z));
}
case ("Goal"):
//Kogu teekond on läbitud
return(null);
default: throw new Exception("Unexpected action " + action);
}
}
