Commit 8bfd8723 authored by Hengwen Zhang's avatar Hengwen Zhang
Browse files

Add support for defining PathGoal using osmId and lon/lat

parent 8a93ee50
......@@ -6,6 +6,7 @@ import java.time.Instant;
import de.rwth.montisim.commons.simulation.SimulationObject;
import de.rwth.montisim.commons.simulation.TimeUpdate;
import de.rwth.montisim.commons.utils.Coordinates;
import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
import de.rwth.montisim.simulation.vehicle.VehicleProperties;
import de.rwth.montisim.simulation.vehicle.task.Task;
......@@ -25,25 +26,27 @@ import de.rwth.montisim.simulation.simulator.vehicleconfigs.DefaultVehicleConfig
import de.rwth.montisim.simulation.vehicle.Vehicle;
public class IntegrationTest {
@Test
public void driveToTarget() throws Exception {
final Vec3 START_DIR = new Vec3(1, 0, 0);
final Vec2 TARGET_POS = new Vec2(-63.83, -171.96);
//System.out.println("Working Dir: " + LibraryService.getWorkingDirectory());
World world = new OsmToWorldLoader(new OsmMap("aachen", new File("src/test/resources/aachen.osm"))).getWorld();
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
final private String MAP_PATH = "src/test/resources/aachen.osm";
private OsmMap osmMap;
private World world;
@Before
public void initResource() throws Exception {
osmMap = new OsmMap("myMap", new File(MAP_PATH));
world = new OsmToWorldLoader(osmMap).getWorld();
world.finalizeGraph();
}
SimulationConfig config = new SimulationConfig();
config.max_duration = Duration.ofSeconds(300);
Simulator simulator = new Simulator(config, world, pathfinding, mtManager);
SimulationLoop loop = new SimulationLoop(simulator, config);
@Test
public void driveToTargetLocalCoordinate() throws Exception {
Vec3 startPos = new Vec3(1, 0, 0);
Vec2 targetPos = new Vec2(-63.83, -171.96);
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(TARGET_POS.x, TARGET_POS.y)
.arrive(startPos.asVec2())
.arrive(targetPos)
.withInRange(10)
.build());
task.addGoal(MetricGoal.newBuilder()
......@@ -52,15 +55,69 @@ public class IntegrationTest {
.greater(1000, "m/s")
.build());
driveToTarget(task);
}
@Test
public void driveToTargetGlobalCoordinate() throws Exception {
Coordinates startCoord = new Coordinates(6.0721450,50.7738916);
Coordinates targetCoord = new Coordinates(6.0690220,50.773491);
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(startCoord)
.arrive(targetCoord)
.withInRange(10)
.build());
task.getPathGoals().forEach(g -> g.convertCoordinate(osmMap, world));
driveToTarget(task);
}
@Test
public void driveToTargetGlobalOsmId() throws Exception {
long startOsmId = 3369847019l;
long targetOsmId = 444540939l;
Task task = new Task();
task.addGoal(PathGoal.newBuilder()
.eventually()
.arrive(startOsmId)
.arrive(targetOsmId)
.withInRange(10)
.build());
task.getPathGoals().forEach(g -> g.convertCoordinate(osmMap, world));
driveToTarget(task);
}
public void driveToTarget(Task task) throws Exception {
Pathfinding pathfinding = new PathfindingImpl(world);
MessageTypeManager mtManager = new MessageTypeManager();
SimulationConfig config = new SimulationConfig();
config.max_duration = Duration.ofSeconds(300);
Simulator simulator = new Simulator(config, world, pathfinding, mtManager);
SimulationLoop loop = new SimulationLoop(simulator, config);
DefaultVehicleConfig vConf = DefaultVehicleConfig.withJavaAutopilot().setTask(task);
Vehicle vehicle = simulator.getVehicleBuilder(vConf.properties).setName("TestVehicle").build();
vehicle.physicsModel.setGroundPosition(new Vec3(0, 0, 0), new Vec2(START_DIR.x, START_DIR.y));
// Read positions from task
PathGoal pg = task.getPathGoals().get(0);
Vec2 startPos = pg.getPath().get(0);
Vec2 targetPos = pg.getPath().get(pg.getPath().size() - 1);
vehicle.physicsModel.setGroundPosition(new Vec3(0, 0, 0), new Vec2(startPos.x, startPos.y));
simulator.addSimulationObject(vehicle);
Navigation nav = (Navigation) vehicle.eesystem.getComponentManager().getComponent("Navigation").get();
nav.pushTargetPos(TARGET_POS);
nav.pushTargetPos(targetPos);
vehicle.properties.setCoordinates(new Coordinates());
world.converter.get().metersToCoords(startPos, vehicle.properties.getCoordinates());
Assert.assertFalse(simulator.allTasksSucceeded());
......@@ -78,7 +135,7 @@ public class IntegrationTest {
continue;
}
double dist = vehicle.physicalObject.pos.distance(new Vec3(TARGET_POS.x, TARGET_POS.y, 0));
double dist = vehicle.physicalObject.pos.distance(new Vec3(targetPos.x, targetPos.y, 0));
System.out.printf("dist: %.2fm, pos: %s\n", dist, vehicle.physicalObject.pos);
System.out.println("Dumping and reloading vehicle...");
......@@ -93,7 +150,7 @@ public class IntegrationTest {
simulator.addSimulationObject(vehicle);
}
double dist = vehicle.physicalObject.pos.distance(new Vec3(TARGET_POS.x, TARGET_POS.y, 0));
double dist = vehicle.physicalObject.pos.distance(new Vec3(targetPos.x, targetPos.y, 0));
System.out.printf("dist: %.2fm, pos: %s\n", dist, vehicle.physicalObject.pos);
// vehicle.addTarget(TARGET_POS);
......
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.simulation.TaskStatus;
import de.rwth.montisim.commons.utils.Coordinates;
import de.rwth.montisim.commons.utils.LTLOperator;
import de.rwth.montisim.commons.utils.Vec2;
import de.rwth.montisim.commons.utils.Vec3;
import de.rwth.montisim.commons.utils.json.Typed;
import de.rwth.montisim.simulation.environment.osmmap.OsmMap;
import de.rwth.montisim.simulation.environment.world.World;
import de.rwth.montisim.simulation.vehicle.Vehicle;
import java.util.List;
import java.util.stream.Collectors;
/**
*
*/
@Typed
public class PathGoal extends Goal {
private List<Vec2> path;
private double radius;
private int currDestIdx;
private List<Long> pathOsmIds; // path as a list of osm IDs
private List<Coordinates> pathLonLat; // global coordinate
private List<Vec2> path; // local coordinate
// if a vehicle arrives location within this range of a point,
// the point will be considered reached by the vehicle.
private double range;
private int currDestIdx; // the index of current destination (next location the vehicle should reach)
public PathGoal() {
}
public PathGoal(LTLOperator ltlOperator, List<Vec2> path, double radius) {
public PathGoal(List<Long> pathOsmIds, LTLOperator ltlOperator, double range) {
this.ltlOperator = ltlOperator;
this.pathOsmIds = pathOsmIds;
this.range = range;
this.currDestIdx = 0;
}
public PathGoal(LTLOperator ltlOperator, double range, List<Coordinates> coordinates) {
this.ltlOperator = ltlOperator;
this.pathLonLat = coordinates;
this.range = range;
this.currDestIdx = 0;
}
public PathGoal(LTLOperator ltlOperator, List<Vec2> path, double range) {
super();
this.ltlOperator = ltlOperator;
this.path = path;
this.radius = radius;
this.range = range;
this.currDestIdx = 0;
}
/**
* Convert path described using OSM ID or lon/lat into a path that all waypoints are described using local coordinate system
*
* @param map
* @param world
*/
public void convertCoordinate(OsmMap map, World world) {
if (pathOsmIds != null)
convertOsmPath(map, world);
else if (pathLonLat != null)
convertLonLatPath(world);
}
private void convertOsmPath(OsmMap map, World world) {
pathLonLat = pathOsmIds.stream().map(osmId ->
// Get lon/lat of each osm node, identified by osmId
// Throw runtime error if no matching can be found
map.nodeTable.stream().filter(node -> node.osmID == osmId).collect(Collectors.toList()).get(0).coords
).collect(Collectors.toList());
convertLonLatPath(world);
}
private void convertLonLatPath(World world) {
path = pathLonLat.stream().map(coord -> {
Vec2 point = new Vec2();
world.converter.get().coordsToMeters(coord, point);
return point;
}).collect(Collectors.toList());
}
public void update(Vehicle v) {
if (ltlOperator.equals(LTLOperator.EVENTUALLY)){
if (path == null)
throw new RuntimeException("path from PathGoal must not be null");
if (ltlOperator.equals(LTLOperator.EVENTUALLY)) {
handleEventually(v);
} else if (ltlOperator.equals(LTLOperator.NEVER)) {
handleNever(v);
......@@ -41,12 +103,10 @@ public class PathGoal extends Goal {
double dist = v.physicalObject.pos.asVec2().distance(dest);
// nothing to update if current destination have not been reached yet
if (dist > radius) {
if (dist > range) {
return;
}
// System.out.printf("%.2fm to target\n", dist);
// a coordinate in path is reached,
// update next destination and goal status
currDestIdx += 1;
......@@ -63,8 +123,8 @@ public class PathGoal extends Goal {
// i.e. the vehicle should never arrive in the area of any given destination
TaskStatus status = TaskStatus.FAILED;
for (Vec2 vec: path) {
if (v.physicalObject.pos.distance(new Vec3(vec.x, vec.y, 0)) < radius) {
for (Vec2 vec : path) {
if (v.physicalObject.pos.distance(new Vec3(vec.x, vec.y, 0)) < range) {
// succeeded if arrived in the area of vec
status = TaskStatus.SUCCEEDED;
break;
......@@ -98,9 +158,32 @@ public class PathGoal extends Goal {
return path;
}
public double getRadius() {
return radius;
public void setPath(List<Vec2> path) {
this.path = path;
}
public List<Coordinates> getPathLonLat() {
return pathLonLat;
}
public void setPathLonLat(List<Coordinates> pathLonLat) {
this.pathLonLat = pathLonLat;
}
public List<Long> getPathOsmIds() {
return pathOsmIds;
}
public void setPathOsmIds(List<Long> pathOsmIds) {
this.pathOsmIds = pathOsmIds;
}
public double getRange() {
return range;
}
public int getCurrDestIdx() {
return currDestIdx;
}
}
package de.rwth.montisim.simulation.vehicle.task.goal;
import de.rwth.montisim.commons.utils.Coordinates;
import de.rwth.montisim.commons.utils.Vec2;
import java.util.LinkedList;
......@@ -10,6 +11,8 @@ public class PathGoalBuilder extends GoalBuilder<PathGoalBuilder> {
final static double DEFAULT_RANGE = 10d;
List<Vec2> coords;
List<Coordinates> coordsLonLat;
List<Long> coordsOsmId;
Optional<Double> range;
@Override
......@@ -19,25 +22,56 @@ public class PathGoalBuilder extends GoalBuilder<PathGoalBuilder> {
@Override
public Goal build() {
return new PathGoal(ltlOperator, coords, range.orElse(DEFAULT_RANGE));
if (coords != null) {
return new PathGoal(ltlOperator, coords, range.orElse(DEFAULT_RANGE));
} else if (coordsLonLat != null) {
return new PathGoal(ltlOperator, range.orElse(DEFAULT_RANGE), coordsLonLat);
} else if (coordsOsmId != null) {
return new PathGoal(coordsOsmId, ltlOperator, range.orElse(DEFAULT_RANGE));
}
return null;
}
public PathGoalBuilder arrive(Vec2 coord) {
if (coords == null){
if (coords == null) {
coords = new LinkedList<>();
coordsOsmId = null;
coordsLonLat = null;
}
coords.add(coord);
return getThis();
}
public PathGoalBuilder arrive(double x, double y) {
if (coords == null){
if (coords == null) {
coords = new LinkedList<>();
coordsOsmId = null;
coordsLonLat = null;
}
coords.add(new Vec2(x, y));
return getThis();
}
public PathGoalBuilder arrive(Coordinates lonlat) {
if (coordsLonLat == null) {
coordsLonLat = new LinkedList<>();
coordsOsmId = null;
coords = null;
}
coordsLonLat.add(lonlat);
return getThis();
}
public PathGoalBuilder arrive(long osmId) {
if (coordsOsmId == null) {
coordsOsmId = new LinkedList<>();
coords = null;
coordsLonLat = null;
}
coordsOsmId.add(osmId);
return getThis();
}
public PathGoalBuilder withInRange(double rangeMeter) {
range = Optional.of(rangeMeter);
return getThis();
......
......@@ -9,6 +9,8 @@ import de.rwth.montisim.simulation.vehicle.VehicleBuilder;
import de.rwth.montisim.simulation.vehicle.physicalvalues.TrueVelocity;
import junit.framework.TestCase;
import java.util.Optional;
import static org.mockito.Mockito.mock;
import static org.mockito.Mockito.when;
......
......@@ -12,6 +12,8 @@ import org.junit.After;
import org.junit.Before;
import org.junit.Test;
import java.util.Optional;
public class PathGoalTest {
@Test
public void testEventually() {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment