package com.xiaopeng.automaticPilot.util;
|
|
import cn.hutool.http.HttpRequest;
|
import cn.hutool.http.HttpResponse;
|
import com.google.protobuf.util.JsonFormat;
|
import com.xiaopeng.api.vo.automaticPilot.protobuf.*;
|
|
import java.util.List;
|
import java.util.UUID;
|
import java.util.concurrent.Executors;
|
import java.util.concurrent.ScheduledExecutorService;
|
import java.util.concurrent.TimeUnit;
|
import java.util.concurrent.atomic.AtomicInteger;
|
|
/**
|
* GPS路径模拟器
|
* 根据起点和终点生成路径，每秒发送一次包含GPS坐标的JSON消息
|
*/
|
public class GpsPathSimulator {
|
|
private static final JsonFormat.Printer jsonPrinter = JsonFormat.printer()
|
.printingEnumsAsInts()
|
.preservingProtoFieldNames();
|
|
/**
|
* 推送XPU消息的接口地址
|
*/
|
private static final String PUSH_MESSAGE_URL =
|
"http://logan-gateway.aliyun-hd1-pre.logan.xiaopeng.inner/xp-robotaxi-automatic-pilot-boot/base/v1/pushXPUMessage";
|
|
/**
|
* 运行模拟器
|
*
|
* @param startLongitude 起点经度
|
* @param startLatitude 起点纬度
|
* @param endLongitude 终点经度
|
* @param endLatitude 终点纬度
|
* @param speed 速度（米/秒），默认10.5
|
* @param uid 用户ID
|
* @param xpuId XPU设备ID
|
* @param orderId 订单ID
|
*/
|
public static void runSimulator(double startLongitude, double startLatitude,
|
double endLongitude, double endLatitude,
|
double speed, long uid, String xpuId, String orderId) {
|
// 生成路径点
|
List<GpsPathGenerator.GpsPoint> pathPoints = GpsPathGenerator.generatePath(
|
startLongitude, startLatitude,
|
endLongitude, endLatitude,
|
1, // 每秒一个点
|
speed
|
);
|
|
System.out.println("===== GPS路径模拟器启动 =====");
|
System.out.println("起点: 经度=" + startLongitude + ", 纬度=" + startLatitude);
|
System.out.println("终点: 经度=" + endLongitude + ", 纬度=" + endLatitude);
|
System.out.println("速度: " + speed + " 米/秒");
|
System.out.println("总路径点数: " + pathPoints.size());
|
System.out.println("预计时长: " + pathPoints.size() + " 秒");
|
System.out.println("=============================\n");
|
|
// 创建定时任务
|
ScheduledExecutorService scheduler = Executors.newScheduledThreadPool(1);
|
AtomicInteger currentIndex = new AtomicInteger(0);
|
AtomicInteger sequence = new AtomicInteger(1);
|
|
scheduler.scheduleAtFixedRate(() -> {
|
int index = currentIndex.getAndIncrement();
|
|
if (index >= pathPoints.size()) {
|
System.out.println("\n===== 路径模拟完成 =====");
|
scheduler.shutdown();
|
return;
|
}
|
|
GpsPathGenerator.GpsPoint currentPoint = pathPoints.get(index);
|
|
// 计算方位角
|
int course = 90; // 默认向东
|
if (index < pathPoints.size() - 1) {
|
GpsPathGenerator.GpsPoint nextPoint = pathPoints.get(index + 1);
|
course = GpsPathGenerator.calculateCourse(
|
currentPoint.latitude, currentPoint.longitude,
|
nextPoint.latitude, nextPoint.longitude
|
);
|
}
|
|
// 创建消息
|
MsgContent message = createMessage(
|
currentPoint.longitude,
|
currentPoint.latitude,
|
course,
|
speed,
|
uid,
|
sequence.getAndIncrement(),
|
xpuId,
|
orderId
|
);
|
|
try {
|
String json = jsonPrinter.print(message);
|
System.out.println("【第 " + (index + 1) + " 秒】发送消息:");
|
System.out.println(json);
|
System.out.println();
|
|
// 调用HTTP接口发送消息
|
sendMessageToServer(json, index + 1);
|
|
} catch (Exception e) {
|
System.err.println("JSON序列化失败: " + e.getMessage());
|
}
|
|
}, 0, 1, TimeUnit.SECONDS);
|
}
|
|
/**
|
* 创建系统运行状态消息（使用Protobuf对象）
|
*/
|
private static MsgContent createMessage(double longitude, double latitude,
|
int course, double speed,
|
long uid, int sequence, String xpuId, String orderId) {
|
long timestamp = System.currentTimeMillis();
|
|
// 构建 MsgContent
|
MsgContent.Builder msgContentBuilder = MsgContent.newBuilder();
|
|
// 构建 Header
|
MsgHeader.Builder headerBuilder = MsgHeader.newBuilder();
|
headerBuilder.setTimestamp(timestamp);
|
headerBuilder.setCmd(14001); // SYSTEM_STATUS_XPU
|
headerBuilder.setSequence(sequence);
|
headerBuilder.setType(MessageType.MESSAGE_TYPE_REQUEST);
|
headerBuilder.setMsgId("MSG-UUID-" + UUID.randomUUID().toString().substring(0, 13));
|
headerBuilder.setSource(SourceType.XPU);
|
msgContentBuilder.setHeader(headerBuilder.build());
|
|
// 构建 Wgs (GPS坐标)
|
Wgs.Builder wgsBuilder = Wgs.newBuilder();
|
wgsBuilder.setLongitude(longitude);
|
wgsBuilder.setLatitude(latitude);
|
wgsBuilder.setCourse(course);
|
wgsBuilder.setPosAcc(0.5);
|
wgsBuilder.setAlt(10);
|
|
// 构建 RealtimePose
|
RealtimePose.Builder poseBuilder = RealtimePose.newBuilder();
|
poseBuilder.setTimestamp(timestamp);
|
poseBuilder.setNaviPose(wgsBuilder.build());
|
|
// 构建 VehicleStaticInfo
|
VehicleStaticInfo.Builder staticInfoBuilder = VehicleStaticInfo.newBuilder();
|
staticInfoBuilder.setXpuId(xpuId);
|
staticInfoBuilder.setTimestamp(timestamp);
|
|
// 构建 VehicleDynamicStatus
|
VehicleDynamicStatus.Builder dynamicStatusBuilder = VehicleDynamicStatus.newBuilder();
|
dynamicStatusBuilder.setGearLevel(GearLevel.GEAR_D);
|
dynamicStatusBuilder.setSteeringAngle(0.5f);
|
dynamicStatusBuilder.setSpeed((float) speed);
|
dynamicStatusBuilder.setTimestamp(timestamp);
|
dynamicStatusBuilder.setBmsSocDisp(80);
|
dynamicStatusBuilder.setDstBatDisp(400.5f);
|
|
// 构建 VehicleStatus
|
VehicleStatus.Builder vehicleStatusBuilder = VehicleStatus.newBuilder();
|
vehicleStatusBuilder.setStaticInfo(staticInfoBuilder.build());
|
vehicleStatusBuilder.setDynamicStatus(dynamicStatusBuilder.build());
|
|
// 构建 SystemStatus
|
SystemStatus.Builder systemStatusBuilder = SystemStatus.newBuilder();
|
systemStatusBuilder.setPhase(SystemPhase.SYSTEM_NORMAL);
|
systemStatusBuilder.setAbnormalReason(AbnormalReason.ABNORMAL_REASON_UNSPECIFIED);
|
systemStatusBuilder.addAvailableServices(ServiceAvailability.ROBOTAXI_AVAILABLE);
|
systemStatusBuilder.setIsEmergency(false);
|
|
// 构建 OrderStatusInfo (运行中的订单状态信息)
|
OrderStatusInfo.Builder orderStatusInfoBuilder = OrderStatusInfo.newBuilder();
|
orderStatusInfoBuilder.setOrderId(orderId);
|
orderStatusInfoBuilder.setStatus(OrderStatus.ORDER_EN_ROUTE_TO_PICKUP); // 前往目的地
|
orderStatusInfoBuilder.setOrderPhase(OrderPhase.ROUTE_TO_PICKUP);
|
orderStatusInfoBuilder.setChangeSource(OrderStateChangeSource.ORDER_STATE_CHANGE_SOURCE_VEHICLE);
|
orderStatusInfoBuilder.setTimestamp(timestamp);
|
|
// 构建 SystemRuntimeStatus
|
SystemRuntimeStatus.Builder statusBuilder = SystemRuntimeStatus.newBuilder();
|
statusBuilder.setTimestamp(timestamp);
|
statusBuilder.setUid(uid);
|
statusBuilder.setVehicleStatus(vehicleStatusBuilder.build());
|
statusBuilder.setSystemStatus(systemStatusBuilder.build());
|
statusBuilder.setDrivingMode(DrivingMode.AUTONOMOUS);
|
statusBuilder.setPose(poseBuilder.build());
|
statusBuilder.setRunningOrderStatusInfo(orderStatusInfoBuilder.build()); // 设置运行中的订单信息
|
|
// 设置 SystemRuntimeStatus 到 MsgContent
|
msgContentBuilder.setSystemRuntimeStatus(statusBuilder.build());
|
|
return msgContentBuilder.build();
|
}
|
|
/**
|
* 发送消息到服务器
|
*
|
* @param jsonMessage JSON格式的消息
|
* @param index 消息序号
|
*/
|
private static void sendMessageToServer(String jsonMessage, int index) {
|
try {
|
HttpResponse response = HttpRequest.post(PUSH_MESSAGE_URL)
|
.body(jsonMessage)
|
.contentType("application/json")
|
.timeout(5000) // 5秒超时
|
.execute();
|
|
if (response.isOk()) {
|
System.out.println("✓ 消息发送成功，响应: " + response.body());
|
} else {
|
System.err.println("✗ 消息发送失败，状态码: " + response.getStatus() + ", 响应: " + response.body());
|
}
|
} catch (Exception e) {
|
System.err.println("✗ 消息发送异常: " + e.getMessage());
|
}
|
}
|
|
/**
|
* 主方法 - 示例用法
|
*/
|
public static void main(String[] args) {
|
// 示例：从广州天河区到珠江新城
|
double startLongitude = 113.382238; // 起点经度
|
double startLatitude = 23.156917; // 起点纬度
|
double endLongitude = 113.383838; // 终点经度
|
double endLatitude = 23.154817; // 终点纬度
|
double speed = 10.5; // 速度（米/秒）
|
long uid = 8190544; // 用户ID
|
String xpuId = "c47b405bee79000f282a36e800000000"; // XPU ID
|
String orderId = "2031203246241185794"; // 订单ID
|
|
// 如果有命令行参数，则使用命令行参数
|
if (args.length >= 7) {
|
try {
|
startLongitude = Double.parseDouble(args[0]);
|
startLatitude = Double.parseDouble(args[1]);
|
endLongitude = Double.parseDouble(args[2]);
|
endLatitude = Double.parseDouble(args[3]);
|
speed = Double.parseDouble(args[4]);
|
uid = Long.parseLong(args[5]);
|
xpuId = args[6];
|
if (args.length >= 8) {
|
orderId = args[7];
|
}
|
} catch (NumberFormatException e) {
|
System.err.println("参数格式错误！");
|
printUsage();
|
return;
|
}
|
}
|
|
runSimulator(startLongitude, startLatitude, endLongitude, endLatitude, speed, uid, xpuId, orderId);
|
}
|
|
private static void printUsage() {
|
System.out.println("使用方法:");
|
System.out.println("java GpsPathSimulator <起点经度> <起点纬度> <终点经度> <终点纬度> <速度(米/秒)> <用户ID> <XPU_ID> [订单ID]");
|
System.out.println();
|
System.out.println("示例:");
|
System.out.println("java GpsPathSimulator 113.3245 23.1235 113.3350 23.1300 10.5 12345678 c47b405bee79000f282a36e800000000 XU100001");
|
}
|
}
|
