yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[RouterOpt]车前引导线拖尾问题优化2

Browse Source
This commit is contained in:
renwj
2022-04-11 18:05:29 +08:00
parent c946f7ca94
commit a0f0d3c2c8
9 changed files with 280 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/MogoRouteOverlayManager.java
View File

@@ -3,6 +3,7 @@ package com.mogo.module.service.routeoverlay;
import static com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.M_OLD_ROUTE;
import android.content.Context;
import android.os.SystemClock;
import androidx.annotation.NonNull;
@@ -17,12 +18,12 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener
import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotPlanningListenerManager;
import com.mogo.eagle.core.function.call.map.CallerMapLocationListenerManager;
import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
import com.mogo.module.common.utils.DrivingDirectionUtils;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import mogo.telematics.pad.MessagePad;
@@ -38,7 +39,7 @@ public class MogoRouteOverlayManager implements
private int STATUS_AUTOPILOT = 0;//0 非自动驾驶 ; 1 自动驾驶
private MogoLatLng mEnding;
private MogoLocation mLocation;
private List<MogoLatLng> mTrajectoryList = new ArrayList<>();
private final List<MogoLatLng> mTrajectoryList = new ArrayList<>();
private MogoRouteOverlayManager(Context context) {
mContext = context;
@@ -62,37 +63,52 @@ public class MogoRouteOverlayManager implements
return sInstance;
}
private void intiDrawer() {
RouteOverlayDrawer.getInstance(mContext).initDraw();
}
private StringBuilder trajectoryMsg = null;
@Override
public void onAutopilotTrajectory(List<MessagePad.TrajectoryPoint> trajectoryInfos) {
if (trajectoryInfos == null || trajectoryInfos.size() == 0) {
return;
}
StringBuilder builder = new StringBuilder();
builder.append("{");
builder.append(System.currentTimeMillis()).append(";");
builder.append(mLocation.getLongitude()).append(";");
builder.append(mLocation.getLatitude()).append(";");
builder.append(mLocation.getAltitude()).append(";");
builder.append(mLocation.getBearing()).append(";");
builder.append(mLocation.getSpeed()).append(";");
List<MogoLatLng> mogoLatLngs = new ArrayList<>();
for (int i = 0; i < trajectoryInfos.size(); i++) {
// 临时解决车尾拖线问题，丢弃距离车最近的几个经纬度，原因是惯性导航的中心靠近车尾，会导致经纬度靠近尾部，且两个数据不同频
if (i > 5) {
MessagePad.TrajectoryPoint a = trajectoryInfos.get(i);
builder.append(a.getLongitude()).append(",");
builder.append(a.getLatitude()).append(",");
mogoLatLngs.add(new MogoLatLng(a.getLatitude(), a.getLongitude()));
public void onAutopilotTrajectory(List<MessagePad.TrajectoryPoint> items) {
long start = SystemClock.elapsedRealtime();
try {
if (items == null || items.size() == 0) {
return;
}
if (trajectoryMsg == null) {
trajectoryMsg = new StringBuilder(256);
}
if (trajectoryMsg.length() > 0) {
trajectoryMsg.setLength(0);
}
if (mLocation == null) {
return;
}
trajectoryMsg.append("{");
trajectoryMsg.append(SystemClock.elapsedRealtime()).append(";");
trajectoryMsg.append(mLocation.getLongitude()).append(";");
trajectoryMsg.append(mLocation.getLatitude()).append(";");
trajectoryMsg.append(mLocation.getAltitude()).append(";");
trajectoryMsg.append(mLocation.getBearing()).append(";");
trajectoryMsg.append(mLocation.getSpeed()).append(";");
List<MogoLatLng> points = new ArrayList<>();
for (int i = 0; i < items.size(); i++) {
// 临时解决车尾拖线问题，丢弃距离车最近的几个经纬度，原因是惯性导航的中心靠近车尾，会导致经纬度靠近尾部，且两个数据不同频
MessagePad.TrajectoryPoint a = items.get(i);
double lon = a.getLongitude();
double lat = a.getLatitude();
trajectoryMsg.append(lon).append(",");
trajectoryMsg.append(lat).append(",");
MogoLatLng latLng = new MogoLatLng(lat, lon);
latLng.acceleration = a.getAcceleration();
points.add(latLng);
}
synchronized (mTrajectoryList) {
mTrajectoryList.clear();
mTrajectoryList.addAll(points);
}
trajectoryMsg.append("}");
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, trajectoryMsg.toString());
} finally {
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onAutopilotTrajectory -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms");
}
mTrajectoryList = mogoLatLngs;
builder.append("}");
}
@Override
@@ -136,7 +152,7 @@ public class MogoRouteOverlayManager implements
if (arrivalNotification == null) {
return;
}
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "onArriveAt data : " + arrivalNotification.toString());
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "onArriveAt data : " + arrivalNotification);
// //演示模式下 到达终点将忽略 引导线绘制 选项关闭
// FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData = false;
}
@@ -149,66 +165,37 @@ public class MogoRouteOverlayManager implements
@Override
public void onLocationChanged(@Nullable MogoLocation location) {
// mLocation = location;
// if (mTrajectoryList.isEmpty()) {
// return;
// }
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged : " + mTrajectoryList.size());
// ArrayList list = new ArrayList();
// for (MogoLatLng latLng : mTrajectoryList) {
// if (!isPointOnCarFront(mLocation, latLng)) {
// list.add(latLng);
// }
// }
//
// StringBuilder builder = new StringBuilder();
// for (int i = 0; i < list.size(); i++) {
// MogoLatLng latLng = (MogoLatLng) list.get(i);
// builder.append(latLng.getLon()).append(",");
// builder.append(latLng.getLat()).append(",");
// }
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged : " + GsonUtils.toJson(builder.toString()));
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG ,"onLocationChanged size = " + list.size() + "---TrajectoryData = " + FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData + "----" + STATUS_AUTOPILOT);
// if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1) {
// RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(list);
// }
mLocation = location;
List<MogoLatLng> temp = mTrajectoryList;
if (temp.isEmpty()) {
return;
}
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG , "onLocationChanged: size = "+ mTrajectoryList.size()+" ----- "+mLocation.getLongitude()+"-"+mLocation.getLatitude());
ArrayList list = new ArrayList();
MogoLatLng carlatLng = new MogoLatLng(CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat(),CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon());
list.add(carlatLng);
for (MogoLatLng latLng : temp) {
// if(!isPointOnCarFront(mLocation,latLng)){
list.add(latLng);
// }
}
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "TrajectoryData = " + FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData + "---status = " + STATUS_AUTOPILOT + "----size = " + list.size());
if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1) {
RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(list);
}
}
public boolean isPointOnCarFront(MogoLocation carLocal, MogoLatLng pointLocal) {
double carLon = carLocal.getLongitude();
double carLat = carLocal.getLatitude();
double poiLon = pointLocal.lon;
double poiLat = pointLocal.lat;
float carAngle = carLocal.getBearing();
// 计算车辆与点之间的夹角
int diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
carLon, carLat, poiLon, poiLat, (int) carAngle);
// CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG,"diffAngle:"+diffAngle);
if (diffAngle <= 90) {
return true;
} else {
return false;
long start = SystemClock.elapsedRealtime();
boolean isExcept = false;
try {
if (location == null) {
isExcept = true;
return;
}
if (mLocation != null && mLocation.getLongitude() == location.getLongitude() && mLocation.getLatitude() == location.getLatitude()) {
return;
}
LinkedList<MogoLatLng> points;
synchronized (mTrajectoryList) {
points = new LinkedList<>(mTrajectoryList);
}
if (points.isEmpty()) {
isExcept = true;
return;
}
if (points.size() < 2) {
isExcept = true;
return;
}
if (FunctionBuildConfig.isIgnoreConditionsDrawAutopilotTrajectoryData || STATUS_AUTOPILOT == 1) {
RouteOverlayDrawer.getInstance(mContext).drawTrajectoryList(location, points);
}
} finally {
mLocation = location;
if (isExcept) {
RouteOverlayDrawer.getInstance(mContext).setVisible(false);
}
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "--- onLocationChanged -- cost:" + (SystemClock.elapsedRealtime() - start) + " ms");
}
}

154
modules/mogo-module-service/src/main/java/com/mogo/module/service/routeoverlay/RouteOverlayDrawer.java
View File

@@ -6,8 +6,12 @@ import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Color;
import android.os.Handler;
import android.os.HandlerThread;
import android.os.SystemClock;
import com.mogo.eagle.core.data.map.MogoLatLng;
import com.mogo.eagle.core.data.map.MogoLocation;
import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager;
import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger;
import com.mogo.eagle.core.utilcode.util.ColorUtils;
@@ -18,20 +22,23 @@ import com.mogo.map.marker.MogoMarkerOptions;
import com.mogo.map.overlay.IMogoOverlayManager;
import com.mogo.map.overlay.IMogoPolyline;
import com.mogo.map.overlay.MogoPolylineOptions;
import com.mogo.module.common.utils.DrivingDirectionUtils;
import com.mogo.module.service.R;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
public class RouteOverlayDrawer {
private static final String TAG = "MogoRouteOverlayManager";
private IMogoPolyline mMoGoPolyline;
private volatile IMogoPolyline mMoGoPolyline;
// 连接线参数
private final MogoPolylineOptions mPolylineOptions;
// 线路径集合
private final List<MogoLatLng> mPolylinePointList;
private Handler mRenderHandler;
// 渐变色
private final List<Integer> mPolylineColors;
private final Bitmap endingBitmap;
@@ -46,14 +53,30 @@ public class RouteOverlayDrawer {
mPolylineOptions = new MogoPolylineOptions();
mPolylineOptions.zIndex(75000f);
mPolylineOptions.setGps(true);
// 绘制路径集合
mPolylinePointList = new ArrayList<>();
// 引导线颜色,
mPolylineColors = new ArrayList<>();
mContext = context;
mogoOverlayManager = MogoOverlayManager.getInstance();
endingBitmap = BitmapFactory.decodeResource(context.getResources(),
R.drawable.icon_route_ending);
List<Integer> list = new ArrayList<>();
int[] startColor = ColorUtils.hexToArgb("#CC64C3EA");
int[] endColor = ColorUtils.hexToArgb("#0064C3EA");
list.add(Color.argb(startColor[0], startColor[1], startColor[2], startColor[3]));
list.add(Color.argb(endColor[0], endColor[1], endColor[2], endColor[3]));
mPolylineColors.addAll(list);
// 线条粗细，渐变，渐变色值
mPolylineOptions.width(20).useGradient(true).colorValues(mPolylineColors);
HandlerThread renderTask = new HandlerThread("xxxx") {
@Override
protected void onLooperPrepared() {
super.onLooperPrepared();
mRenderHandler = new Handler(getLooper());
}
};
renderTask.start();
}
public static RouteOverlayDrawer getInstance(Context context) {
@@ -103,7 +126,6 @@ public class RouteOverlayDrawer {
if (mMoGoPolyline != null) {
mMoGoPolyline.remove();
mMoGoPolyline = null;
mPolylinePointList.clear();
mPolylineColors.clear();
}
}
@@ -114,57 +136,91 @@ public class RouteOverlayDrawer {
MogoMarkerManager.getInstance(mContext).removeMarkers(markerType);
}
private class RenderTask implements Runnable {
private volatile LinkedList<MogoLatLng> routeList;
private volatile MogoLocation location;
public void drawTrajectoryList(List<MogoLatLng> routeList) {
// clearMogoRouteOverlay();
mPolylinePointList.clear();
if (routeList != null && routeList.size() > 0) {
for (MogoLatLng latLng : routeList) {
mPolylinePointList.add(latLng);
public void setData(MogoLocation location, LinkedList<MogoLatLng> routeList) {
this.location = location;
this.routeList = routeList;
}
@Override
public void run() {
try {
long drawStart = SystemClock.elapsedRealtime();
double lon = CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon();
double lat = CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat();
if (lon == 0.0 || lat == 0.0) {
return;
}
LinkedList<MogoLatLng> points = this.routeList;
MogoLocation location = this.location;
if (location != null && points != null && points.size() > 0) {
int i = 0;
int max = Math.min(20, points.size() / 2);
while (i++ < max) {
MogoLatLng first = points.peek();
if (first == null) {
continue;
}
if (!isPointOnCarFront(lon, lat, location.getBearing(), first.lon, first.lat)) {
points.poll();
}
}
points.addFirst(new MogoLatLng(lat, lon));
if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
mPolylineOptions.points(points);
mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
} else {
mPolylineOptions.points(points);
mMoGoPolyline.setOption(mPolylineOptions);
}
if (mMoGoPolyline != null && !mMoGoPolyline.isDestroyed() && !mMoGoPolyline.isVisible()) {
mMoGoPolyline.setVisible(true);
}
} else {
if(mMoGoPolyline != null) {
mMoGoPolyline.setVisible(false);
}
}
long drawEnd = SystemClock.elapsedRealtime();
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "drawTrajectoryList cost : " + (drawEnd - drawStart));
} catch (Throwable t) {
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "drawTrajectoryList error : " + t);
}
mPolylineColors.clear();
// mPolylineColors.addAll(ColorUtils.gradientAlpha_("#FF2AAFFD", "#7b2965ED", "#002965ED", mPolylinePointList.size()));
List<Integer> list = new ArrayList<>();
// list = ColorUtils.gradientAlpha("#FF2AAFFD", "#002965ED", mPolylinePointList.size());
int[] startColor = ColorUtils.hexToArgb("#CC64C3EA");
int[] endColor = ColorUtils.hexToArgb("#0064C3EA");
list.add(Color.argb(startColor[0], startColor[1], startColor[2], startColor[3]));
list.add(Color.argb(endColor[0], endColor[1], endColor[2], endColor[3]));
}
mPolylineColors.addAll(list);
// 线条粗细，渐变，渐变色值
mPolylineOptions.width(20).useGradient(true).colorValues(mPolylineColors);
if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
mPolylineOptions.points(mPolylinePointList);
mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
} else {
mPolylineOptions.points(mPolylinePointList);
mMoGoPolyline.setOption(mPolylineOptions);
private boolean isPointOnCarFront(double car_lon, double car_lat, double car_head, double lon, double lat) {
long start = SystemClock.elapsedRealtime();
try {
// 计算车辆与点之间的夹角
long diffAngle = DrivingDirectionUtils.getDegreeOfCar2Poi(
car_lon, car_lat, lon, lat, car_head);
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "isPointOnCarFront: angle->" + diffAngle);
return diffAngle < 90;
} finally {
CallerLogger.INSTANCE.d(M_OLD_ROUTE + TAG, "isPointOnCarFront cost:" + (SystemClock.elapsedRealtime() - start) + "ms");
}
}
}
public void initDraw() {
mPolylinePointList.clear();
MogoLatLng latLng = new MogoLatLng(
CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lat(),
CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84Lon());
mPolylinePointList.add(latLng);
mPolylinePointList.add(latLng);
mPolylineColors.clear();
long start = System.currentTimeMillis();
List<Integer> list = new ArrayList<>();
list = ColorUtils.gradientAlpha("#FF2AAFFD", "#002965ED", mPolylinePointList.size());
private volatile RenderTask mRenderTask;
mPolylineColors.addAll(list);
// 线条粗细，渐变，渐变色值
mPolylineOptions.width(12).useGradient(true).colorValues(mPolylineColors);
if (mMoGoPolyline == null || mMoGoPolyline.isDestroyed()) {
mPolylineOptions.points(mPolylinePointList);
mMoGoPolyline = mogoOverlayManager.addPolyline(mPolylineOptions);
} else {
mPolylineOptions.points(mPolylinePointList);
mMoGoPolyline.setOption(mPolylineOptions);
public void drawTrajectoryList(MogoLocation carLoc, LinkedList<MogoLatLng> routeList) {
if (mRenderTask == null) {
mRenderTask = new RenderTask();
}
mRenderTask.setData(carLoc, routeList);
if (mRenderHandler != null) {
mRenderHandler.removeCallbacks(mRenderTask);
mRenderHandler.post(mRenderTask);
}
}
public void setVisible(boolean isVisible) {
if (mMoGoPolyline != null && !mMoGoPolyline.isDestroyed()) {
mMoGoPolyline.setVisible(isVisible);
}
}
}