public override void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound) { if(behavior is UTurnBehavior) { // } // check to see if we are at the upper bound RndfWayPoint upperWaypoint = channelListener.RndfNetwork.Waypoints[upperBound]; if (!(behavior is TurnBehavior) && upperWaypoint.NextLanePartition != null && location.DistanceTo(upperWaypoint.Position) < 3) { lowerBound = upperWaypoint.WaypointID; upperBound = upperWaypoint.NextLanePartition.FinalWaypoint.WaypointID; } Console.WriteLine(" > Received Instruction to Execute Behavior: " + behavior.ToString()); if (behavior is StayInLaneBehavior) { StayInLaneBehavior stayInLane = (StayInLaneBehavior)behavior; if (stayInLane.SpeedCommand is StopLineLaneSpeedCommand) { StopLineLaneSpeedCommand speedCommand = (StopLineLaneSpeedCommand)stayInLane.SpeedCommand; if (speedCommand.Distance < 2) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 0; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } } else if (stayInLane.SpeedCommand is StopLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = -5; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else if(stayInLane.SpeedCommand is DefaultLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } else { throw new ArgumentException("Unknown Lane Speed Type", "stayInLane.SpeedCommand"); } } // TODO: include midway point else if (behavior is TurnBehavior) { TurnBehavior currentBehavior = (TurnBehavior)behavior; RndfWayPoint exitWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.ExitPoint]; if (location.DistanceTo(exitWaypoint.Position) < 0.1) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); RndfWayPoint entryWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint]; Common.Coordinates change = entryWaypoint.Position - exitWaypoint.Position; Common.Coordinates midpoint = exitWaypoint.Position + change/2; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.ExitPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); vehicleState.xyPosition = midpoint; List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint].Position; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.EntryPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } } else { throw new ArgumentException("Unknown Behavior Type", "behavior"); } //Console.WriteLine("Sent Back Position \n"); }