diff --git a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/AdasRecognizedResultDrawer.java b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/AdasRecognizedResultDrawer.java index 4e9a68bff1..b06bbdf117 100644 --- a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/AdasRecognizedResultDrawer.java +++ b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/AdasRecognizedResultDrawer.java @@ -1,19 +1,18 @@ package com.mogo.module.common.drawer; +import android.os.Build; import android.text.TextUtils; import android.util.Log; -import com.mogo.cloud.socket.entity.SocketDownData; -import com.mogo.cloud.socket.entity.SocketDownDataHelper; +import com.mogo.cloud.commons.utils.CoordinateUtils; import com.mogo.commons.debug.DebugConfig; import com.mogo.map.MogoLatLng; import com.mogo.map.marker.IMogoMarker; import com.mogo.map.marker.MogoMarkerOptions; import com.mogo.module.common.MogoApisHandler; import com.mogo.module.common.constants.DataTypes; -import com.mogo.module.common.utils.SimpleHandlerThreadPool; +import com.mogo.module.common.utils.Trigonometric; import com.mogo.service.adas.entity.ADASRecognizedResult; -import com.mogo.service.adas.IMogoADASController; import java.util.ArrayList; import java.util.Iterator; @@ -154,7 +153,8 @@ class AdasRecognizedResultDrawer extends BaseDrawer { } /** - * 对数据补点 //todo 重构 + * 对数据补点 + * * @param in * @param out */ @@ -163,22 +163,43 @@ class AdasRecognizedResultDrawer extends BaseDrawer { out.addAll(in); } + private final static String FORECAST = "adasForecast"; + /** - * 预测补点 + * 基于工控机识别的数据点速度预测当前位置和距离自车距离 * - * @param data 道路数据集合 + * @param in 数据源 */ - private void foreCastPoint(List data) { -// if (data == null || data.isEmpty()) { -// return; -// } -// List newList = new ArrayList<>(); -// for (ADASRecognizedResult adasRecognizedResult : data) { -// //todo 补点 -// newList.add(adasRecognizedResult); -// } -// data.clear(); -// data.addAll(newList); + private void foreCastPoint(List in) { + if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.N) { + for (ADASRecognizedResult adasResult : in) { + Log.d(FORECAST, "ready to foreCast current uuid : " + adasResult.uuid); + long internal = getCurSatelliteTime() - adasResult.satelliteTime; + if (internal <= 0) { + Log.d(FORECAST, "time internal less than 0 , uuid : " + adasResult.uuid); + continue; + } + long startTime = System.nanoTime(); + // 预测点 + Log.d(FORECAST, "time internal : " + internal); + double foreCastDistance = adasResult.speed * internal; + Log.d(FORECAST, "foreCastDistance : " + foreCastDistance); + MogoLatLng mogoLatLng = new MogoLatLng(adasResult.lat, adasResult.lon); + MogoLatLng foreCastMogoLatLon = Trigonometric.getNewLocation(mogoLatLng, foreCastDistance, adasResult.heading); + + // 计算与自车距离 + float distanceFromSelf = CoordinateUtils.calculateLineDistance(getCurCoordinates()[0], getCurCoordinates()[1] + , foreCastMogoLatLon.getLon(), foreCastMogoLatLon.getLat()); + + long foreCastInternal = System.nanoTime() - startTime; + Log.d(FORECAST, "foreCastInternal :" + foreCastInternal); //todo 看是否耗时，增加的时间需要从当前SNTP时间减去 + + adasResult.lat = foreCastMogoLatLon.getLat(); + adasResult.lon = foreCastMogoLatLon.getLon(); + adasResult.satelliteTime = (getCurSatelliteTime() - foreCastInternal); + adasResult.distance = distanceFromSelf; + } + } } /** @@ -207,7 +228,7 @@ class AdasRecognizedResultDrawer extends BaseDrawer { return; } Iterator iterator = mLastPositions.values().iterator(); - Log.d("EmArrow","removeUselessLastRecord size : " + mLastPositions.size()); + Log.d("EmArrow", "removeUselessLastRecord size : " + mLastPositions.size()); while (iterator.hasNext()) { ADASRecognizedResult result = iterator.next(); long internal = result.satelliteTime - getCurSatelliteTime(); @@ -219,6 +240,7 @@ class AdasRecognizedResultDrawer extends BaseDrawer { /** * 过滤无用数据 + * * @param recognizedListResult {@link ADASRecognizedResult} * @return useless */ @@ -236,7 +258,7 @@ class AdasRecognizedResultDrawer extends BaseDrawer { /** * 绘制某个物体的一个数据 todo 缓存问题 * - * @param recognizedListResult {@link ADASRecognizedResult} + * @param recognizedListResult {@link ADASRecognizedResult} * @param newAdasRecognizedMarkersCaches */ private void renderAdasOneFrame(IMogoMarker marker, diff --git a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/SnapshotSetDataDrawer.java b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/SnapshotSetDataDrawer.java index 81763cda2c..02f2812a5d 100644 --- a/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/SnapshotSetDataDrawer.java +++ b/modules/mogo-module-common/src/main/java/com/mogo/module/common/drawer/SnapshotSetDataDrawer.java @@ -7,6 +7,7 @@ import android.view.LayoutInflater; import android.view.View; import android.widget.ImageView; +import com.mogo.cloud.commons.utils.CoordinateUtils; import com.mogo.cloud.socket.entity.SocketDownData; import com.mogo.cloud.socket.entity.SocketDownDataHelper; import com.mogo.commons.AbsMogoApplication; @@ -358,28 +359,42 @@ public class SnapshotSetDataDrawer extends BaseDrawer implements IMogoMarkerClic data.addAll(newList); } + private final static String FORECAST = "snapshotForecast"; + + /** + * 基于云平台下发的数据点速度预测当前位置和距离自车距离 + * + * @param in 数据源 + */ private void foreCastPoint(List in) { if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.N) { for (SocketDownData.CloudRoadDataProto proto : in) { SocketDownData.CloudRoadDataProto.Builder builder = proto.toBuilder(); + Log.d(FORECAST, "ready to foreCast current uuid : " + proto.getUuid()); long internal = getCurSatelliteTime() - builder.getSatelliteTime(); if (internal <= 0) { + Log.d(FORECAST, "time internal less than 0 , uuid : " + proto.getUuid()); continue; } long startTime = System.nanoTime(); // 预测点 + Log.d(FORECAST, "time internal : " + internal); double foreCastDistance = proto.getSpeed() * internal; - MogoLatLng mogoLatLng = new MogoLatLng(proto.getWgslat(),proto.getWgslon()); - MogoLatLng foreCastMogoLatLon = Trigonometric.getNewLocation(mogoLatLng,foreCastDistance,proto.getHeading()); + Log.d(FORECAST, "foreCastDistance : " + foreCastDistance); + MogoLatLng mogoLatLng = new MogoLatLng(proto.getWgslat(), proto.getWgslon()); + MogoLatLng foreCastMogoLatLon = Trigonometric.getNewLocation(mogoLatLng, foreCastDistance, proto.getHeading()); // 计算与自车距离 -// getCurCoordinates(); + float distanceFromSelf = CoordinateUtils.calculateLineDistance(getCurCoordinates()[0], getCurCoordinates()[1] + , foreCastMogoLatLon.getLon(), foreCastMogoLatLon.getLat()); long foreCastInternal = System.nanoTime() - startTime; - Log.d("ForeCastTime","foreCastInternal :" + foreCastDistance); //todo 看是否耗时，增加的时间需要从当前SNTP时间减去 + Log.d(FORECAST, "foreCastInternal :" + foreCastInternal); //todo 看是否耗时，增加的时间需要从当前SNTP时间减去 + builder.setWgslat(foreCastMogoLatLon.getLat()); builder.setWgslon(foreCastMogoLatLon.getLon()); builder.setSatelliteTime(getCurSatelliteTime() - foreCastInternal); + builder.setDistance(distanceFromSelf); } } } diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/marker/MapMarkerManager.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/marker/MapMarkerManager.java index c59c133bbd..34c221c1f3 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/marker/MapMarkerManager.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/marker/MapMarkerManager.java @@ -148,10 +148,11 @@ public class MapMarkerManager implements IMogoMarkerClickListener, @Override public void onMsgReceived(SocketDownData.LauncherSnapshotProto mogoSnapshotSetData) { DebugConfig.setStatus(DebugConfig.sDownloadSnapshot, true); - Message msg = mSnapshotHandler.obtainMessage(); + Message msg = new Message(); msg.obj = mogoSnapshotSetData; msg.what = MSG_SNAPSHOT; msg.sendToTarget(); + mSnapshotHandler.sendMessage(msg); } });