@jcmap-sdk-web/navegador
Version:
甲虫室内定位导航引擎
881 lines (815 loc) • 26.9 kB
text/typescript
import { EventEmitter } from "events";
import type { Navegador, GetNavigationChainOptions } from "./navegador";
import type { CartogramPosition, Cartogram } from "@jcmap-sdk-web/jcmap";
import type {
NavigationMode,
NavigationPositionOnChain,
NavigationChain,
NavigationStatus,
NavigationSegment,
Direction,
} from "./types";
import { Directions } from "./types";
import {
getCoord as turfGetCoord,
getCoords as turfGetCoords,
} from "@turf/invariant";
import turfDistance from "@turf/distance";
import { propOr, pathOr } from "ramda";
import turfAlong from "@turf/along";
import {
point as turfPoint,
Units,
lineString as turfLineString,
convertLength as turfConvertLength,
} from "@turf/helpers";
import type { NavigationPosition } from "@jcmap-sdk-web/locator";
interface NavegadorTaskOptions {
navegador: Navegador;
navigationMode: NavigationMode;
finishingPoint: CartogramPosition;
startingPoint: CartogramPosition;
routeOptions?: GetNavigationChainOptions;
/**
* 重新规划路线前所需偏离判定的次数
*
* 默认值: 5
*/
offRoadCountLimit?: number;
/**
* 重新规划路线前所需后退判定的次数
*
* 默认值: 5
*/
backwardCountLimit?: number;
/**
* 判定为偏离的最小距离
*/
offRoadMinDistance?: {
/**
* 蓝牙定位:8米
*/
bluetooth?: number;
/**
* 卫星定位:16米
*/
satellite?: number;
};
/**
* 判定到达终点的距离
*
* 默认值: 8(米)
*/
arrivalDistance?: number;
/**
* 判定转弯的距离
*
* 默认值:8(米)
*/
cornerDistance?: number;
/**
* 定位点距离对重新规划计数器的调整
*
* 默认值:1.0
*/
distanceCountAdjust?: number;
/**
* 定位时间间隔对重新规划计数器的调整
*
* 默认值:1.0
*/
timeDiffCountAdjust?: number;
/**
* 定位楼层不同对重新规划计数器的调整
*
* 默认值:1.0
*/
cartogramDiffCountAdjust?: number;
/**
* 定位精度对重新规划计数的调整系数
*
* 默认值:0.05
*/
locationAccuracyAdjust?: number;
}
const WALK_SPEED_PER_MINUTE = 5000 / 60; // 约 5 km/h,参考 https://en.wikipedia.org/wiki/Preferred_walking_speed
interface DirectionToTextParams {
navegador: Navegador;
currentCartogramId: string;
navigationStatus: NavigationStatus;
}
export interface NavigationInfo {
/**
* 完整导航链
*/
fullNavigationPath: NavigationChain;
/**
* 导航模式
*/
navigationMode: NavigationMode;
/**
* 终点
*/
finishingPoint: CartogramPosition;
/**
* 起点
*/
startingPoint: CartogramPosition;
/**
* 目标位置
*/
targetPositionOnPath: NavigationPositionOnChain;
/**
* 当前显示位置
*/
currentPositionOnPath: NavigationPositionOnChain;
/**
* 当前剩余导航路径
*/
currentNavigationPath: NavigationChain;
/**
* 当前导航剩余距离
*/
restOfDistance: number;
/**
* 当前导航剩余时间,单位:分钟
*/
restOfTime: number;
/**
* 文字提示
*/
navigationHint: string;
/**
* 下一步动作
*/
nextDirection: Direction;
}
/**
* 导航任务
*/
export class NavegadorTask extends EventEmitter {
private offRoadCountLimit: number;
private backwardCountLimit: number;
private offRoadCount: number;
private backwardCount: number;
private navegador: Navegador;
private navigationMode: NavigationMode;
private finishingPoint: CartogramPosition;
private startingPoint: CartogramPosition;
private routeOptions?: GetNavigationChainOptions;
/**
* 完整导航路径
*/
private fullNavigationPath: NavigationChain;
/**
* 上一次定位距离终点的距离
*/
private lastDistToEnd: number;
/**
* 上一次定位更新时间
*/
private lastLocationTime: number;
/**
* 动画时间,因为行走速度很慢
*/
private animaTime = 950;
/**
* 当前显示位置
*/
private currentPositionOnPath: NavigationPositionOnChain;
/**
* 当前剩余导航路径
*/
private currentNavigationPath: NavigationChain;
/**
* 需要移动到的目标位置
*/
private targetPositionOnPath: NavigationPositionOnChain;
/**
* 最近一次更新位置后,需要沿路线前进的距离
*/
private animationDistance: number;
/**
* 最近一次更新位置后,剩余的导航线路
*/
private animationChain: NavigationChain;
/**
* 当前剩余总距离
*/
private restOfDistance: number;
/**
* 当请剩余总时间
*/
private restOfTime: number;
/**
* 文字提示
*/
private navigationHint: string;
/**
* 下一步动作
*/
private nextDirection: Direction;
/**
* 定位点距离对重新规划计数器的调整
*
* 默认值:1.0
*/
private distanceCountAdjust: number;
/**
* 定位时间间隔对重新规划计数器的调整
*
* 默认值:1.0
*/
private timeDiffCountAdjust: number;
/**
* 定位楼层不同对重新规划计数器的调整
*
* 默认值:1.0
*/
private cartogramDiffCountAdjust: number;
/**
* 定位精度对重新规划计数的调整系数
*
* 默认值:0.05
*/
private locationAccuracyAdjust: number;
/**
* 判定为偏离的最小距离
*/
private offRoadMinDistance: {
/**
* 蓝牙定位:8米
*/
bluetooth: number;
/**
* 卫星定位:16米
*/
satellite: number;
};
constructor(options: NavegadorTaskOptions) {
super();
const navegador = (this.navegador = options.navegador);
const navigationMode = (this.navigationMode = options.navigationMode);
const finishingPoint = (this.finishingPoint = options.finishingPoint);
const startingPoint = (this.startingPoint = options.startingPoint);
const routeOptions = (this.routeOptions = options.routeOptions);
this.offRoadCountLimit = propOr(5, "offRoadCountLimit", options);
this.backwardCountLimit = propOr(5, "backwardCountLimit", options);
this.offRoadCount = 0;
this.backwardCount = 0;
this.timeDiffCountAdjust = propOr(1, "timeDiffCountAdjust", options);
this.distanceCountAdjust = propOr(1, "distanceCountAdjust", options);
this.cartogramDiffCountAdjust = propOr(
1,
"cartogramDiffCountAdjust",
options
);
this.locationAccuracyAdjust = propOr(
0.05,
"locationAccuracyAdjust",
options
);
this.offRoadMinDistance = {
bluetooth: pathOr(8, ["offRoadMinDistance", "bluetooth"], options),
satellite: pathOr(8, ["offRoadMinDistance", "satellite"], options),
};
navegador.setCurrentArrivalDistance(options.arrivalDistance || 8);
navegador.setCornerDistance(options.cornerDistance || 8);
const startingPointOnRoad = navegador.projectionPositionOnRoad(
startingPoint
);
if (!startingPointOnRoad) {
throw new Error("starting point can not be projected on road");
}
const fullNavigationPath = navegador.getNavigationChain(
navigationMode,
startingPoint,
finishingPoint,
routeOptions
);
if (!fullNavigationPath) {
throw new Error("can not get navigation path");
}
this.fullNavigationPath = fullNavigationPath;
const startingPointOnPath = navegador.projectionPositionOnChain(
startingPointOnRoad
);
if (!startingPointOnPath) {
throw new Error("starting point can not be projected on navigation path");
}
const restOfDistance = (this.lastDistToEnd = navegador.getDistanceToNavigationChainEnd(
startingPointOnPath
) as number);
const navigationStatus = navegador.getNavigationStatus(startingPointOnRoad);
if (!navigationStatus) {
throw new Error("can not get navigation status");
}
this.lastLocationTime = Date.now();
this.animationDistance = 0;
this.animationChain = fullNavigationPath;
this.targetPositionOnPath = startingPointOnPath;
this.currentPositionOnPath = startingPointOnPath;
this.currentNavigationPath = fullNavigationPath;
this.restOfDistance = Math.floor(restOfDistance);
this.restOfTime = Math.ceil(restOfDistance / WALK_SPEED_PER_MINUTE);
this.navigationHint =
this.getNavigationStatusText({
currentCartogramId: startingPointOnPath.properties["cartogram:id"],
navegador,
navigationStatus,
}) || "";
this.nextDirection = navigationStatus.nextDirection;
}
private sliceNavigationChain(
nc: NavigationChain,
startPt: NavigationPositionOnChain
): NavigationChain {
const lines = Array.from(nc.features);
if (!lines.length) {
return nc;
}
const [firstSegment, ...restFeatures] = nc.features.filter(
(f) => f.properties.segment_index >= startPt.properties.segment_index
);
return {
...nc,
features: [
turfLineString<NavigationSegment["properties"]>(
[turfGetCoord(startPt), turfGetCoords(firstSegment)[1]],
firstSegment.properties
) as NavigationSegment,
...restFeatures,
],
};
}
private segmentDistance(segment: NavigationSegment) {
if (
segment.properties["starting:cartogram:id"] ===
segment.properties["finishing:cartogram:id"]
) {
const coords = turfGetCoords(segment);
return turfDistance(coords[0], coords[1], {
units: "meters",
});
}
return segment.properties.distance;
}
private alongNavigationPath(
nc: NavigationChain,
distance: number,
options?: Parameters<typeof turfAlong>[2]
): NavigationPositionOnChain {
const lines = Array.from(nc.features);
let line = lines.shift();
if (!line) {
throw new Error("no segment in NavigationChain");
}
const units: Units = propOr("kilometers", "units", options);
const ncDistanceUnit: Units = "meters";
let remainDistance = turfConvertLength(distance, units, ncDistanceUnit);
// 如果距离小于等于0,那么返回导航链的起点
if (remainDistance <= 0) {
const cartogramId = line.properties["starting:cartogram:id"];
const startCartogram = this.navegador._cartograms[cartogramId];
return turfPoint<NavigationPositionOnChain["properties"]>(
line.geometry.coordinates[0],
{
accuracy: 0,
"cartogram:id": cartogramId,
"cartogram:name": startCartogram.properties.name,
"cartogram:floor_number": startCartogram.properties.floor_number,
"cartogram:floor_label": startCartogram.properties.floor_label,
"building:id": startCartogram.properties["building:id"],
"building:name": startCartogram.properties["building:name"],
segment_index: line.properties.segment_index,
source: "pseudo",
timestamp: Date.now(),
road_index: line.properties["starting:road_index"],
}
) as NavigationPositionOnChain;
}
let cartogramId = line.properties["starting:cartogram:id"];
let startCartogram = this.navegador._cartograms[cartogramId];
let point: NavigationPositionOnChain = turfPoint<
NavigationPositionOnChain["properties"]
>(line.geometry.coordinates[0], {
accuracy: 0,
"cartogram:id": cartogramId,
"cartogram:name": startCartogram.properties.name,
"cartogram:floor_number": startCartogram.properties.floor_number,
"cartogram:floor_label": startCartogram.properties.floor_label,
"building:id": startCartogram.properties["building:id"],
"building:name": startCartogram.properties["building:name"],
segment_index: line.properties.segment_index,
source: "pseudo",
timestamp: Date.now(),
road_index: line.properties["starting:road_index"],
}) as NavigationPositionOnChain;
while (remainDistance && line) {
const lineDistance = this.segmentDistance(line);
if (remainDistance <= lineDistance) {
cartogramId = line.properties["starting:cartogram:id"];
startCartogram = this.navegador._cartograms[cartogramId];
point = turfPoint<NavigationPositionOnChain["properties"]>(
turfGetCoord(
turfAlong(line, remainDistance, { units: ncDistanceUnit })
),
{
accuracy: 0,
"cartogram:id": cartogramId,
"cartogram:name": startCartogram.properties.name,
"cartogram:floor_number": startCartogram.properties.floor_number,
"cartogram:floor_label": startCartogram.properties.floor_label,
"building:id": startCartogram.properties["building:id"],
"building:name": startCartogram.properties["building:name"],
segment_index: line.properties.segment_index,
source: "pseudo",
timestamp: Date.now(),
road_index: line.properties["starting:road_index"],
}
) as NavigationPositionOnChain;
break;
}
remainDistance -= lineDistance;
line = lines.shift();
}
return point;
}
private sliceNavigationChainAlong(
nc: NavigationChain,
startDist: number,
options?: Parameters<typeof turfAlong>[2]
): NavigationChain {
const lines = Array.from(nc.features);
if (!lines.length) {
return nc;
}
const _startPt = this.alongNavigationPath(nc, startDist, options);
const [firstSegment, ...restFeatures] = nc.features.filter(
(f) => f.properties.segment_index >= _startPt.properties.segment_index
);
return {
...nc,
features: [
turfLineString<NavigationSegment["properties"]>(
[turfGetCoord(_startPt), turfGetCoords(firstSegment)[1]],
firstSegment.properties
) as NavigationSegment,
...restFeatures,
],
};
}
getInfo(): NavigationInfo {
const startTime = this.lastLocationTime;
const animaTime = this.animaTime;
const now = Date.now();
const diff = now - startTime;
const ratio = diff >= animaTime ? 1 : diff / animaTime;
const moveDistance = this.animationDistance * ratio;
const nextPositionOnPath: NavigationPositionOnChain =
ratio === 1
? this.targetPositionOnPath
: this.alongNavigationPath(this.animationChain, moveDistance, {
units: "meters",
});
const currentNavigationChain = this.sliceNavigationChainAlong(
this.animationChain,
moveDistance,
{ units: "meters" }
);
const navigationStatus = this.navegador.getNavigationStatus(
nextPositionOnPath
);
if (navigationStatus) {
this.currentPositionOnPath = nextPositionOnPath;
this.currentNavigationPath = currentNavigationChain;
this.navigationHint =
this.getNavigationStatusText({
currentCartogramId: nextPositionOnPath.properties["cartogram:id"],
navegador: this.navegador,
navigationStatus,
}) || "";
this.nextDirection = navigationStatus.nextDirection;
const restOfDistance = (this.restOfDistance = Math.floor(
navigationStatus.distanceToEnd
));
this.restOfTime = Math.ceil(restOfDistance / WALK_SPEED_PER_MINUTE);
}
return {
navigationMode: this.navigationMode,
finishingPoint: this.finishingPoint,
startingPoint: this.startingPoint,
fullNavigationPath: this.fullNavigationPath,
targetPositionOnPath: this.targetPositionOnPath,
currentPositionOnPath: this.currentPositionOnPath,
currentNavigationPath: this.currentNavigationPath,
restOfDistance: this.restOfDistance,
restOfTime: this.restOfTime,
navigationHint: this.navigationHint,
nextDirection: this.nextDirection,
};
}
setCurrentLocation(currentLocation: NavigationPosition) {
const {
navegador,
lastLocationTime,
timeDiffCountAdjust,
distanceCountAdjust,
cartogramDiffCountAdjust,
targetPositionOnPath,
locationAccuracyAdjust,
} = this;
const now = Date.now();
const currentLocationOnRoad = navegador.projectionPositionOnRoad(
currentLocation
);
if (!currentLocationOnRoad) {
return;
}
const currentLocationOnPath = navegador.projectionPositionOnChain(
currentLocationOnRoad
);
// 无法投影到导航路径上时,比如刚开始导航时就朝反方向移动的时候,此时会投影到导航路径延长线而不是导航路径上
if (!currentLocationOnPath) {
this.offRoadCount +=
1 * timeDiffCountAdjust * ((now - lastLocationTime) / 1000);
this.needReroute(currentLocation);
return;
}
const isCrossRoad =
currentLocationOnRoad.properties.road_index !==
currentLocationOnPath.properties.road_index;
// 当映射到道路上的道路编号不同于映射到导航路径上的道路编号时,典型情况是处于路口时
// 或者是跨楼层时
if (isCrossRoad) {
const dist =
turfDistance(currentLocationOnRoad, currentLocationOnPath) * 1000;
const isSameCartogram =
targetPositionOnPath.properties["cartogram:id"] ===
currentLocationOnPath.properties["cartogram:id"];
const minDistance =
currentLocationOnPath.properties.source === "satellite"
? this.offRoadMinDistance.satellite
: this.offRoadMinDistance.bluetooth;
// 增加偏离计数时考虑以下因素
// 1. 与上一次定位之间的时间差
// 2. 与上一次定位之间的距离
// 3. 本次定位的精度
this.offRoadCount +=
1 *
((now - lastLocationTime) / 1000) *
timeDiffCountAdjust *
(currentLocation.properties.accuracy * locationAccuracyAdjust) *
(isSameCartogram
? dist >= minDistance
? ((dist - 4) / 4) * distanceCountAdjust
: 0
: cartogramDiffCountAdjust);
if (dist > minDistance) {
this.needReroute(currentLocation);
}
return;
}
// 检测倒退的情况
const distToEnd = navegador.getDistanceToNavigationChainEnd(
currentLocationOnPath
) as number;
const distDiff = distToEnd - this.lastDistToEnd;
if (distDiff > 8) {
const isSameCartogram =
this.targetPositionOnPath.properties["cartogram:id"] ===
currentLocationOnPath.properties["cartogram:id"];
const minDistance =
currentLocationOnPath.properties.source === "satellite"
? this.offRoadMinDistance.satellite
: this.offRoadMinDistance.bluetooth;
// 增加后退计数时考虑以下因素
// 1. 与上一次定位之间的时间差
// 2. 与上一次定位之间的距离
// 3. 本次定位的精度
this.backwardCount +=
1 *
((now - lastLocationTime) / 1000) *
timeDiffCountAdjust *
(currentLocation.properties.accuracy * locationAccuracyAdjust) *
(isSameCartogram
? distDiff >= minDistance
? ((distDiff - 4) / 4) * distanceCountAdjust
: 0
: cartogramDiffCountAdjust);
this.needReroute(currentLocation);
}
if (distDiff > 0) {
return;
}
this.targetPositionOnPath = currentLocationOnPath;
this.lastDistToEnd = distToEnd;
// 如果执行到此处,说明一切正常, 说明没有发生倒退也没有定位到其他道路上的情况
// 表示用户在沿着导航路径前进,此时重置 count,缩短导航路径
this.offRoadCount = 0;
this.backwardCount = 0;
this.lastLocationTime = now;
// 计算需要移动的距离
const sSegmentIndex = this.currentPositionOnPath.properties.segment_index;
const tSegmentIndex = currentLocationOnPath.properties.segment_index;
this.animationDistance =
sSegmentIndex === tSegmentIndex
? turfDistance(this.currentPositionOnPath, currentLocationOnPath, {
units: "meters",
})
: this.fullNavigationPath.features.reduce((d, f) => {
const fSegmentIndex = f.properties.segment_index;
if (fSegmentIndex === sSegmentIndex) {
const sp = this.currentPositionOnPath;
const tp = turfPoint(turfGetCoords(f)[1]);
const ld = turfDistance(sp, tp, {
units: "meters",
});
return d + ld;
}
if (fSegmentIndex === tSegmentIndex) {
return (
d +
turfDistance(
turfPoint(turfGetCoords(f)[0]),
this.targetPositionOnPath,
{
units: "meters",
}
)
);
}
if (
fSegmentIndex > sSegmentIndex &&
fSegmentIndex < tSegmentIndex
) {
return d + this.segmentDistance(f);
}
return d;
}, 0);
this.animationChain = this.sliceNavigationChain(
this.fullNavigationPath,
this.currentPositionOnPath
);
this.emit("info");
}
private needReroute(currentLocation: NavigationPosition) {
if (
this.offRoadCount >= this.offRoadCountLimit ||
this.backwardCount >= this.backwardCountLimit
) {
try {
this.reroute(currentLocation);
} catch (err) {
console.error(err);
}
}
}
private reroute(currentLocation: NavigationPosition) {
const navegador = this.navegador;
const navigationMode = this.navigationMode;
const finishingPoint = this.finishingPoint;
const startingPoint = currentLocation;
const routeOptions = this.routeOptions;
const startingPointOnRoad = navegador.projectionPositionOnRoad(
startingPoint
);
if (!startingPointOnRoad) {
throw new Error("starting point can not be projected on road");
}
const fullNavigationPath = navegador.getNavigationChain(
navigationMode,
startingPoint,
finishingPoint,
routeOptions
);
if (!fullNavigationPath) {
throw new Error("can not get navigation path");
}
const startingPointOnPath = navegador.projectionPositionOnChain(
startingPointOnRoad
);
if (!startingPointOnPath) {
throw new Error("starting point can not be projected on navigation path");
}
const navigationStatus = navegador.getNavigationStatus(startingPointOnRoad);
if (!navigationStatus) {
throw new Error("can not get navigation status");
}
this.fullNavigationPath = fullNavigationPath;
this.lastLocationTime = Date.now();
this.animationDistance = 0;
this.animationChain = fullNavigationPath;
this.targetPositionOnPath = startingPointOnPath;
this.currentPositionOnPath = startingPointOnPath;
this.currentNavigationPath = fullNavigationPath;
this.offRoadCount = 0;
this.backwardCount = 0;
const restOfDistance = (this.lastDistToEnd = navegador.getDistanceToNavigationChainEnd(
startingPointOnPath
) as number);
this.restOfDistance = Math.floor(restOfDistance);
this.restOfTime = Math.ceil(restOfDistance / WALK_SPEED_PER_MINUTE);
this.navigationHint =
this.getNavigationStatusText({
currentCartogramId: startingPointOnPath.properties["cartogram:id"],
navegador: this.navegador,
navigationStatus,
}) || "";
this.nextDirection = navigationStatus.nextDirection;
this.emit("info");
this.emit("reroute");
}
private getNavigationStatusText(params: DirectionToTextParams) {
const {
navegador,
currentCartogramId,
navigationStatus: { nextCartogramId, nextDirection, text },
} = params;
let preActionText = "";
switch (nextDirection) {
case Directions.LeftChangeFloor:
case Directions.LeftChangeFloorLeft:
case Directions.LeftChangeFloorRight:
case Directions.LeftChangeFloorStraight:
preActionText = "左转";
break;
case Directions.RightChangeFloor:
case Directions.RightChangeFloorLeft:
case Directions.RightChangeFloorRight:
case Directions.RightChangeFloorStraight:
preActionText = "右转";
break;
case Directions.StraightChangeFloorLeft:
case Directions.StraightChangeFloorRight:
case Directions.StraightChangeFloorStraight:
default:
preActionText = "";
}
let postActionText = "";
switch (nextDirection) {
case Directions.LeftChangeFloorLeft:
case Directions.RightChangeFloorLeft:
case Directions.StraightChangeFloorLeft:
case Directions.LeftChangeFloorRight:
case Directions.RightChangeFloorRight:
case Directions.StraightChangeFloorRight:
case Directions.LeftChangeFloor:
case Directions.LeftChangeFloorStraight:
case Directions.RightChangeFloor:
case Directions.RightChangeFloorStraight:
case Directions.StraightChangeFloorStraight:
default:
postActionText = "";
break;
}
switch (nextDirection) {
case Directions.Left:
return "左转";
case Directions.Right:
return "右转";
case Directions.Straight:
return "直行";
case Directions.LeftArrive:
case Directions.RightArrive:
case Directions.Arrive:
return "目的地就在附近";
case Directions.LeftLeft:
return "连续左转";
case Directions.LeftRight:
return "左转后再右转";
case Directions.RightRight:
return "连续右转";
case Directions.RightLeft:
return "右转后再左转";
case Directions.ChangeFloor:
case Directions.LeftChangeFloor:
case Directions.LeftChangeFloorLeft:
case Directions.LeftChangeFloorRight:
case Directions.LeftChangeFloorStraight:
case Directions.RightChangeFloor:
case Directions.RightChangeFloorLeft:
case Directions.RightChangeFloorRight:
case Directions.RightChangeFloorStraight:
case Directions.StraightChangeFloorLeft:
case Directions.StraightChangeFloorRight:
case Directions.StraightChangeFloorStraight: {
const currentFloor = navegador._cartograms[currentCartogramId];
const nextFloor = navegador._cartograms[nextCartogramId];
const currentFloorNumber = (currentFloor as Cartogram).properties
.floor_number;
const {
floor_number: nextFloorNumber,
floor_label,
} = (nextFloor as Cartogram).properties;
if (nextFloorNumber > currentFloorNumber) {
return `请${preActionText}上楼到 ${floor_label} ${postActionText}`.trim();
}
return `请${preActionText}下楼到 ${floor_label} ${postActionText}`.trim();
}
default:
return text;
}
}
}
export default NavegadorTask;