From b00e553fd4a09763c9b454f74adcb0418854cec2 Mon Sep 17 00:00:00 2001 From: liujing Date: Wed, 20 Jan 2021 21:20:14 +0800 Subject: [PATCH] [add] --- .../cloud/commons/utils/CoordinateUtils.java | 119 ++++++++ .../constant/SnapshotUploadInTime.java | 120 ++++++++ .../realtime/entity/ADASRecognizedResult.java | 72 +++++ .../realtime/entity/CloudLocationInfo.java | 174 ++++++++++++ .../realtime/location/LocationResult.java | 35 +++ .../realtime/location/MogoRTKLocation.java | 179 ++++++++++++ .../com/mogo/realtime/util/MortonCode.java | 149 ++++++++++ .../util/SimpleLocationCorrectStrategy.java | 257 ++++++++++++++++++ .../realtime/websocket/LocationResult.java | 36 +++ .../websocket/OnePerSecondSendContent.java | 27 ++ 10 files changed, 1168 insertions(+) create mode 100644 foudations/mogo-commons/src/main/java/com/mogo/cloud/commons/utils/CoordinateUtils.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/constant/SnapshotUploadInTime.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/ADASRecognizedResult.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/CloudLocationInfo.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/location/LocationResult.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/location/MogoRTKLocation.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/util/MortonCode.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/util/SimpleLocationCorrectStrategy.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/LocationResult.java create mode 100644 modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/OnePerSecondSendContent.java diff --git a/foudations/mogo-commons/src/main/java/com/mogo/cloud/commons/utils/CoordinateUtils.java b/foudations/mogo-commons/src/main/java/com/mogo/cloud/commons/utils/CoordinateUtils.java new file mode 100644 index 0000000..93aec7a --- /dev/null +++ b/foudations/mogo-commons/src/main/java/com/mogo/cloud/commons/utils/CoordinateUtils.java @@ -0,0 +1,119 @@ +package com.mogo.cloud.commons.utils; + +/** + * @author donghongyu + */ +public class CoordinateUtils { + + private static double a = 6378245.0; + private static double ee = 0.00669342162296594323; + + /** + * 手机GPS坐标转火星坐标 + * + * @return + */ + public static double[] transformFromWGSToGCJ( double lat, double lon ) { + + //如果在国外，则默认不进行转换 + if ( outOfChina( lat, lon ) ) { + return new double[]{lat, lon}; + } + double dLat = transformLat( lon - 105.0, lat - 35.0 ); + double dLon = transformLon( lon - 105.0, lat - 35.0 ); + double radLat = lat / 180.0 * Math.PI; + double magic = Math.sin( radLat ); + magic = 1 - ee * magic * magic; + double sqrtMagic = Math.sqrt( magic ); + dLat = ( dLat * 180.0 ) / ( ( a * ( 1 - ee ) ) / ( magic * sqrtMagic ) * Math.PI ); + dLon = ( dLon * 180.0 ) / ( a / sqrtMagic * Math.cos( radLat ) * Math.PI ); + + return new double[]{lat + dLat, lon + dLon}; + } + + public static double transformLat( double x, double y ) { + double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * Math.sqrt( x > 0 ? x : -x ); + ret += ( 20.0 * Math.sin( 6.0 * x * Math.PI ) + 20.0 * Math.sin( 2.0 * x * Math.PI ) ) * 2.0 / 3.0; + ret += ( 20.0 * Math.sin( y * Math.PI ) + 40.0 * Math.sin( y / 3.0 * Math.PI ) ) * 2.0 / 3.0; + ret += ( 160.0 * Math.sin( y / 12.0 * Math.PI ) + 320 * Math.sin( y * Math.PI / 30.0 ) ) * 2.0 / 3.0; + return ret; + } + + public static double transformLon( double x, double y ) { + double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * Math.sqrt( x > 0 ? x : -x ); + ret += ( 20.0 * Math.sin( 6.0 * x * Math.PI ) + 20.0 * Math.sin( 2.0 * x * Math.PI ) ) * 2.0 / 3.0; + ret += ( 20.0 * Math.sin( x * Math.PI ) + 40.0 * Math.sin( x / 3.0 * Math.PI ) ) * 2.0 / 3.0; + ret += ( 150.0 * Math.sin( x / 12.0 * Math.PI ) + 300.0 * Math.sin( x / 30.0 * Math.PI ) ) * 2.0 / 3.0; + return ret; + } + + public static boolean outOfChina( double lat, double lon ) { + if ( lon < 72.004 || lon > 137.8347 ) + return true; + if ( lat < 0.8293 || lat > 55.8271 ) + return true; + return false; + } + + public static final double[] transformGcj02toWgs84( double lat, double lng ) { + double[] var10000; + if ( outOfChina( lat, lng ) ) { + var10000 = new double[]{lng, lat}; + } else { + double dlat = transformLat( lng - 105.0D, lat - 35.0D ); + double dlng = transformLon( lng - 105.0D, lat - 35.0D ); + double radlat = lat / 180.0D * Math.PI; + double magic = Math.sin( radlat ); + magic = ( double ) 1 - 0.006693421622965943D * magic * magic; + double sqrtmagic = Math.sqrt( magic ); + dlat = dlat * 180.0D / ( 6335552.717000426D / ( magic * sqrtmagic ) * Math.PI ); + dlng = dlng * 180.0D / ( 6378245.0D / sqrtmagic * Math.cos( radlat ) * Math.PI ); + double mglat = lat + dlat; + double mglng = lng + dlng; + var10000 = new double[]{lng * ( double ) 2 - mglng, lat * ( double ) 2 - mglat}; + } + + return var10000; + } + + /** + * @param lon1 + * @param lat1 + * @param lon2 + * @param lat2 + * @return 两坐标的距离 单位：米（M） + */ + public static float calculateLineDistance( double lon1, double lat1, double lon2, double lat2 ) { + try { + double var2 = lon1; + double var4 = lat1; + double var6 = lon2; + double var8 = lat2; + var2 *= 0.01745329251994329D; + var4 *= 0.01745329251994329D; + var6 *= 0.01745329251994329D; + var8 *= 0.01745329251994329D; + double var10 = Math.sin( var2 ); + double var12 = Math.sin( var4 ); + double var14 = Math.cos( var2 ); + double var16 = Math.cos( var4 ); + double var18 = Math.sin( var6 ); + double var20 = Math.sin( var8 ); + double var22 = Math.cos( var6 ); + double var24 = Math.cos( var8 ); + double[] var28 = new double[3]; + double[] var29 = new double[3]; + var28[0] = var16 * var14; + var28[1] = var16 * var10; + var28[2] = var12; + var29[0] = var24 * var22; + var29[1] = var24 * var18; + var29[2] = var20; + return ( float ) ( Math.asin( Math.sqrt( ( var28[0] - var29[0] ) * ( var28[0] - var29[0] ) + ( var28[1] - var29[1] ) * ( var28[1] - var29[1] ) + ( var28[2] - var29[2] ) * ( var28[2] - var29[2] ) ) / 2.0D ) * 1.27420015798544E7D ); + } catch ( Throwable var26 ) { + var26.printStackTrace(); + return 0.0F; + } + } + +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/constant/SnapshotUploadInTime.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/constant/SnapshotUploadInTime.java new file mode 100644 index 0000000..54026d1 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/constant/SnapshotUploadInTime.java @@ -0,0 +1,120 @@ +package com.mogo.realtime.constant; + +import android.content.Context; + +import com.mogo.realtime.entity.ADASRecognizedResult; +import com.mogo.realtime.entity.CloudLocationInfo; +import com.mogo.realtime.location.LocationResult; +import com.mogo.realtime.location.MogoRTKLocation; +import com.mogo.realtime.util.MortonCode; +import com.mogo.realtime.util.SimpleLocationCorrectStrategy; +import com.mogo.realtime.websocket.OnePerSecondSendContent; + +import java.util.List; +import java.util.logging.Logger; + +/** + * @author congtaowang + * @since 2020/12/14 + * + * 实时上报坐标、识别物体 + */ +public class SnapshotUploadInTime implements MogoRTKLocation.RTKLocationListener { + + private static final String TAG = "SnapshotUploadInTime"; + + private static volatile SnapshotUploadInTime sInstance; + private Context mContext; + + private SnapshotUploadInTime() { + } + + public static SnapshotUploadInTime getInstance() { + if ( sInstance == null ) { + synchronized ( SnapshotUploadInTime.class ) { + if ( sInstance == null ) { + sInstance = new SnapshotUploadInTime(); + } + } + } + return sInstance; + } + + public synchronized void release() { + sInstance = null; + } + + private Object readResolve() { + // 阻止反序列化，必须实现 Serializable 接口 + return sInstance; + } + + public void start( Context context ) { + mContext = context.getApplicationContext(); + MogoRTKLocation.getInstance().registerRTKLocationListener( this ); + } + + public void stop() { + MogoRTKLocation.getInstance().unregisterRTKLocationListener(); + MogoRTKLocation.getInstance().stop(); + } + + @Override + public void onLocationChanged( List< CloudLocationInfo > cloudLocationInfos ) { + startSendCarLocationAndAdasRecognizedResult2Server( cloudLocationInfos ); + } + + private CloudLocationInfo mLastInfo; + + private void startSendCarLocationAndAdasRecognizedResult2Server( List< CloudLocationInfo > cloudLocationInfo ) { + CloudLocationInfo lastInfo = null; + // 如果数组内容不为空，就用数组最后一个值 + if ( cloudLocationInfo != null && !cloudLocationInfo.isEmpty() ) { + lastInfo = cloudLocationInfo.get( cloudLocationInfo.size() - 1 ); + mLastInfo = lastInfo; + } + if ( lastInfo == null ) { + lastInfo = mLastInfo; + } + LocationResult locationResult = null; + if ( lastInfo != null ) { + // 定位点预测纠偏 + lastInfo = SimpleLocationCorrectStrategy.getInstance().correct( lastInfo ); + locationResult = new LocationResult(); + if ( lastInfo != null ) { + locationResult.lastCoordinate = lastInfo; + locationResult.mortonCode = MortonCode.wrapEncodeMorton( lastInfo.getLon(), lastInfo.getLat() ); + } +// locationResult.coordinates = new ArrayList<>(); + locationResult.sn = com.mogo.commons.network.Utils.getSn(); +// if ( cloudLocationInfo == null ) { +// locationResult.coordinates.addAll( new ArrayList<>() ); +// } else { +// locationResult.coordinates.addAll( cloudLocationInfo ); +// } + } + List recognizedResults = null;//MarkerServiceHandler.getADASController().getLastADASRecognizedResult();//外显接口返回 + OnePerSecondSendContent content = new OnePerSecondSendContent(); + content.self = locationResult; + content.adas = recognizedResults; + + if ( content.self == null && + ( content.adas == null || content.adas.isEmpty() ) ) { + Logger.d( TAG, "no information 2 sent" ); + return; + } + + + MarkerServiceHandler.getApis().getWebSocketManagerApi( mContext ).sendMsg( content, new IMogoOnWebSocketMessageListener() { + @Override + public WebSocketMsgType getDownLinkType() { + return null; + } + + @Override + public WebSocketMsgType getUpLinkType() { + return WebSocketMsgType.MSG_TYPE_UPLINK_CAR_DATA; + } + } ); + } +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/ADASRecognizedResult.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/ADASRecognizedResult.java new file mode 100644 index 0000000..ab8a0b7 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/ADASRecognizedResult.java @@ -0,0 +1,72 @@ +package com.mogo.realtime.entity; + +public +/** + * @author congtaowang + * @since 2020/10/25 + * + * adas 识别物体参数 + */ +class ADASRecognizedResult { + + /** + * 识别物体类型 + */ + public int type; + + /** + * 识别物体唯一标识 + */ + public String uuid; + + /** + * 红绿灯颜色 + */ + public String color; + + /** + * + */ + public String carId; + + /** + * 识别物体的纬度 + */ + public double lat; + + /** + * 识别物体的经度 + */ + public double lon; + + /** + * 朝向 + */ + public double heading; + + /** + * 系统时间 + */ + public long systemTime; + + /** + * 定位卫星时间 + */ + public long satelliteTime; + + /** + * 海拔 + */ + public double alt; + + /** + * 速度 + */ + public double speed; + + /** + * 莫顿码 + */ + public long mortonCode; + +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/CloudLocationInfo.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/CloudLocationInfo.java new file mode 100644 index 0000000..ea09ba1 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/entity/CloudLocationInfo.java @@ -0,0 +1,174 @@ +package com.mogo.realtime.entity; + +import android.os.Parcel; +import android.os.Parcelable; + +import com.mogo.utils.CoordinateUtils; + +import java.util.Objects; + +/** + * 云端定位信息和自车定位信息 + * + * @author tongchenfei + */ +public class CloudLocationInfo implements Parcelable { + private double lat; + private double lon; + private double heading; + private long systemTime; + private long satelliteTime; + private double alt; + private double speed; + + public CloudLocationInfo() { + } + + public CloudLocationInfo(CloudLocationInfo info ) { + this.lat = info.getLat(); + this.lon = info.getLon(); + this.heading = info.getHeading(); + this.systemTime = System.currentTimeMillis(); + this.satelliteTime = System.currentTimeMillis(); + this.alt = info.alt; + this.speed = info.speed; + } + + public void convertCoor2GCJ02(){ + double[] amapCoord = CoordinateUtils.transformFromWGSToGCJ( lat, lon ); + if ( amapCoord != null ) { + this.lat = amapCoord[0]; + this.lon = amapCoord[1]; + } + } + + protected CloudLocationInfo(Parcel in ) { + lat = in.readDouble(); + lon = in.readDouble(); + heading = in.readDouble(); + systemTime = in.readLong(); + satelliteTime = in.readLong(); + alt = in.readDouble(); + speed = in.readDouble(); + } + + @Override + public void writeToParcel( Parcel dest, int flags ) { + dest.writeDouble( lat ); + dest.writeDouble( lon ); + dest.writeDouble( heading ); + dest.writeLong( systemTime ); + dest.writeLong( satelliteTime ); + dest.writeDouble( alt ); + dest.writeDouble( speed ); + } + + @Override + public int describeContents() { + return 0; + } + + public static final Creator< CloudLocationInfo > CREATOR = new Creator< CloudLocationInfo >() { + @Override + public CloudLocationInfo createFromParcel( Parcel in ) { + return new CloudLocationInfo( in ); + } + + @Override + public CloudLocationInfo[] newArray( int size ) { + return new CloudLocationInfo[size]; + } + }; + + public double getLat() { + return lat; + } + + public void setLat( double lat ) { + this.lat = lat; + } + + public double getLon() { + return lon; + } + + public void setLon( double lon ) { + this.lon = lon; + } + + public double getHeading() { + return heading; + } + + public void setHeading( double heading ) { + this.heading = heading; + } + + public long getSystemTime() { + return systemTime; + } + + public void setSystemTime( long systemTime ) { + this.systemTime = systemTime; + } + + public long getSatelliteTime() { + return satelliteTime; + } + + public void setSatelliteTime( long satelliteTime ) { + this.satelliteTime = satelliteTime; + } + + public double getAlt() { + return alt; + } + + public void setAlt( double alt ) { + this.alt = alt; + } + + public double getSpeed() { + return speed; + } + + public void setSpeed( double speed ) { + this.speed = speed; + } + + @Override + public String toString() { + return "CloudLocationInfo{" + + "lat=" + lat + + ", lon=" + lon + + ", heading=" + heading + + ", systemTime=" + systemTime + + ", satelliteTime=" + satelliteTime + + ", alt=" + alt + + ", speed=" + speed + + '}'; + } + + public String print() { + return "CloudLocation{ lon: " + lon + " lat: " + lat + " heading: " + heading + " speed: " + + speed + "}"; + } + + @Override + public boolean equals( Object o ) { + if ( this == o ) { + return true; + } + if ( o == null || getClass() != o.getClass() ) { + return false; + } + CloudLocationInfo that = ( CloudLocationInfo ) o; + return Double.compare( that.lat, lat ) == 0 && + Double.compare( that.lon, lon ) == 0; + } + + @Override + public int hashCode() { + return Objects.hash( lat, lon ); + } +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/LocationResult.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/LocationResult.java new file mode 100644 index 0000000..fb8ff9f --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/LocationResult.java @@ -0,0 +1,35 @@ +package com.mogo.realtime.location; + +import com.mogo.realtime.entity.CloudLocationInfo; + +import java.util.List; + +public +/** + * @author congtaowang + * @since 2020/10/25 + * + * 自车定位信息 + */ +class LocationResult { + + /** + * sn + */ + public String sn; + + /** + * 最后一个定位点的莫顿码 + */ + public long mortonCode; + + /** + * 最后一个定位点 + */ + public CloudLocationInfo lastCoordinate; + + /** + * 1s 内的连续定位点 + */ + public List< CloudLocationInfo > coordinates; +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/MogoRTKLocation.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/MogoRTKLocation.java new file mode 100644 index 0000000..68bcf03 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/location/MogoRTKLocation.java @@ -0,0 +1,179 @@ +package com.mogo.realtime.location; + +import android.content.BroadcastReceiver; +import android.content.Context; +import android.content.Intent; +import android.content.IntentFilter; +import android.location.Criteria; +import android.location.Location; +import android.location.LocationListener; +import android.location.LocationManager; +import android.os.Bundle; +import android.os.Handler; +import android.os.Message; + +import com.mogo.realtime.entity.CloudLocationInfo; + +import java.util.ArrayList; +import java.util.List; +import java.util.logging.Logger; + +public class MogoRTKLocation { + + private static final String TAG = "MogoRTKLocation"; + private static final int MSG_DATA_CHANGED = 0x100; + private static final long MSG_DATA_INTERNAL = 500L; + + private Handler mHandler; + private LocationManager locationManager; + private RTKLocationListener rtkLocationListener; + private List cacheList = new ArrayList<>(); + + public static MogoRTKLocation getInstance() { + return RTKHolder.rtkLoc; + } + + private static class RTKHolder { + private static final MogoRTKLocation rtkLoc = new MogoRTKLocation(); + } + + private MogoRTKLocation() { + mHandler = new Handler(WorkThreadHandler.newInstance( TAG ).getLooper() ) { + @Override + public void handleMessage(Message msg) { + super.handleMessage(msg); + if (msg.what == MSG_DATA_CHANGED) { + mHandler.sendEmptyMessageDelayed(MSG_DATA_CHANGED, uploadDelay); + sendLocationData(); + +// Logger.d(TAG,"handleMessage开始发送消息"); + } + } + }; + mHandler.sendEmptyMessage(MSG_DATA_CHANGED); + Logger.d(TAG,"构造方法开始发送消息"); + } + + public interface RTKLocationListener { + void onLocationChanged(List cloudLocationInfos); + } + + private void sendLocationData() { + + if (rtkLocationListener != null) { + List list = new ArrayList<>(cacheList); + rtkLocationListener.onLocationChanged(list); + } + if (cacheList != null && cacheList.size() > 0) { + cacheList.clear(); + } + } + + public void registerRTKLocationListener(RTKLocationListener locationListener) { + rtkLocationListener = locationListener; + } + + public void unregisterRTKLocationListener(){ + rtkLocationListener = null; + } + + public void init() { + locationManager = (LocationManager) AbsMogoApplication.getApp().getApplicationContext().getSystemService(Context.LOCATION_SERVICE); + String provider = locationManager.getBestProvider(getCriteria(), true); + Logger.d(TAG, "init provider : " + provider); + if (locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER)) { + try { + locationManager.requestLocationUpdates(provider, 0, 0, locationListener); + Location location = locationManager.getLastKnownLocation(provider); + if (location != null) { + Logger.i(TAG, "location : " + location.toString()); + } + } catch (Exception e) { + e.printStackTrace(); + Logger.d(TAG, "RTK LocationManager requestLocationUpdates has Exception : " + e.getMessage()); + } + } else { + Logger.d(TAG, "RTK LocationManager Provider GPS_PROVIDER unable"); + } + + // 注册修改上报间隔的广播, 临时使用，后面可直接干掉，发送广播的地方在EntranceFragment + IntentFilter filter = new IntentFilter("com.mogo.launcher.action.FIX_UPLOAT_DELAY"); + AbsMogoApplication.getApp().registerReceiver(fixUploadDelayReceiver, filter); + } + + private Criteria getCriteria() { + Criteria criteria = new Criteria(); + criteria.setAccuracy(Criteria.ACCURACY_FINE); //高精 + criteria.setAltitudeRequired(false); + criteria.setBearingRequired(true); + criteria.setSpeedRequired(true); + criteria.setPowerRequirement(Criteria.POWER_LOW); + return criteria; + } + + private LocationListener locationListener = new LocationListener() { + @Override + public void onLocationChanged(Location location) { + if (location != null) { + CloudLocationInfo cloudLocationInfo = new CloudLocationInfo(); + cloudLocationInfo.setAlt(location.getAltitude()); + cloudLocationInfo.setHeading(location.getBearing()); + cloudLocationInfo.setLat(location.getLatitude()); + cloudLocationInfo.setLon(location.getLongitude()); + cloudLocationInfo.setSpeed(location.getSpeed()); + cloudLocationInfo.setSatelliteTime(location.getTime()); + cloudLocationInfo.setSystemTime(System.currentTimeMillis()); + cloudLocationInfo.convertCoor2GCJ02(); + cacheList.add(cloudLocationInfo); + } else { + Logger.e(TAG, "location == null"); + } + } + + @Override + public void onStatusChanged(String provider, int status, Bundle extras) { + Logger.d(TAG, "onStatusChanged status: " + status); + } + + @Override + public void onProviderEnabled(String provider) { + Logger.d(TAG, "onProviderEnabled"); + } + + @Override + public void onProviderDisabled(String provider) { + Logger.d(TAG, "onProviderEnabled"); + } + }; + + public void stop() { + Logger.d(TAG, "stop RTK Location"); + if (locationManager != null && locationListener != null) { + locationManager.removeUpdates(locationListener); + } else { + Logger.d(TAG, "stop failed , reason : loc" + locationManager + " , or loc listener: " + locationListener + " is null"); + } + } + + private long uploadDelay = MSG_DATA_INTERNAL; + + private FixUploadDelayReceiver fixUploadDelayReceiver = new FixUploadDelayReceiver(); + + private class FixUploadDelayReceiver extends BroadcastReceiver{ + @Override + public void onReceive(Context context, Intent intent) { + uploadDelay = intent.getIntExtra("fixTime", 0); + } + } + + /** + * 默认保持{@link #uploadDelay}间隔进行位置上报，如遇服务端控制，进行上报间隔修改 + * @param delay 上报间隔 + */ + public void resetUploadDelay(long delay) { + if (mHandler != null && mHandler.hasMessages(MSG_DATA_CHANGED)) { + mHandler.removeMessages(MSG_DATA_CHANGED); + mHandler.sendEmptyMessageDelayed(MSG_DATA_CHANGED, delay); + } + } +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/MortonCode.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/MortonCode.java new file mode 100644 index 0000000..90257fb --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/MortonCode.java @@ -0,0 +1,149 @@ +package com.mogo.realtime.util; + +import java.text.DecimalFormat; + +/** + * 莫顿编码 + * + * @author linyang + * @since 2020.07.09 + */ +public class MortonCode { + + /** + * morton 转 经纬度 时的中间常量 + */ + private static final long NDS_180_DEGREES = 0x7fffffff; + + /** + * morton 转 经纬度 时的中间常量 + */ + private static final long NDS_360_DEGREES = 4294967295L; + + /** + * morton 转 经纬度 时的中间常量 + */ + private static final long NDS_90_DEGREES = 0x3fffffff; + + /** + * 经纬度转 morton 时的中间常量 + */ + private static final double RULE_MORTON = Math.pow( 2, 32 ) / 360; + + /** + * morton 转 经纬度 时的中间常量 + */ + private static final double RULE_MORTON_TO_LONLAT = 360.0 / Math.pow( 2, 32 ); + + /** + * @param lon + * @param lat + * @return + */ + public static long wrapEncodeMorton( Double lon, Double lat ) { + DecimalFormat decimalFormat = new DecimalFormat( "#.######" ); + return encodeMorton( Double.valueOf( decimalFormat.format( lon ) ), + Double.valueOf( decimalFormat.format( lat ) ) ); + } + + /** + * 编码 morton code + * + * @param lon + * @param lat + * @return + */ + public static long encodeMorton( Double lon, Double lat ) { + + Long bit = 1L; + long mortonCode = 0L; + long x = ( long ) ( lon * RULE_MORTON ); + long y = ( long ) ( lat * RULE_MORTON ); + + if ( y < 0 ) { + y += 0x7FFFFFFF; + } + y = y << 1; + for ( int i = 0; i < 32; i++ ) { + // x-part + mortonCode = mortonCode | ( x & bit ); + x = x << 1; + bit = bit << 1; + // y-part + mortonCode = mortonCode | ( y & bit ); + y = y << 1; + bit = bit << 1; + } + + return mortonCode; + } + + /** + * 将莫顿码解码为坐标 + * + * @param mortonCode + * @return + */ + public static double[] decodeMorton( long mortonCode ) { + long[] midPoint = mortonCodeToCoord( mortonCode ); + normalizeCoord( midPoint ); + double[] point = new double[2]; + + // 将经纬度长整数转化为 浮点类型 + point[0] = midPoint[0] * RULE_MORTON_TO_LONLAT; + point[1] = midPoint[1] * RULE_MORTON_TO_LONLAT; + return point; + } + + /** + * 莫顿码分别拆解为 编码后的经纬度长整数 + * + * @param mortonCode + * @return + */ + private static long[] mortonCodeToCoord( long mortonCode ) { + long bit = 1L; + long[] longPoint = new long[2]; + + for ( int i = 0; i < 32; i++ ) { + longPoint[0] |= mortonCode & bit; + mortonCode >>= 1; + longPoint[1] |= mortonCode & bit; + bit <<= 1; + } + return longPoint; + } + + /** + * 对编码后的经纬度长整数进行解码 + * + * @param midPoint + */ + private static void normalizeCoord( long[] midPoint ) { + // if x > 180 degrees, then subtract 360 degrees + if ( midPoint[0] > NDS_180_DEGREES ) { + midPoint[0] -= + NDS_360_DEGREES + 1; // add 1 because 0 must be counted as well + } else if ( midPoint[0] < -NDS_180_DEGREES ) // if x < 180 , x += 360 + { + midPoint[0] += + NDS_360_DEGREES + 1; // add 1 because 0 must be counted as well + } + + // if y > 90 degrees, then subtract 180 degrees + if ( midPoint[1] > NDS_90_DEGREES ) { + midPoint[1] -= + NDS_180_DEGREES + 1; // add 1 because 0 must be counted as well + } else if ( midPoint[1] < -NDS_90_DEGREES ) // if y < 90, y += 180 + { + midPoint[1] += + NDS_180_DEGREES + 1; // add 1 because 0 must be counted as well + } + return; + } + + + public static void main( String[] args ) { + System.out.println( encodeMorton( 116.39584, 39.98152 ) ); + } +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/SimpleLocationCorrectStrategy.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/SimpleLocationCorrectStrategy.java new file mode 100644 index 0000000..d1d3795 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/util/SimpleLocationCorrectStrategy.java @@ -0,0 +1,257 @@ +package com.mogo.realtime.util; + +import android.os.SystemClock; + +import com.mogo.map.MogoLatLng; +import com.mogo.module.common.MogoApisHandler; +import com.mogo.module.common.entity.CloudLocationInfo; +import com.mogo.realtime.entity.CloudLocationInfo; +import com.mogo.utils.logger.Logger; + +import java.util.ArrayList; +import java.util.List; + +/** + * 定位预测纠错策略 + * + * @author tongchenfei + */ +public class SimpleLocationCorrectStrategy { + private static final String TAG = "SimpleLocationCorrectStrategy"; + private static final int ERR_COUNT_THRESHOLD = 3; + /** + * 目标距离误差是10米，就是在原目标距离基础上增加10米 + */ + private static final float TARGET_DISTANCE_DEVIATION = 10; + + private CloudLocationInfo lastLocation = null; + private long anchorTime; + private int errCount; + + private static SimpleLocationCorrectStrategy instance = new SimpleLocationCorrectStrategy(); + + public static SimpleLocationCorrectStrategy getInstance(){ + return instance; + } + + private List historyList = new ArrayList<>(); + private List validList = new ArrayList<>(); + private List correctList = new ArrayList<>(); + private List errList = new ArrayList<>(); + + public CloudLocationInfo correct(CloudLocationInfo info) { + Logger.d(TAG, "info: " + info.print()); + if(isLocationValid(info)) { + if(recordLocation()) { + historyList.add(info); + } + + if (lastLocation == null) { + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + Logger.d(TAG, "第一条数据"); + if(recordLocation()) { + validList.add(lastLocation); + } + return info; + } + if (lastLocation.equals(info)) { + Logger.d(TAG, "相同坐标点=="); + return info; + } + try { + float targetDistance = + (float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION; + float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info)); + Logger.d(TAG, + "准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}"); + if (distance <= targetDistance) { + // 新的定位点在目标距离范围内，认为此数据有效 + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + errCount = 0; + Logger.d(TAG, "在范围内，为有效点"); + if(recordLocation()) { + validList.add(lastLocation); + } + return info; + } else { + // 出现异常点 + if (errCount >= ERR_COUNT_THRESHOLD) { + // 出错次数超过阈值，认为本次出错点为正确点 + if(recordLocation()) { + errList.add(new CloudLocationInfo(lastLocation)); + correctList.add(info); + } + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + errCount = 0; + Logger.d(TAG, "出错次数超限，异常点变有效点"); + return info; + } else { + // 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同 + CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); + MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(), + lastLocation.getLat(), lastLocation.getHeading(), targetDistance); + nextInfo.setLon(nextLatLon.lon); + nextInfo.setLat(nextLatLon.lat); + if(recordLocation()) { + errList.add(info); + correctList.add(nextInfo); + } + lastLocation = nextInfo; + anchorTime = SystemClock.elapsedRealtime(); + errCount++; + Logger.d(TAG, "异常点纠偏 info: " + lastLocation); +// return lastLocation; + if(recordLocation()) { + correctList.add(nextInfo); + } + return nextInfo; + } + } + } catch (Exception e) { + Logger.e(TAG, e, "纠偏异常"); + e.printStackTrace(); + } + }else{ + Logger.d(TAG, "定位点异常"); + if (lastLocation == null) { + return null; + }else{ + try { + float targetDistance = + (float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION; + float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info)); + Logger.d(TAG, + "异常定位点\n准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}"); + // 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同 + CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); + MogoLatLng nextLatLon = computerThatLonLat(lastLocation.getLon(), + lastLocation.getLat(), lastLocation.getHeading(), targetDistance); + nextInfo.setLon(nextLatLon.lon); + nextInfo.setLat(nextLatLon.lat); + if(recordLocation()) { + errList.add(info); + correctList.add(nextInfo); + } + lastLocation = nextInfo; + anchorTime = SystemClock.elapsedRealtime(); + errCount++; + Logger.d(TAG, "异常点纠偏 info: " + lastLocation); + if(recordLocation()) { + correctList.add(nextInfo); + } +// return lastLocation; + return nextInfo; + }catch (Exception e){ + Logger.e(TAG, e, "纠偏异常"); + e.printStackTrace(); + } + } + } + return null; + } + + private boolean isLocationValid(CloudLocationInfo info) { + return info.getLat() != 0 && info.getLon() != 0; + } + + private RecordLocationListener recordLocationListener = null; + private boolean hasCallbackReocrd = false; + + public void setRecordLocationListener(RecordLocationListener recordLocationListener) { + this.recordLocationListener = recordLocationListener; + } + + private boolean recordLocation(){ + if (historyList.size() >= 100 && !hasCallbackReocrd && recordLocationListener != null) { + hasCallbackReocrd = true; + recordLocationListener.onRecordFinish(historyList, correctList,validList,correctList); + } + return historyList.size() < 100; + } + + + /** + * 根据距离和角度计算下一个经纬度 + * 大地坐标系资料WGS-84 长半径a=6378137 短半径b=6356752.3142 扁率f=1/298.2572236 + */ + public MogoLatLng computerThatLonLat(double lon, double lat, double brng, double dist) { + + double alpha1 = rad(brng); + double sinAlpha1 = Math.sin(alpha1); + double cosAlpha1 = Math.cos(alpha1); + + // 扁率f=1/298.2572236 + double f = 1 / 298.2572236; + double tanU1 = (1 - f) * Math.tan(rad(lat)); + double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)); + double sinU1 = tanU1 * cosU1; + double sigma1 = Math.atan2(tanU1, cosAlpha1); + double sinAlpha = cosU1 * sinAlpha1; + double cosSqAlpha = 1 - sinAlpha * sinAlpha; + // 长半径a=6378137 + double a = 6378137; + // 短半径b=6356752.3142 + double b = 6356752.3142; + double uSq = cosSqAlpha * (a * a - b * b) / (b * b); + double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))); + double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); + + double cos2SigmaM=0; + double sinSigma=0; + double cosSigma=0; + double sigma = dist / (b * A), sigmaP = 2 * Math.PI; + while (Math.abs(sigma - sigmaP) > 1e-12) { + cos2SigmaM = Math.cos(2 * sigma1 + sigma); + sinSigma = Math.sin(sigma); + cosSigma = Math.cos(sigma); + double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) + - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM))); + sigmaP = sigma; + sigma = dist / (b * A) + deltaSigma; + } + + double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1; + double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1, + (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp)); + double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1); + double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha)); + double L = lambda - (1 - C) * f * sinAlpha + * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM))); + + // final bearing + double revAz = Math.atan2(sinAlpha, -tmp); + + System.out.println(revAz); + System.out.println(lon+deg(L)+","+deg(lat2)); + return new MogoLatLng(deg(lat2), lon + deg(L)); + } + + /** + * 度换成弧度 + * + * @param d + * 度 + * @return 弧度 + */ + private double rad(double d) { + return d * Math.PI / 180.0; + } + + /** + * 弧度换成度 + * + * @param x + * 弧度 + * @return 度 + */ + private double deg(double x) { + return x * 180 / Math.PI; + } + + public interface RecordLocationListener{ + void onRecordFinish(List history, List correct, List valid, List err); + } +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/LocationResult.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/LocationResult.java new file mode 100644 index 0000000..25ed4f2 --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/LocationResult.java @@ -0,0 +1,36 @@ +package com.mogo.realtime.websocket; + + +import com.mogo.realtime.entity.CloudLocationInfo; + +import java.util.List; + +public +/** + * @author congtaowang + * @since 2020/10/25 + * + * 自车定位信息 + */ +class LocationResult { + + /** + * sn + */ + public String sn; + + /** + * 最后一个定位点的莫顿码 + */ + public long mortonCode; + + /** + * 最后一个定位点 + */ + public CloudLocationInfo lastCoordinate; + + /** + * 1s 内的连续定位点 + */ + public List< CloudLocationInfo > coordinates; +} diff --git a/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/OnePerSecondSendContent.java b/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/OnePerSecondSendContent.java new file mode 100644 index 0000000..5ec0a3a --- /dev/null +++ b/modules/mogo-realtime/src/main/java/com/mogo/realtime/websocket/OnePerSecondSendContent.java @@ -0,0 +1,27 @@ +package com.mogo.realtime.websocket; + + +import com.mogo.realtime.entity.ADASRecognizedResult; +import com.mogo.realtime.location.LocationResult; + +import java.util.List; + +public +/** + * @author congtaowang + * @since 2020/10/25 + * + * 一秒一次的上行数据 + */ +class OnePerSecondSendContent { + + /** + * 自车定位点 + */ + public LocationResult self; + + /** + * adas 识别物体：1s 识别到的最后帧 + */ + public List adas; +}