+1 (972) 883-2091
utdmavs@gmail.com
ECSS 4.220, 800 W Campbell Rd, Richardson, TX 75083–0688, US

Implementing the “task” package

1. Package Description

The role of a vehicle agent’s Task Module is to define the set of concrete tasks a vehicle agent can perform. In addition, it specifies the translation of vehicle agent’s tasks into a set of stimuli that are sent later to the environemnt to combine and decide there effects. As shown in the figure below, we define the following tasks for a vehicle agent: 1) “MoveForwardTask”; 2) “TurnTask”; 3) “TurnRightTask”; and 4) “TurnLeftTask”. The figure also shows that these tasks are eventually translated into “MoveStimulus” and “ChangeHeadingStimulus”.

stimulus

Next, we describe the detailed implementation of the “task” package.

2. Implementing the “MoveForwardTask” class

    1. Inside the “tasks” package, create a class named “MoveForwardTask” that extends the class “edu.utdallas.mavs.traffic.simulation.sim.agent.task.AbstractTask”. This class describes a vehicle agent’s ability to move to other location.
    2. Write the following code that describes the implementation of the MoveForwardTask class. The code describes the MoveForwardTask class attributes and how the MoveForwardTask is translated into a set of stimuli after execution.
package edu.utdallas.mavs.traffic.simulation.sim.agent.tasks;

import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.task.AbstractTask;
import edu.utdallas.mavs.divas.core.sim.agent.task.Task;
import edu.utdallas.mavs.divas.core.sim.common.stimulus.Stimuli;
import edu.utdallas.mavs.traffic.simulation.sim.common.state.Posture;
import edu.utdallas.mavs.traffic.simulation.sim.common.stimulus.ChangeHeadingStimulus;
import edu.utdallas.mavs.traffic.simulation.sim.common.stimulus.MoveStimulus;

public class MoveForwardTask extends AbstractTask
{
    private static final long  serialVersionUID = 1L;

    /**
     * The name of this task
     */
    public static final String NAME             = "MoveForward";

    /**
     * The type of this task
     */
    public static final String TYPE             = "MoveForward";

    /**
     * The position of the agent after this task is executed
     */
    private Vector3f           position;

    /**
     * The current position of the agent after this task is executed
     */
    private Vector3f           currentPosition;
    
    /**
     * The posture associated with the MoveForwardTask
     */
    private Posture posture;

    public MoveForwardTask(boolean enabled)
    {
        this(-1, enabled);
    }

    public MoveForwardTask(long executionCycle, boolean enabled)
    {
        super(NAME, TYPE, executionCycle, enabled);
    }

    @Override
    public Task createTask(long executionCycle)
    {
        return new MoveForwardTask(executionCycle, enabled);
    }

    public MoveForwardTask createTask(long executionCycle, Vector3f currentPosition, Vector3f position)
    {
        MoveForwardTask task = new MoveForwardTask(executionCycle, enabled);
        task.position = position;
        task.currentPosition = currentPosition;
        posture = Posture.forward_mid;
        return task;
    }

    @Override
    public Stimuli execute(int agentId)
    {
        Stimuli stimuli = new Stimuli();
        stimuli.add(new MoveStimulus(agentId, position));
        stimuli.add(new ChangeHeadingStimulus(agentId, position.subtract(currentPosition)));
        return stimuli;
    }

    public Posture getPosture()
    {
        return posture;
    }
}

3. Implementing the “TurnTask” class

The turn task class describes a vehicle agent’s ability to make turns. It is responsible for implementing a smooth vehicle turn by anticipating and calculating the vehicle’s travel trajectory during the execution of a turn task. This calculation depends on the following factors:

  1. “startPostion”: The position of the vehicle before starting the turn.
  2. “endPosition”: The position of the vehicle after finishing the turn.
  3. “INTERPOLATION_STEPS”: The total number of steps needed to complete a turn.

Implementation Steps

  1. Inside the “tasks” package, create an abstract class named “TurnTask” that extends the class “edu.utdallas.mavs.traffic.simulation.sim.agent.task.AbstractTask”.
  2. Write the following code that describes the implementation of the TurnTask class.
package edu.utdallas.mavs.traffic.simulation.sim.agent.tasks;

import java.util.ArrayList;
import java.util.List;

import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.task.AbstractTask;
import edu.utdallas.mavs.divas.core.sim.common.stimulus.Stimuli;
import edu.utdallas.mavs.traffic.simulation.sim.common.state.Posture;
import edu.utdallas.mavs.traffic.simulation.sim.common.stimulus.ChangeHeadingStimulus;
import edu.utdallas.mavs.traffic.simulation.sim.common.stimulus.MoveStimulus;

public abstract class TurnTask extends AbstractTask
{
    private static final long serialVersionUID      = 1L;

    public static final int   INTERPOLATION_STEPS   = 5;

    /**
     * The origin position of the agent after this task is executed
     */
    protected Vector3f        startPosition;

    /**
     * The destination position of the agent after this task is executed
     */
    protected Vector3f        endPosition;

    /**
     * Path interpolated positions for turn task
     */
    protected List<Vector3f>  interpolatedPositions = new ArrayList<>();

    /**
     * Path interpolated headings for turn task
     */
    protected List<Vector3f>  interpolatedHeadings  = new ArrayList<>();
    
    /**
     * The posture associated with the TurnTask
     */
    protected Posture posture;

    protected int             currentStep           = 1;

    public TurnTask(String name, String type, long executionCycle, boolean enabled)
    {
        super(name, type, executionCycle, enabled);
    }

    @Override
    public Stimuli execute(int agentId)
    {
        Stimuli stimuli = new Stimuli();
        stimuli.add(new MoveStimulus(agentId, interpolatedPositions.get(currentStep)));
        stimuli.add(new ChangeHeadingStimulus(agentId, interpolatedHeadings.get(currentStep)));
        currentStep++;
        return stimuli;
    }

    protected void interpolatePath(Vector3f startPosition, Vector3f endPosition, float turnAngle, boolean turnLeft, List<Vector3f> interpolatedPositions, List<Vector3f> interpolatedHeadings)
    {
        // calculates the pivot of rotation
        Vector3f pivot = calculatePivotPoint(startPosition, endPosition, turnLeft);
        // interpolates intermediate positions (circular path)
        for(float i = 0; i <= FastMath.HALF_PI; i = i + FastMath.HALF_PI / INTERPOLATION_STEPS)
        {
            Vector3f normal = startPosition.subtract(pivot);
            Vector3f point = rotatedVector(normal, i, turnLeft);
            Vector3f position = pivot.add(point);
            interpolatedPositions.add(position);
            interpolatedHeadings.add(getOrtonormalVector(pivot.subtract(position), !turnLeft));
        }
        
    }

    private Vector3f calculatePivotPoint(Vector3f startPosition, Vector3f endPosition, boolean turnLeft)
    {
        Vector3f midPoint = endPosition.add(startPosition).divide(2f);
        Vector3f normal = midPoint.add(getOrtonormalVector(endPosition.subtract(startPosition), turnLeft).mult(endPosition.distance(startPosition) / 2f));
        return normal;
    }

    private Vector3f getOrtonormalVector(Vector3f v, boolean turnLeft)
    {
        return rotatedVector(v, FastMath.HALF_PI, turnLeft).normalizeLocal();
    }

    private Vector3f rotatedVector(Vector3f v, float angle, boolean turnLeft)
    {
        float rot = (turnLeft ? 1 : -1) * angle;
        float tx = v.x * FastMath.cos(rot) - v.z * FastMath.sin(rot);
        float tz = v.x * FastMath.sin(rot) + v.z * FastMath.cos(rot);
        return new Vector3f(tx, 0, tz);
    }

    /**
     * Gets the origin position to which the vehicle should be moved
     */
    public Vector3f getStartPosition()
    {
        return startPosition;
    }

    /**
     * Gets the destination position to which the vehicle should be moved
     */
    public Vector3f getEndPosition()
    {
        return endPosition;
    }

    /**
     * Gets the interpolated list of path positions
     * 
     * @return
     */
    public List<Vector3f> getInterpolatedPositions()
    {
        return interpolatedPositions;
    }

    /**
     * Gets the interpolated list of heading positions
     * 
     * @return
     */
    public List<Vector3f> getInterpolatedHeadings()
    {
        return interpolatedHeadings;
    }

    public boolean hasCompleted()
    {
        return currentStep == INTERPOLATION_STEPS + 1;
    }
}

In our implementation, the “TurnTask” provides the method “interoplatePath” that uses the aforementioned factors to compute the interpolated positions and headings during a vehicle turn. This method is invoked once a “TurnTask” is created by the vehicle agent. The figure below illustrates the execution of the “interpolatePath” method. At each interpolation step, two stimuli are generated: 1) “MoveStimulus”: describes the vehicle movement intention to the interpolated position at the current interpolation step and 2) “ChangeHeadingStimulus”: describes the vehicle intention to change its heading (See the green arrows in the figure below).

stimulus

4. Implementing the “TurnRightTask” and “TurnLeftTask” classes

As illustrated the “task” package figure, the “TurnRightTask” and “TurnLeftTask” are special cases of the “TurnTask” described earlier. However, each of these turn tasks has a different travel trajectory that is defined by its own pivot point (see the figure below). From an implementation perspective, this is achieved by invoking the “interpolatePath” method defined in the “TurnTask” class with passing different arguments for the start and end positions. In addition, the animated posture for vehicle’s wheels is specified according to the turning direction.

stimulus

Implementation Steps for “TurnRightTask”

  1. Inside the “tasks” package, create a class named “TurnRightTask” that extends the “TurnTask” class.
  2. Provide an implementation for the “TurnRightTask”. As mentioned earlier, in this class you should invoke the method “interoplatePath” by passing the start and end positions of a vehicle making a right turn. In addition, you should set the posture to “Posture.forward_mid_right”. This information allows the 3D visualizer to set the correct animation for the vehicle while making right turns. The following code provides a full implementation for the class “TurnRightTask”.
package edu.utdallas.mavs.traffic.simulation.sim.agent.tasks;

import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.task.Task;
import edu.utdallas.mavs.traffic.simulation.sim.common.state.Posture;

public class TurnRightTask extends TurnTask
{
    private static final long  serialVersionUID = 1L;

    /**
     * The name of this task
     */
    public static final String NAME             = "TurnRight";

    /**
     * The type of this task
     */
    public static final String TYPE             = "TurnRight";

    public TurnRightTask(boolean enabled)
    {
        this(-1, enabled);
    }

    public TurnRightTask(long executionCycle, boolean enabled)
    {
        super(NAME, TYPE, executionCycle, enabled);
    }

    @Override
    public Task createTask(long executionCycle)
    {
        return new TurnRightTask(executionCycle, enabled);
    }

    public TurnRightTask createTask(long time, Vector3f startPosition, Vector3f endPosition)
    {
        TurnRightTask task = new TurnRightTask(time, enabled);
        task.startPosition = startPosition;
        task.endPosition = endPosition;
        interpolatePath(startPosition, endPosition, FastMath.HALF_PI, true, task.interpolatedPositions, task.interpolatedHeadings);
        posture = Posture.forward_mid_right;
        return task;
    }
}

Implementation Steps for “TurnLeftTask”

  1. Inside the “tasks” package, create a class named “TurnLeftTask” that extends the “TurnTask” class.
  2. Provide an implementation for the “TurnLeftTask”. As mentioned earlier, in this class you should invoke the method “interoplatePath” by passing the start and end position of a vehicle making a left turn. In addition, you should set the posture to “Posture.forward_mid_left”. This information allows the 3D visualizer to set the correct animation for the vehicle while making left turns. The following code provides a full implementation for the class “TurnLeftTask”.
package edu.utdallas.mavs.traffic.simulation.sim.agent.tasks;

import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.task.Task;
import edu.utdallas.mavs.traffic.simulation.sim.common.state.Posture;

public class TurnLeftTask extends TurnTask
{
    private static final long  serialVersionUID = 1L;

    /**
     * The name of this task
     */
    public static final String NAME             = "TurnLeft";

    /**
     * The type of this task
     */
    public static final String TYPE             = "TurnLeft";

    public TurnLeftTask(boolean enabled)
    {
        this(-1, enabled);
    }

    public TurnLeftTask(long executionCycle, boolean enabled)
    {
        super(NAME, TYPE, executionCycle, enabled);
    }

    @Override
    public Task createTask(long executionCycle)
    {
        return new TurnLeftTask(executionCycle, enabled);
    }

    public TurnLeftTask createTask(long time, Vector3f startPosition, Vector3f endPosition)
    {
        TurnLeftTask task = new TurnLeftTask(time, enabled);
        task.startPosition = startPosition;
        task.endPosition = endPosition;
        interpolatePath(startPosition, endPosition, FastMath.HALF_PI, false, task.interpolatedPositions, task.interpolatedHeadings);
        posture = Posture.forward_mid_left;
        return task;
    }
}

5. Implementing the “VehicleTaskModule” class

The vehicle task module aggregates the set of tasks a vehicle agent can perform. At initialization time, the vehicle task module loads the set of concrete vehicle agent’s tasks defined earlier: 1) “MoveForwardTask”; 2) “TurnRightTask”; and 3) “TurnLeftTask”. After that, the vehicle task module will be ready for use to create tasks as needed to implement a vehicle plan.

Implementation Steps for “VehicleTaskModule”

  1. Inside the “tasks” package, create a class named “VehicleTaskModule” that extends the “AbstractTaskModule” class.
  2. Provide an implementation for the “VehicleTaskModule”. The implementation should load all concrete tasks defined for a vehicle agent. Additionally, it should provide methods to create each type of tasks. The following code provides a full implementation for the “VehicleTaskModule”.
package edu.utdallas.mavs.traffic.simulation.sim.agent.tasks;

import java.io.Serializable;
import java.util.Set;

import com.jme3.math.Vector3f;

import edu.utdallas.mavs.divas.core.sim.agent.task.AbstractTaskModule;
import edu.utdallas.mavs.traffic.simulation.sim.agent.knowledge.VehicleKnowledgeModule;

public class VehicleTaskModule extends AbstractTaskModule<VehicleKnowledgeModule> implements Serializable
{

    private static final long        serialVersionUID = 1L;  

    private MoveForwardTask          moveForwardTask;

    private TurnLeftTask             turnLeftTask;

    private TurnRightTask            turnRightTask;

    public VehicleTaskModule(VehicleKnowledgeModule knowledgeModule)
    {
        super(knowledgeModule);        
    }

    @Override
    protected void loadTasks()
    {
        Set<String> availableTasks = knowledgeModule.getSelf().getTaskNames();
        moveForwardTask = new MoveForwardTask(availableTasks.contains(MoveForwardTask.TYPE));
        turnLeftTask = new TurnLeftTask(availableTasks.contains(TurnLeftTask.TYPE));
        turnRightTask = new TurnRightTask(availableTasks.contains(TurnRightTask.TYPE));
        tasks.put(MoveForwardTask.TYPE, moveForwardTask);
        tasks.put(TurnLeftTask.TYPE, turnLeftTask);
        tasks.put(TurnRightTask.TYPE, turnRightTask);
    }

    public MoveForwardTask createMoveForwardTask(Vector3f position)
    {
        return moveForwardTask.createTask(knowledgeModule.getTime(), knowledgeModule.getSelf().getPosition(), position);
    }

    public TurnLeftTask createTurnLeftTask(Vector3f endPosition)
    {
        return turnLeftTask.createTask(knowledgeModule.getTime(), knowledgeModule.getSelf().getPosition(), endPosition);
    }

    public TurnRightTask createTurnRightTask(Vector3f endPosition)
    {
        return turnRightTask.createTask(knowledgeModule.getTime(), knowledgeModule.getSelf().getPosition(), endPosition);
    }
    
}