增加了异形有障碍物路径算法

张世豪

昨天 0803b041d32a284ee8585914618219ecae82b21f

增加了异形有障碍物路径算法

 Obstacledge.properties

@@ -1,5 +1,5 @@
# 割草机地块障碍物配置文件
# 生成时间：2025-12-25T19:02:14.286913600
# 生成时间：2025-12-26T15:20:50.727323200
# 坐标系：WGS84（度分格式）

# ============ 地块基准站配置 ============

 dikuai.properties

@@ -1,8 +1,8 @@
#Dikuai Properties
#Thu Dec 25 19:02:14 CST 2025
#Fri Dec 26 15:20:50 CST 2025
LAND1.angleThreshold=-1
LAND1.baseStationCoordinates=3949.89151752,N,11616.79267501,E
LAND1.boundaryCoordinates=121.86,-611.32;130.67,-577.12;173.17,-587.48;167.47,-621.17
LAND1.boundaryCoordinates=4.30,87.65;-2.36,-65.51;44.25,-66.72;49.70,-14.05;98.13,-15.87;99.34,-69.75;137.48,-67.93;134.45,90.07;4.30,87.65
LAND1.boundaryOriginalCoordinates=39.831522,116.279873,49.25;39.831524,116.279878,49.25;39.831525,116.279878,49.24;39.831524,116.279912,49.30;39.831524,116.279911,49.29;39.831523,116.279911,49.23;39.831521,116.279915,49.31;39.831517,116.279925,49.34;39.831514,116.279940,49.30;39.831514,116.279957,49.28;39.831516,116.279974,49.28;39.831518,116.279991,49.29;39.831521,116.280008,49.24;39.831524,116.280025,49.30;39.831526,116.280042,49.24;39.831529,116.280059,49.29;39.831529,116.280076,49.26;39.831530,116.280093,49.32;39.831531,116.280110,49.28;39.831533,116.280127,49.28;39.831535,116.280144,49.26;39.831539,116.280161,49.27;39.831544,116.280175,49.25;39.831551,116.280190,49.24;39.831558,116.280204,49.26;39.831566,116.280219,49.26;39.831574,116.280234,49.22;39.831583,116.280248,49.24;39.831591,116.280260,49.24;39.831600,116.280272,49.23;39.831608,116.280285,49.18;39.831615,116.280298,49.12;39.831618,116.280312,49.11;39.831618,116.280328,49.12;39.831615,116.280342,49.15;39.831610,116.280356,49.21;39.831602,116.280369,49.23;39.831592,116.280379,49.25;39.831581,116.280388,49.25;39.831569,116.280394,49.19;39.831559,116.280395,49.23;39.831552,116.280387,49.28;39.831547,116.280373,49.32;39.831544,116.280357,49.33;39.831541,116.280340,49.29;39.831539,116.280324,49.27;39.831536,116.280307,49.24;39.831534,116.280290,49.25;39.831531,116.280273,49.26;39.831527,116.280257,49.28;39.831522,116.280242,49.21;39.831514,116.280232,49.28;39.831504,116.280229,49.24;39.831491,116.280230,49.33;39.831478,116.280233,49.34;39.831466,116.280236,49.31;39.831454,116.280239,49.31;39.831441,116.280242,49.26;39.831429,116.280244,49.23;39.831416,116.280247,49.25;39.831402,116.280250,49.22;39.831389,116.280253,49.25;39.831376,116.280256,49.26;39.831364,116.280258,49.24;39.831351,116.280261,49.25;39.831338,116.280265,49.26;39.831324,116.280268,49.20;39.831311,116.280271,49.16;39.831298,116.280274,49.17;39.831285,116.280277,49.22;39.831271,116.280278,49.16;39.831261,116.280273,49.23
LAND1.boundaryOriginalXY=-1
LAND1.boundaryPointInterval=-1
@@ -18,9 +18,9 @@
LAND1.mowingTrack=-1
LAND1.mowingWidth=50
LAND1.obstacleCoordinates=-1
LAND1.plannedPath=122.510775,-610.918324;167.039920,-620.534901;172.565118,-587.878071;131.052742,-577.758818;122.510775,-610.918324;122.635602,-610.433754;167.123414,-620.041405;167.206909,-619.547910;122.760428,-609.949185;122.885254,-609.464616;167.290403,-619.054415;167.373897,-618.560919;123.010080,-608.980047;123.134906,-608.495477;167.457392,-618.067424;167.540886,-617.573928;123.259733,-608.010908;123.384559,-607.526339;167.624380,-617.080433;167.707875,-616.586937;123.509385,-607.041769;123.634211,-606.557200;167.791369,-616.093442;167.874863,-615.599947;123.759037,-606.072631;123.883864,-605.588061;167.958358,-615.106451;168.041852,-614.612956;124.008690,-605.103492;124.133516,-604.618923;168.125346,-614.119460;168.208841,-613.625965;124.258342,-604.134353;124.383168,-603.649784;168.292335,-613.132470;168.375829,-612.638974;124.507994,-603.165215;124.632821,-602.680645;168.459324,-612.145479;168.542818,-611.651983;124.757647,-602.196076;124.882473,-601.711507;168.626312,-611.158488;168.709807,-610.664993;125.007299,-601.226937;125.132125,-600.742368;168.793301,-610.171497;168.876795,-609.678002;125.256952,-600.257799;125.381778,-599.773229;168.960290,-609.184506;169.043784,-608.691011;125.506604,-599.288660;125.631430,-598.804091;169.127278,-608.197516;169.210773,-607.704020;125.756256,-598.319521;125.881083,-597.834952;169.294267,-607.210525;169.377761,-606.717029;126.005909,-597.350383;126.130735,-596.865813;169.461256,-606.223534;169.544750,-605.730038;126.255561,-596.381244;126.380387,-595.896675;169.628244,-605.236543;169.711739,-604.743048;126.505214,-595.412106;126.630040,-594.927536;169.795233,-604.249552;169.878727,-603.756057;126.754866,-594.442967;126.879692,-593.958398;169.962221,-603.262561;170.045716,-602.769066;127.004518,-593.473828;127.129344,-592.989259;170.129210,-602.275571;170.212704,-601.782075;127.254171,-592.504690;127.378997,-592.020120;170.296199,-601.288580;170.379693,-600.795084;127.503823,-591.535551;127.628649,-591.050982;170.463187,-600.301589;170.546682,-599.808094;127.753475,-590.566412;127.878302,-590.081843;170.630176,-599.314598;170.713670,-598.821103;128.003128,-589.597274;128.127954,-589.112704;170.797165,-598.327607;170.880659,-597.834112;128.252780,-588.628135;128.377606,-588.143566;170.964153,-597.340617;171.047648,-596.847121;128.502433,-587.658996;128.627259,-587.174427;171.131142,-596.353626;171.214636,-595.860130;128.752085,-586.689858;128.876911,-586.205288;171.298131,-595.366635;171.381625,-594.873139;129.001737,-585.720719;129.126564,-585.236150;171.465119,-594.379644;171.548614,-593.886149;129.251390,-584.751580;129.376216,-584.267011;171.632108,-593.392653;171.715602,-592.899158;129.501042,-583.782442;129.625868,-583.297872;171.799097,-592.405662;171.882591,-591.912167;129.750694,-582.813303;129.875521,-582.328734;171.966085,-591.418672;172.049580,-590.925176;130.000347,-581.844165;130.125173,-581.359595;172.133074,-590.431681;172.216568,-589.938185;130.249999,-580.875026;130.374825,-580.390457;172.300063,-589.444690;172.383557,-588.951195;130.499652,-579.905887;130.624478,-579.421318;172.467051,-588.457699;172.550546,-587.964204;130.749304,-578.936749;130.874130,-578.452179;157.378188,-584.176033
LAND1.plannedPath=136.940221,-67.425155;133.930254,89.530244;4.807861,87.129352;-1.807069,-64.994176;43.773311,-66.177447;49.223902,-13.501734;98.648661,-15.359117;99.857663,-69.194695;136.940221,-67.425155;136.690404,-67.437076;133.680297,89.525597;133.180383,89.516301;136.190769,-67.460918;135.691134,-67.484760;132.680469,89.507006;132.180556,89.497710;135.191500,-67.508602;134.691865,-67.532444;131.680642,89.488415;131.180728,89.479120;134.192230,-67.556286;133.692595,-67.580128;130.680815,89.469824;130.180901,89.460529;133.192961,-67.603970;132.693326,-67.627812;129.680987,89.451234;129.181074,89.441938;132.193691,-67.651654;131.694057,-67.675496;128.681160,89.432643;128.181246,89.423348;131.194422,-67.699338;130.694787,-67.723180;127.681333,89.414052;127.181419,89.404757;130.195152,-67.747022;129.695518,-67.770864;126.681505,89.395462;126.181592,89.386166;129.195883,-67.794706;128.696248,-67.818548;125.681678,89.376871;125.181764,89.367575;128.196614,-67.842390;127.696979,-67.866232;124.681851,89.358280;124.181937,89.348985;127.197344,-67.890074;126.697709,-67.913916;123.682023,89.339689;123.182110,89.330394;126.198075,-67.937758;125.698440,-67.961600;122.682196,89.321099;122.182282,89.311803;125.198805,-67.985442;124.699171,-68.009284;121.682369,89.302508;121.182455,89.293213;124.199536,-68.033126;123.699901,-68.056968;120.682541,89.283917;120.182628,89.274622;123.200267,-68.080811;122.700632,-68.104653;119.682714,89.265327;119.182800,89.256031;122.200997,-68.128495;121.701362,-68.152337;118.682887,89.246736;118.182973,89.237440;121.201728,-68.176179;120.702093,-68.200021;117.683059,89.228145;117.183145,89.218850;120.202458,-68.223863;119.702824,-68.247705;116.683232,89.209554;116.183318,89.200259;119.203189,-68.271547;118.703554,-68.295389;115.683404,89.190964;115.183491,89.181668;118.203919,-68.319231;117.704285,-68.343073;114.683577,89.172373;114.183663,89.163078;117.204650,-68.366915;116.705015,-68.390757;113.683750,89.153782;113.183836,89.144487;116.205381,-68.414599;115.705746,-68.438441;112.683922,89.135191;112.184009,89.125896;115.206111,-68.462283;114.706476,-68.486125;111.684095,89.116601;111.184181,89.107305;114.206842,-68.509967;113.707207,-68.533809;110.684268,89.098010;110.184354,89.088715;113.207572,-68.557651;112.707938,-68.581493;109.684440,89.079419;109.184527,89.070124;112.208303,-68.605335;111.708668,-68.629177;108.684613,89.060829;108.184699,89.051533;111.209034,-68.653019;110.709399,-68.676861;107.684786,89.042238;107.184872,89.032943;110.209764,-68.700703;109.710129,-68.724545;106.684958,89.023647;106.185045,89.014352;109.210495,-68.748387;108.710860,-68.772229;105.685131,89.005056;105.185217,88.995761;108.211225,-68.796072;107.711591,-68.819914;104.685304,88.986466;104.185390,88.977170;107.211956,-68.843756;106.712321,-68.867598;103.685476,88.967875;103.185563,88.958580;106.212686,-68.891440;105.713052,-68.915282;102.685649,88.949284;102.185735,88.939989;105.213417,-68.939124;104.713782,-68.962966;101.685822,88.930694;101.185908,88.921398;104.214148,-68.986808;103.714513,-69.010650;100.685994,88.912103;100.186081,88.902807;103.214878,-69.034492;102.715243,-69.058334;99.686167,88.893512;99.186253,88.884217;102.215609,-69.082176;101.715974,-69.106018;98.686340,88.874921;98.186426,88.865626;101.216339,-69.129860;100.716705,-69.153702;97.686512,88.856331;97.186599,88.847035;100.217070,-69.177544;98.896710,-26.404472;96.686685,88.837740;96.186771,88.828445;98.184464,-15.341673;97.684011,-15.322866;95.686858,88.819149;95.186944,88.809854;97.183559,-15.304059;96.683106,-15.285252;94.687030,88.800559;94.187116,88.791263;96.182654,-15.266445;95.682201,-15.247638;93.687203,88.781968;93.187289,88.772672;95.181748,-15.228831;94.681296,-15.210024;92.687375,88.763377;92.187462,88.754082;94.180843,-15.191217;93.680391,-15.172410;91.687548,88.744786;91.187634,88.735491;93.179938,-15.153602;92.679485,-15.134795;90.687721,88.726196;90.187807,88.716900;92.179033,-15.115988;91.678580,-15.097181;89.687893,88.707605;89.187980,88.698310;91.178128,-15.078374;90.677675,-15.059567;88.688066,88.689014;88.188152,88.679719;90.177222,-15.040760;89.676770,-15.021953;87.688239,88.670423;87.188325,88.661128;89.176317,-15.003146;88.675865,-14.984339;86.688411,88.651833;86.188498,88.642537;88.175412,-14.965532;87.674959,-14.946725;85.688584,88.633242;85.188670,88.623947;87.174507,-14.927918;86.674054,-14.909111;84.688757,88.614651;84.188843,88.605356;86.173602,-14.890304;85.673149,-14.871497;83.688929,88.596061;83.189016,88.586765;85.172696,-14.852690;84.672244,-14.833883;82.689102,88.577470;82.189188,88.568175;84.171791,-14.815076;83.671339,-14.796269;81.689275,88.558879;81.189361,88.549584;83.170886,-14.777462;82.670433,-14.758655;80.689447,88.540288;80.189534,88.530993;82.169981,-14.739848;81.669528,-14.721041;79.689620,88.521698;79.189706,88.512402;81.169076,-14.702234;80.668623,-14.683427;78.689793,88.503107;78.189879,88.493812;80.168170,-14.664620;79.667718,-14.645813;77.689965,88.484516;77.190052,88.475221;79.167265,-14.627006;78.666813,-14.608199;76.690138,88.465926;76.190224,88.456630;78.166360,-14.589392;77.665907,-14.570585;75.690311,88.447335;75.190397,88.438039;77.165455,-14.551778;76.665002,-14.532971;74.690483,88.428744;74.190570,88.419449;76.164550,-14.514164;75.664097,-14.495357;73.690656,88.410153;73.190742,88.400858;75.163644,-14.476550;74.663192,-14.457743;72.690829,88.391563;72.190915,88.382267;74.162739,-14.438936;73.662287,-14.420129;71.691001,88.372972;71.191087,88.363677;73.161834,-14.401322;72.661381,-14.382515;70.691174,88.354381;70.191260,88.345086;72.160929,-14.363708;71.660476,-14.344901;69.691346,88.335791;69.191433,88.326495;71.160024,-14.326094;70.659571,-14.307287;68.691519,88.317200;68.191605,88.307904;70.159118,-14.288480;69.658666,-14.269673;67.691692,88.298609;67.191778,88.289314;69.158213,-14.250866;68.657761,-14.232059;66.691864,88.280018;66.191951,88.270723;68.157308,-14.213252;67.656855,-14.194445;65.692037,88.261428;65.192123,88.252132;67.156403,-14.175638;66.655950,-14.156831;64.692210,88.242837;64.192296,88.233542;66.155498,-14.138024;65.655045,-14.119217;63.692382,88.224246;63.192469,88.214951;65.154592,-14.100410;64.654140,-14.081603;62.692555,88.205655;62.192641,88.196360;64.153687,-14.062796;63.653235,-14.043989;61.692728,88.187065;61.192814,88.177769;63.152782,-14.025182;62.652329,-14.006375;60.692900,88.168474;60.192987,88.159179;62.151877,-13.987568;61.651424,-13.968761;59.693073,88.149883;59.193159,88.140588;61.150972,-13.949954;60.650519,-13.931147;58.693246,88.131293;58.193332,88.121997;60.150066,-13.912340;59.649614,-13.893532;57.693418,88.112702;57.193505,88.103407;59.149161,-13.874725;58.648709,-13.855918;56.693591,88.094111;56.193677,88.084816;58.148256,-13.837111;57.647803,-13.818304;55.693764,88.075520;55.193850,88.066225;57.147351,-13.799497;56.646898,-13.780690;54.693936,88.056930;54.194023,88.047634;56.146446,-13.761883;55.645993,-13.743076;53.694109,88.038339;53.194195,88.029044;55.145540,-13.724269;54.645088,-13.705462;52.694282,88.019748;52.194368,88.010453;54.144635,-13.686655;53.644183,-13.667848;51.694454,88.001158;51.194541,87.991862;53.143730,-13.649041;52.643277,-13.630234;50.694627,87.982567;50.194713,87.973271;52.142825,-13.611427;51.642372,-13.592620;49.694800,87.963976;49.194886,87.954681;51.141920,-13.573813;50.641467,-13.555006;48.694972,87.945385;48.195059,87.936090;50.141014,-13.536199;49.640562,-13.517392;47.695145,87.926795;47.195231,87.917499;49.153262,-14.184416;48.731362,-18.261751;46.695317,87.908204;46.195404,87.898909;48.309462,-22.339085;47.887562,-26.416420;45.695490,87.889613;45.195576,87.880318;47.465662,-30.493755;47.043762,-34.571089;44.695663,87.871023;44.195749,87.861727;46.621861,-38.648424;46.199961,-42.725758;43.695835,87.852432;43.195922,87.843136;45.778061,-46.803093;45.356161,-50.880428;42.696008,87.833841;42.196094,87.824546;44.934261,-54.957762;44.512361,-59.035097;41.696181,87.815250;41.196267,87.805955;44.090461,-63.112431;43.649086,-66.174222;40.696353,87.796660;40.196440,87.787364;43.148745,-66.161233;42.648404,-66.148244;39.696526,87.778069;39.196612,87.768774;42.148063,-66.135255;41.647722,-66.122266;38.696699,87.759478;38.196785,87.750183;41.147381,-66.109277;40.647040,-66.096288;37.696871,87.740887;37.196958,87.731592;40.146699,-66.083299;39.646358,-66.070310;36.697044,87.722297;36.197130,87.713001;39.146017,-66.057322;38.645676,-66.044333;35.697217,87.703706;35.197303,87.694411;38.145335,-66.031344;37.644994,-66.018355;34.697389,87.685115;34.197476,87.675820;37.144653,-66.005366;36.644312,-65.992377;33.697562,87.666525;33.197648,87.657229;36.143971,-65.979388;35.643630,-65.966399;32.697735,87.647934;32.197821,87.638639;35.143289,-65.953410;34.642948,-65.940421;31.697907,87.629343;31.197994,87.620048;34.142607,-65.927433;33.642266,-65.914444;30.698080,87.610752;30.198166,87.601457;33.141925,-65.901455;32.641584,-65.888466;29.698253,87.592162;29.198339,87.582866;32.141243,-65.875477;31.640902,-65.862488;28.698425,87.573571;28.198512,87.564276;31.140561,-65.849499;30.640219,-65.836510;27.698598,87.554980;27.198684,87.545685;30.139878,-65.823521;29.639537,-65.810532;26.698771,87.536390;26.198857,87.527094;29.139196,-65.797544;28.638855,-65.784555;25.698943,87.517799;25.199030,87.508503;28.138514,-65.771566;27.638173,-65.758577;24.699116,87.499208;24.199202,87.489913;27.137832,-65.745588;26.637491,-65.732599;23.699288,87.480617;23.199375,87.471322;26.137150,-65.719610;25.636809,-65.706621;22.699461,87.462027;22.199547,87.452731;25.136468,-65.693632;24.636127,-65.680643;21.699634,87.443436;21.199720,87.434141;24.135786,-65.667655;23.635445,-65.654666;20.699806,87.424845;20.199893,87.415550;23.135104,-65.641677;22.634763,-65.628688;19.699979,87.406255;19.200065,87.396959;22.134422,-65.615699;21.634081,-65.602710;18.700152,87.387664;18.200238,87.378368;21.133740,-65.589721;20.633399,-65.576732;17.700324,87.369073;17.200411,87.359778;20.133058,-65.563743;19.632717,-65.550754;16.700497,87.350482;16.200583,87.341187;19.132376,-65.537766;18.632035,-65.524777;15.700670,87.331892;15.200756,87.322596;18.131694,-65.511788;17.631353,-65.498799;14.700842,87.313301;14.200929,87.304006;17.131012,-65.485810;16.630671,-65.472821;13.701015,87.294710;13.201101,87.285415;16.130330,-65.459832;15.629989,-65.446843;12.701188,87.276119;12.201274,87.266824;15.129648,-65.433854;14.629307,-65.420865;11.701360,87.257529;11.201447,87.248233;14.128966,-65.407877;13.628625,-65.394888;10.701533,87.238938;10.201619,87.229643;13.128284,-65.381899;12.627943,-65.368910;9.701706,87.220347;9.201792,87.211052;12.127602,-65.355921;11.627261,-65.342932;8.701878,87.201757;8.201965,87.192461;11.126920,-65.329943;10.626579,-65.316954;7.702051,87.183166;7.202137,87.173871;10.126238,-65.303965;9.625896,-65.290976;6.702224,87.164575;6.202310,87.155280;9.125555,-65.277988;8.625214,-65.264999;5.702396,87.145984;5.202483,87.136689;8.124873,-65.252010;7.624532,-65.239021;4.734767,85.448411;4.387726,77.467519;7.124191,-65.226032;6.623850,-65.213043;4.040686,69.486626;3.693645,61.505734;6.123509,-65.200054;5.623168,-65.187065;3.346605,53.524841;2.999564,45.543949;5.122827,-65.174076;4.622486,-65.161087;2.652523,37.563056;2.305483,29.582163;4.122145,-65.148099;3.621804,-65.135110;1.958442,21.601271;1.611401,13.620378;3.121463,-65.122121;2.621122,-65.109132;1.264361,5.639486;0.917320,-2.341407;2.120781,-65.096143;1.620440,-65.083154;0.570279,-10.322299;0.223239,-18.303192;1.120099,-65.070165;0.619758,-65.057176;-0.123802,-26.284084;-0.470843,-34.264977;0.119417,-65.044187;-0.380924,-65.031198;-0.817883,-42.245869;-1.164924,-50.226762;-0.881265,-65.018210;-1.381606,-65.005221;-1.511964,-58.207655
LAND1.returnPathCoordinates=-1
LAND1.returnPathRawCoordinates=-1
LAND1.returnPointCoordinates=-1
LAND1.updateTime=2025-12-25 19\:02\:14
LAND1.updateTime=2025-12-26 15\:20\:50
LAND1.userId=-1

 set.properties

@@ -1,5 +1,5 @@
#Mower Configuration Properties - Updated
#Fri Dec 26 12:48:50 CST 2025
#Fri Dec 26 15:24:54 CST 2025
appVersion=-1
boundaryLengthVisible=false
currentWorkLandNumber=LAND1
@@ -8,12 +8,12 @@
handheldMarkerId=1872
idleTrailDurationSeconds=60
manualBoundaryDrawingMode=false
mapScale=7.48
mapScale=1.59
measurementModeEnabled=false
mowerId=6258
serialAutoConnect=true
serialBaudRate=115200
serialPortName=COM15
simCardNumber=-1
viewCenterX=-141.32
viewCenterY=608.58
viewCenterX=-72.81
viewCenterY=5.47

 src/gecaoji/Gecaoji.java

@@ -62,7 +62,7 @@

        double x = parseCoordinate(device.getRealtimeX());
        double y = parseCoordinate(device.getRealtimeY());
        double heading = parseHeading(device.getHeading());
        double heading = parseHeading(device.getYaw());
        if (Double.isNaN(x) || Double.isNaN(y)) {
            // Keep showing the last known mower position when temporary sensor glitches occur.
            return;
@@ -151,7 +151,8 @@
        double iconHeight = icon.getHeight(null);
        double maxSide = Math.max(iconWidth, iconHeight);
        double scaleFactor = worldSize / Math.max(maxSide, MIN_SCALE);
        double rotationRadians = Math.toRadians(-headingDegrees);
        // 割草机图标默认朝南，Yaw=0表示正北，需要旋转180度
        double rotationRadians = Math.toRadians(headingDegrees + 180);

        AffineTransform original = g2d.getTransform();
        AffineTransform transformed = new AffineTransform(original);
@@ -176,7 +177,9 @@
        g2d.fill(fallbackShape);
        g2d.setColor(Color.WHITE);
        g2d.draw(fallbackShape);
    double rotationRadians = Math.toRadians(-headingDegrees);
        // Yaw=0表示正北(0, -1)，使用sin/cos计算坐标
        // sin(180)=0, cos(180)=-1 -> 正北
        double rotationRadians = Math.toRadians(180 - headingDegrees);
    double lineLength = radius;
    double dx = lineLength * Math.sin(rotationRadians);
    double dy = lineLength * Math.cos(rotationRadians);

 src/gecaoji/Getgecaojiimu_data.java

@@ -15,7 +15,7 @@
            if (status.getRoll() != null) {
                device.setRoll(String.valueOf(status.getRoll()));
            }
            if (status.getYaw() != null) {
            if (status.getYaw() != null) {//偏航角（-180到180度，0度表示正北）
                device.setYaw(String.valueOf(status.getYaw()));
            }
        } 

 src/lujing/MowingPathGenerationPage.java

@@ -627,8 +627,9 @@
                } else if (grassType == 2) {
                    // 异形地块，有障碍物 -> 调用 YixinglujingHaveObstacel
                    // 传入参数：boundary(A), obstacles(B), plannerWidth(C), safetyMarginStr(D)
                    // 注意：YixinglujingHaveObstacel.planPath 返回 String
                    generated = YixinglujingHaveObstacel.planPath(boundary, obstacles, plannerWidth, safetyMarginStr);
                    List<YixinglujingHaveObstacel.PathSegment> segments = 
                        YixinglujingHaveObstacel.planPath(boundary, obstacles, plannerWidth, safetyMarginStr);
                    generated = formatYixingHaveObstaclePathSegments(segments);
                } else {
                    // 无法判断地块类型，默认按凸形处理或提示
                    if (showMessages) {
@@ -781,6 +782,40 @@
    }

    /**
     * 格式化 YixinglujingHaveObstacel.PathSegment 列表为坐标字符串
     */
    private String formatYixingHaveObstaclePathSegments(List<YixinglujingHaveObstacel.PathSegment> segments) {
        if (segments == null || segments.isEmpty()) {
            return "";
        }
        StringBuilder sb = new StringBuilder();
        YixinglujingHaveObstacel.Point last = null;
        for (YixinglujingHaveObstacel.PathSegment segment : segments) {
            // 如果是第一段，或者当前段起点与上一段终点不连续，则添加起点
            if (last == null || !equalsYixingHaveObstaclePoint(last, segment.start)) {
                appendYixingHaveObstaclePoint(sb, segment.start);
            }
            // 添加终点
            appendYixingHaveObstaclePoint(sb, segment.end);
            last = segment.end;
        }
        return sb.toString();
    }

    private boolean equalsYixingHaveObstaclePoint(YixinglujingHaveObstacel.Point p1, YixinglujingHaveObstacel.Point p2) {
        if (p1 == null || p2 == null) return false;
        double tolerance = 1e-6;
        return Math.abs(p1.x - p2.x) < tolerance && Math.abs(p1.y - p2.y) < tolerance;
    }

    private void appendYixingHaveObstaclePoint(StringBuilder sb, YixinglujingHaveObstacel.Point point) {
        if (sb.length() > 0) {
            sb.append(";");
        }
        sb.append(String.format(Locale.US, "%.6f,%.6f", point.x, point.y));
    }

    /**
     * 比较两个点是否相同（使用小的容差）
     */
    private boolean equals2D(AoxinglujingNoObstacle.Point p1, AoxinglujingNoObstacle.Point p2) {

 src/lujing/YixinglujingHaveObstacel.java

@@ -1,23 +1,351 @@
package lujing;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;

/**
 * 有障碍物异形地块路径规划类
 * 异形草地路径规划 - 避障增强版 V7.0
 * 优化：增加了多边形外扩稳定性、障碍物碰撞预判以及冗余路径消除。
 */
public class YixinglujingHaveObstacel {
    
    public static List<PathSegment> planPath(String coordinates, String obstaclesStr, String widthStr, String marginStr) {
        List<Point> rawPoints = parseCoordinates(coordinates);
        if (rawPoints.size() < 3) return new ArrayList<>();

        double mowWidth = Double.parseDouble(widthStr);
        double safeMargin = Double.parseDouble(marginStr);

        // 1. 预处理地块（确保逆时针）
        ensureCounterClockwise(rawPoints);
        List<Point> boundary = getOffsetPolygon(rawPoints, -safeMargin); // 内缩
        if (boundary.size() < 3) return new ArrayList<>();

        // 2. 规划基础路径 (无障碍物状态)
        double bestAngle = findOptimalAngle(boundary);
        Point firstScanStart = getFirstScanPoint(boundary, mowWidth, bestAngle);
        List<Point> alignedBoundary = alignBoundaryStart(boundary, firstScanStart);

        List<PathSegment> baseLines = new ArrayList<>();
        // 第一阶段：围边
        for (int i = 0; i < alignedBoundary.size(); i++) {
            baseLines.add(new PathSegment(alignedBoundary.get(i), alignedBoundary.get((i + 1) % alignedBoundary.size()), true));
        }
        // 第二阶段：内部扫描
        Point lastEdgePos = alignedBoundary.get(0);
        baseLines.addAll(generateGlobalScanPath(boundary, mowWidth, bestAngle, lastEdgePos));

        // 3. 处理障碍物：解析并执行外扩 (障碍物需向外扩 margin)
        List<Obstacle> obstacles = parseObstacles(obstaclesStr, safeMargin);

        // 4. 路径裁剪与优化连接
        return optimizeAndClipPath(baseLines, obstacles);
    }

    private static List<PathSegment> optimizeAndClipPath(List<PathSegment> originalPath, List<Obstacle> obstacles) {
        List<PathSegment> result = new ArrayList<>();
        Point currentPos = null;

        for (PathSegment segment : originalPath) {
            List<PathSegment> clipped = new ArrayList<>();
            clipped.add(segment);

            for (Obstacle obs : obstacles) {
                List<PathSegment> nextIter = new ArrayList<>();
                for (PathSegment s : clipped) {
                    nextIter.addAll(obs.clipSegment(s));
                }
                clipped = nextIter;
            }

            for (PathSegment s : clipped) {
                // 优化点：消除长度几乎为0的无效线段
                if (Math.hypot(s.start.x - s.end.x, s.start.y - s.end.y) < 1e-4) continue;

                if (currentPos != null && Math.hypot(currentPos.x - s.start.x, currentPos.y - s.start.y) > 0.01) {
                    // 添加空载路径
                    result.add(new PathSegment(currentPos, s.start, false));
                }
                result.add(s);
                currentPos = s.end;
            }
        }
        return result;
    }

    // --- 障碍物模型 ---
    abstract static class Obstacle {
        abstract boolean isInside(Point p);
        abstract List<PathSegment> clipSegment(PathSegment seg);
    }

    static class PolyObstacle extends Obstacle {
        List<Point> points;
        double minX, maxX, minY, maxY;

        public PolyObstacle(List<Point> pts) {
            this.points = pts;
            // 预计算 AABB 边界框提升效率
            minX = minY = Double.MAX_VALUE;
            maxX = maxY = -Double.MAX_VALUE;
            for (Point p : pts) {
                minX = Math.min(minX, p.x); maxX = Math.max(maxX, p.x);
                minY = Math.min(minY, p.y); maxY = Math.max(maxY, p.y);
            }
        }

        @Override
        boolean isInside(Point p) {
            if (p.x < minX || p.x > maxX || p.y < minY || p.y > maxY) return false;
            boolean inside = false;
            for (int i = 0, j = points.size() - 1; i < points.size(); j = i++) {
                if (((points.get(i).y > p.y) != (points.get(j).y > p.y)) &&
                    (p.x < (points.get(j).x - points.get(i).x) * (p.y - points.get(i).y) / (points.get(j).y - points.get(i).y) + points.get(i).x)) {
                    inside = !inside;
                }
            }
            return inside;
        }

        @Override
        List<PathSegment> clipSegment(PathSegment seg) {
            List<Double> ts = new ArrayList<>(Arrays.asList(0.0, 1.0));
            for (int i = 0; i < points.size(); i++) {
                double t = getIntersectionT(seg.start, seg.end, points.get(i), points.get((i + 1) % points.size()));
                if (t > 0 && t < 1) ts.add(t);
            }
            Collections.sort(ts);
            List<PathSegment> res = new ArrayList<>();
            for (int i = 0; i < ts.size() - 1; i++) {
                Point s = interpolate(seg.start, seg.end, ts.get(i));
                Point e = interpolate(seg.start, seg.end, ts.get(i + 1));
                if (!isInside(new Point((s.x + e.x) / 2, (s.y + e.y) / 2))) {
                    res.add(new PathSegment(s, e, seg.isMowing));
                }
            }
            return res;
        }
    }

    static class CircleObstacle extends Obstacle {
        Point center; double radius;
        public CircleObstacle(Point c, double r) { this.center = c; this.radius = r; }

        @Override
        boolean isInside(Point p) { return Math.hypot(p.x - center.x, p.y - center.y) < radius - 1e-4; }

        @Override
        List<PathSegment> clipSegment(PathSegment seg) {
            List<Double> ts = new ArrayList<>(Arrays.asList(0.0, 1.0));
            double dx = seg.end.x - seg.start.x, dy = seg.end.y - seg.start.y;
            double fx = seg.start.x - center.x, fy = seg.start.y - center.y;
            double a = dx * dx + dy * dy;
            double b = 2 * (fx * dx + fy * dy);
            double c = fx * fx + fy * fy - radius * radius;
            double disc = b * b - 4 * a * c;
            if (disc >= 0) {
                disc = Math.sqrt(disc);
                double t1 = (-b - disc) / (2 * a), t2 = (-b + disc) / (2 * a);
                if (t1 > 0 && t1 < 1) ts.add(t1);
                if (t2 > 0 && t2 < 1) ts.add(t2);
            }
            Collections.sort(ts);
            List<PathSegment> res = new ArrayList<>();
            for (int i = 0; i < ts.size() - 1; i++) {
                Point s = interpolate(seg.start, seg.end, ts.get(i));
                Point e = interpolate(seg.start, seg.end, ts.get(i + 1));
                if (!isInside(new Point((s.x + e.x) / 2, (s.y + e.y) / 2))) res.add(new PathSegment(s, e, seg.isMowing));
            }
            return res;
        }
    }

    // --- 算法工具类 ---

    private static List<Obstacle> parseObstacles(String obsStr, double margin) {
        List<Obstacle> obstacles = new ArrayList<>();
        if (obsStr == null || obsStr.trim().isEmpty()) return obstacles;
        for (String group : obsStr.split("\\$")) {
            List<Point> pts = parseCoordinates(group);
            if (pts.size() == 2) {
                double r = Math.hypot(pts.get(0).x - pts.get(1).x, pts.get(0).y - pts.get(1).y);
                obstacles.add(new CircleObstacle(pts.get(0), r + margin));
            } else if (pts.size() > 2) {
                ensureCounterClockwise(pts);
                // 多边形外扩：offset 为正
                obstacles.add(new PolyObstacle(getOffsetPolygon(pts, margin)));
            }
        }
        return obstacles;
    }

    /**
     * 生成路径
     * @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..."
     * @param obstacleCoordsStr 障碍物坐标字符串
     * @param mowingWidthStr 割草宽度字符串，如 "0.34"
     * @param safetyMarginStr 安全边距字符串，如 "0.2"
     * @return 路径坐标字符串，格式 "x1,y1;x2,y2;..."
     * 优化后的多边形外扩/内缩算法
     * @param offset 正数为外扩，负数为内缩
     */
    public static String planPath(String boundaryCoordsStr, String obstacleCoordsStr, String mowingWidthStr, String safetyMarginStr) {
        // TODO: 实现异形地块有障碍物路径规划算法
        // 目前使用默认方法作为临时实现
        throw new UnsupportedOperationException("YixinglujingHaveObstacel.planPath 尚未实现");
    private static List<Point> getOffsetPolygon(List<Point> points, double offset) {
        List<Point> result = new ArrayList<>();
        int n = points.size();
        for (int i = 0; i < n; i++) {
            Point p1 = points.get((i - 1 + n) % n), p2 = points.get(i), p3 = points.get((i + 1) % n);
            
            double v1x = p2.x - p1.x, v1y = p2.y - p1.y;
            double v2x = p3.x - p2.x, v2y = p3.y - p2.y;
            double l1 = Math.hypot(v1x, v1y), l2 = Math.hypot(v2x, v2y);
            
            if (l1 < 1e-6 || l2 < 1e-6) continue;

            // 法向量
            double n1x = -v1y / l1, n1y = v1x / l1;
            double n2x = -v2y / l2, n2y = v2x / l2;

            // 角平分线
            double bx = n1x + n2x, by = n1y + n2y;
            double bl = Math.hypot(bx, by);
            if (bl < 1e-6) { bx = n1x; by = n1y; } else { bx /= bl; by /= bl; }

            // 修正距离
            double sinHalf = n1x * bx + n1y * by;
            double d = offset / Math.max(sinHalf, 0.1);
            result.add(new Point(p2.x + bx * d, p2.y + by * d));
        }
        return result;
    }

    private static List<PathSegment> generateGlobalScanPath(List<Point> polygon, double width, double angle, Point currentPos) {
        List<PathSegment> segments = new ArrayList<>();
        List<Point> rotated = new ArrayList<>();
        for (Point p : polygon) rotated.add(rotatePoint(p, -angle));
        
        double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE;
        for (Point p : rotated) { minY = Math.min(minY, p.y); maxY = Math.max(maxY, p.y); }

        boolean l2r = true;
        for (double y = minY + width/2; y <= maxY - width/2; y += width) {
            List<Double> xInters = getXIntersections(rotated, y);
            if (xInters.size() < 2) continue;
            Collections.sort(xInters);

            List<PathSegment> row = new ArrayList<>();
            for (int i = 0; i < xInters.size() - 1; i += 2) {
                Point s = rotatePoint(new Point(xInters.get(i), y), angle);
                Point e = rotatePoint(new Point(xInters.get(i + 1), y), angle);
                row.add(new PathSegment(s, e, true));
            }
            if (!l2r) {
                Collections.reverse(row);
                for (PathSegment s : row) { Point t = s.start; s.start = s.end; s.end = t; }
            }
            for (PathSegment s : row) {
                if (Math.hypot(currentPos.x - s.start.x, currentPos.y - s.start.y) > 0.01) {
                    segments.add(new PathSegment(currentPos, s.start, false));
                }
                segments.add(s);
                currentPos = s.end;
            }
            l2r = !l2r;
        }
        return segments;
    }

    // --- 基础数学函数 ---
    private static double getIntersectionT(Point a, Point b, Point c, Point d) {
        double ux = b.x - a.x, uy = b.y - a.y, vx = d.x - c.x, vy = d.y - c.y;
        double det = vx * uy - vy * ux;
        if (Math.abs(det) < 1e-6) return -1;
        return (vx * (c.y - a.y) - vy * (c.x - a.x)) / det;
    }

    private static Point interpolate(Point a, Point b, double t) {
        return new Point(a.x + (b.x - a.x) * t, a.y + (b.y - a.y) * t);
    }

    private static Point rotatePoint(Point p, double ang) {
        return new Point(p.x * Math.cos(ang) - p.y * Math.sin(ang), p.x * Math.sin(ang) + p.y * Math.cos(ang));
    }

    private static List<Double> getXIntersections(List<Point> poly, double y) {
        List<Double> res = new ArrayList<>();
        for (int i = 0; i < poly.size(); i++) {
            Point p1 = poly.get(i), p2 = poly.get((i + 1) % poly.size());
            if ((p1.y <= y && p2.y > y) || (p2.y <= y && p1.y > y)) {
                res.add(p1.x + (y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y));
            }
        }
        return res;
    }

    private static Point getFirstScanPoint(List<Point> poly, double w, double a) {
        List<Point> rot = new ArrayList<>();
        for (Point p : poly) rot.add(rotatePoint(p, -a));
        double minY = Double.MAX_VALUE;
        for (Point p : rot) minY = Math.min(minY, p.y);
        List<Double> xs = getXIntersections(rot, minY + w/2);
        if (xs.isEmpty()) return poly.get(0);
        Collections.sort(xs);
        return rotatePoint(new Point(xs.get(0), minY + w/2), a);
    }

    private static List<Point> alignBoundaryStart(List<Point> poly, Point target) {
        int idx = 0; double minD = Double.MAX_VALUE;
        for (int i = 0; i < poly.size(); i++) {
            double d = Math.hypot(poly.get(i).x - target.x, poly.get(i).y - target.y);
            if (d < minD) { minD = d; idx = i; }
        }
        List<Point> res = new ArrayList<>();
        for (int i = 0; i < poly.size(); i++) res.add(poly.get((idx + i) % poly.size()));
        return res;
    }

    private static double findOptimalAngle(List<Point> poly) {
        double bestA = 0, minH = Double.MAX_VALUE;
        for (int i = 0; i < poly.size(); i++) {
            Point p1 = poly.get(i), p2 = poly.get((i + 1) % poly.size());
            double a = Math.atan2(p2.y - p1.y, p2.x - p1.x);
            double h = 0, miY = Double.MAX_VALUE, maY = -Double.MAX_VALUE;
            for (Point p : poly) {
                Point r = rotatePoint(p, -a);
                miY = Math.min(miY, r.y); maY = Math.max(maY, r.y);
            }
            h = maY - miY;
            if (h < minH) { minH = h; bestA = a; }
        }
        return bestA;
    }

    private static void ensureCounterClockwise(List<Point> pts) {
        double s = 0;
        for (int i = 0; i < pts.size(); i++) s += (pts.get((i + 1) % pts.size()).x - pts.get(i).x) * (pts.get((i + 1) % pts.size()).y + pts.get(i).y);
        if (s > 0) Collections.reverse(pts);
    }

    private static List<Point> parseCoordinates(String s) {
        List<Point> pts = new ArrayList<>();
        if (s == null || s.isEmpty()) return pts;
        for (String p : s.split(";")) {
            String[] xy = p.split(",");
            if (xy.length == 2) pts.add(new Point(Double.parseDouble(xy[0]), Double.parseDouble(xy[1])));
        }
        if (pts.size() > 1 && pts.get(0).equals(pts.get(pts.size() - 1))) pts.remove(pts.size() - 1);
        return pts;
    }

    public static class Point {
        public double x, y;
        public Point(double x, double y) { this.x = x; this.y = y; }
        @Override
        public boolean equals(Object o) {
            if (!(o instanceof Point)) return false;
            Point p = (Point) o;
            return Math.abs(x - p.x) < 1e-4 && Math.abs(y - p.y) < 1e-4;
        }
    }

    public static class PathSegment {
        public Point start, end;
        public boolean isMowing;
        public PathSegment(Point s, Point e, boolean m) { this.start = s; this.end = e; this.isMowing = m; }
    }
}

 src/udpdell/UDPServer.java

@@ -76,7 +76,7 @@
        String[] fields = message.split(",");
        // 检查字段数量是否完整
        if (fields.length != 21) {
            System.err.println("Invalid message format, expected 21 fields but got " + fields.length);
            System.err.println("Invalid message format, expected 21 fields but got " + fields.length + ". Message content: [" + message + "]");
            return;
        }