|
@@ -1,29 +1,189 @@
|
|
|
package com.fdkk.sxz;
|
|
|
|
|
|
-import cn.hutool.core.date.TimeInterval;
|
|
|
-import cn.hutool.core.lang.Console;
|
|
|
-import cn.hutool.core.thread.ThreadUtil;
|
|
|
+import com.alibaba.fastjson.JSON;
|
|
|
+import com.alibaba.fastjson.JSONArray;
|
|
|
+import com.alibaba.fastjson.JSONObject;
|
|
|
+
|
|
|
+import java.util.ArrayList;
|
|
|
+import java.util.List;
|
|
|
+import java.util.Map;
|
|
|
+import java.util.stream.Collectors;
|
|
|
|
|
|
public class main {
|
|
|
public static void main(String[] args) {
|
|
|
- final TimeInterval timer = new TimeInterval();
|
|
|
-
|
|
|
-// 分组1
|
|
|
- timer.start();
|
|
|
- ThreadUtil.sleep(800);
|
|
|
- Console.log("Timer 1 took {} ms", timer.intervalMs());
|
|
|
-
|
|
|
- timer.intervalRestart();
|
|
|
- ThreadUtil.sleep(1500);
|
|
|
-
|
|
|
- Console.log("Timer 2 took {} Second", timer.intervalMs());
|
|
|
-
|
|
|
- Console.log("Timer 2 took {} intervalRestart", timer.intervalRestart());
|
|
|
-
|
|
|
- ThreadUtil.sleep(3500);
|
|
|
- timer.restart();
|
|
|
- ThreadUtil.sleep(1200);
|
|
|
- Console.log("Timer 3 took {} Second", timer.intervalMs());
|
|
|
+ String json = "{\n" +
|
|
|
+ " \"sweepLocations\": [\n" +
|
|
|
+ " {\n" +
|
|
|
+ " \"subgroup\": 0,\n" +
|
|
|
+ " \"pose\": {\n" +
|
|
|
+ " \"rotation\": {\n" +
|
|
|
+ " \"w\": 0.95928264,\n" +
|
|
|
+ " \"x\": -0.0017092235,\n" +
|
|
|
+ " \"y\": -0.0016763769,\n" +
|
|
|
+ " \"z\": -0.28243768\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"translation\": {\n" +
|
|
|
+ " \"x\": 3.6112025,\n" +
|
|
|
+ " \"y\": 0.12166519,\n" +
|
|
|
+ " \"z\": 0.07516695\n" +
|
|
|
+ " }\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"visibles3\": [\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"visibles2\": [\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"puck\": {\n" +
|
|
|
+ " \"x\": 3.6112025,\n" +
|
|
|
+ " \"y\": 0.12166519,\n" +
|
|
|
+ " \"z\": -1.39718955\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"uuid\": \"0\",\n" +
|
|
|
+ " \"visibles\": [\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"group\": 1\n" +
|
|
|
+ " },\n" +
|
|
|
+ " {\n" +
|
|
|
+ " \"subgroup\": 0,\n" +
|
|
|
+ " \"pose\": {\n" +
|
|
|
+ " \"rotation\": {\n" +
|
|
|
+ " \"w\": 0.97998166,\n" +
|
|
|
+ " \"x\": -0.00051350286,\n" +
|
|
|
+ " \"y\": -0.00007887775,\n" +
|
|
|
+ " \"z\": -0.19908713\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"translation\": {\n" +
|
|
|
+ " \"x\": 1.6813623,\n" +
|
|
|
+ " \"y\": 0.20879818,\n" +
|
|
|
+ " \"z\": 0.07516695\n" +
|
|
|
+ " }\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"visibles3\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"visibles2\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"puck\": {\n" +
|
|
|
+ " \"x\": 1.6813623,\n" +
|
|
|
+ " \"y\": 0.20879818,\n" +
|
|
|
+ " \"z\": -1.39718955\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"uuid\": \"2\",\n" +
|
|
|
+ " \"visibles\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 2,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"group\": 1\n" +
|
|
|
+ " },\n" +
|
|
|
+ " {\n" +
|
|
|
+ " \"subgroup\": 0,\n" +
|
|
|
+ " \"pose\": {\n" +
|
|
|
+ " \"rotation\": {\n" +
|
|
|
+ " \"w\": 0.7688388,\n" +
|
|
|
+ " \"x\": -0.0022818057,\n" +
|
|
|
+ " \"y\": 0.0016657045,\n" +
|
|
|
+ " \"z\": -0.6394364\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"translation\": {\n" +
|
|
|
+ " \"x\": -1.0941758,\n" +
|
|
|
+ " \"y\": -0.15828772,\n" +
|
|
|
+ " \"z\": 0.07516695\n" +
|
|
|
+ " }\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"visibles3\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"visibles2\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"puck\": {\n" +
|
|
|
+ " \"x\": -1.0941758,\n" +
|
|
|
+ " \"y\": -0.15828772,\n" +
|
|
|
+ " \"z\": -1.39718955\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"uuid\": \"4\",\n" +
|
|
|
+ " \"visibles\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 3\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"group\": 1\n" +
|
|
|
+ " },\n" +
|
|
|
+ " {\n" +
|
|
|
+ " \"subgroup\": 1,\n" +
|
|
|
+ " \"pose\": {\n" +
|
|
|
+ " \"rotation\": {\n" +
|
|
|
+ " \"w\": 0.75378096,\n" +
|
|
|
+ " \"x\": -0.00085483055,\n" +
|
|
|
+ " \"y\": 0.0011985971,\n" +
|
|
|
+ " \"z\": -0.6571241\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"translation\": {\n" +
|
|
|
+ " \"x\": -4.213454,\n" +
|
|
|
+ " \"y\": -0.17369196,\n" +
|
|
|
+ " \"z\": 0.07516695\n" +
|
|
|
+ " }\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"visibles3\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"visibles2\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"puck\": {\n" +
|
|
|
+ " \"x\": -4.213454,\n" +
|
|
|
+ " \"y\": -0.17369196,\n" +
|
|
|
+ " \"z\": -1.39718955\n" +
|
|
|
+ " },\n" +
|
|
|
+ " \"uuid\": \"6\",\n" +
|
|
|
+ " \"visibles\": [\n" +
|
|
|
+ " 0,\n" +
|
|
|
+ " 1,\n" +
|
|
|
+ " 2\n" +
|
|
|
+ " ],\n" +
|
|
|
+ " \"group\": 1\n" +
|
|
|
+ " }\n" +
|
|
|
+ " ]\n" +
|
|
|
+ "}";
|
|
|
+ JSONObject visionJson = JSON.parseObject(json);
|
|
|
+ JSONArray sweepArray = visionJson.getJSONArray("sweepLocations");
|
|
|
+ List<JSONObject> roamingPoint = new ArrayList();
|
|
|
+ for (int i = 0; i < sweepArray.size(); i++) {
|
|
|
+ JSONObject objectJson = sweepArray.getJSONObject(i);
|
|
|
+ JSONObject newJson = new JSONObject();
|
|
|
+ newJson.put("uuid", objectJson.getString("uuid"));
|
|
|
+ JSONObject puck = objectJson.getJSONObject("puck");
|
|
|
+ puck.put("y", puck.getDoubleValue("y") * -1);
|
|
|
+ puck.remove("z");
|
|
|
+ newJson.put("puck", puck);
|
|
|
+ newJson.put("subgroup", objectJson.getInteger("subgroup"));
|
|
|
+ roamingPoint.add(newJson);
|
|
|
+ }
|
|
|
+ Map<Integer, List<JSONObject>> subgroup = roamingPoint.stream().collect(
|
|
|
+ Collectors.groupingBy(x -> x.getInteger("subgroup")));
|
|
|
|
|
|
+ System.out.println(JSON.toJSONString(subgroup));
|
|
|
}
|
|
|
}
|