Merge pull request 'main' (#14) from ray/RobotKernal-UESTC:main into main

合并草坪测试通过版本

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(upbot_following)
+project(FollowingCar)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp

Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>upbot_following</name>
+  <name>FollowingCar</name>
   <version>0.0.0</version>
-  <description>The upbot_following package</description>
+  <description>The FollowingCar package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/upbot_following</url> -->
+  <!-- <url type="website">http://wiki.ros.org/FollowingCar</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/data/data_2024-04-09_08-06-34.txt

@@ -0,0 +1,226 @@
+time,goalx,goaly,goaltheta,currentx,currenty,currenttheta,flag,linearx,anglez
+1712649995.270271216,-2.14681,-1.24004,-0.0680199,0.000126608,-0.118708,0.00108108,0,0.0666667,-0.2842111712649995.270350534,
+1712649995.420440111,-2.14681,-1.24004,-0.0680199,0.0001273,-0.119356,0.00107517,0,0.0666667,-0.2842111712649995.420477729,
+1712649995.570565265,-2.14681,-1.24004,-0.0680199,0.000128683,-0.120654,0.00116402,0,0.0666667,-0.2842111712649995.570598800,
+1712649995.720681087,-2.14681,-1.24004,-0.0680199,0.000129375,-0.121302,0.00119952,0,0.0666667,-0.2842111712649995.720718121,
+1712649995.870816156,-2.14681,-1.24004,-0.0680199,0.000130759,-0.122599,0.00120747,0,0.0666667,-0.2842111712649995.870869520,
+1712649996.020970483,-2.14681,-1.24004,-0.0680199,0.00013145,-0.123248,0.00114803,0,0.0666667,-0.2842111712649996.021015099,
+1712649996.171096324,-2.14681,-1.24004,-0.0680199,0.000132833,-0.124544,0.001076,0,0.0666667,-0.2842111712649996.171133650,
+1712649996.321217790,-2.14681,-1.24004,-0.0680199,0.000133525,-0.125193,0.00109211,0,0.0666667,-0.2842111712649996.321256574,
+1712649996.471346839,-2.14681,-1.24004,-0.0680199,0.000134907,-0.126489,0.00103656,0,0.0666667,-0.2842111712649996.471384457,
+1712649996.621469181,-2.14681,-1.24004,-0.0680199,0.000135598,-0.127137,0.000994454,0,0.0666667,-0.2842111712649996.621506215,
+1712649996.771591231,-2.14681,-1.24004,-0.0680199,0.00013698,-0.128432,0.0010035,0,0.0666667,-0.2842111712649996.771628265,
+1712649996.921712698,-2.14681,-1.24004,-0.0680199,0.00013767,-0.12908,0.00104683,0,0.0666667,-0.2842111712649996.921751190,
+1712649997.071835087,-2.14681,-1.24004,-0.0680199,0.000139051,-0.130374,0.00105186,0,0.0666667,-0.2842111712649997.071873288,
+1712649997.221955782,-2.14681,-1.24004,-0.0680199,0.000139741,-0.131021,0.00110925,0,0.0666667,-0.3221051712649997.221995441,
+1712649997.372082310,-2.14681,-1.24004,-0.0680199,0.00014112,-0.132314,0.0011201,0,0.0666667,-0.3221051712649997.372122844,
+1712649997.522205338,-2.14681,-1.24004,-0.0680199,0.000141809,-0.132961,0.00117067,0,0.0666667,-0.3221051712649997.522241498,
+1712649997.672329825,-2.14681,-1.24004,-0.0680199,0.000143187,-0.134252,0.00117975,0,0.0666667,-0.3221051712649997.672369192,
+1712649997.822454020,-2.14681,-1.24004,-0.0680199,0.000143876,-0.134898,0.00118662,0,0.0666667,-0.3221051712649997.822491054,
+1712649997.972573549,-2.14681,-1.24004,-0.0680199,0.000145252,-0.136188,0.00126379,0,0.0666667,-0.3221051712649997.972612333,
+1712649998.122701325,-2.14681,-1.24004,-0.0680199,0.000145939,-0.136833,0.00130866,0,0.0666667,-0.3221051712649998.122738943,
+1712649998.272825040,-2.14681,-1.24004,-0.0680199,0.000147314,-0.138121,0.00137592,0,0.0666667,-0.3221051712649998.272859742,
+1712649998.422944089,-2.14681,-1.24004,-0.0680199,0.000148,-0.138765,0.00133704,0,0.0666667,-0.3221051712649998.422981999,
+1712649998.573071303,-2.14681,-1.24004,-0.0680199,0.000149372,-0.140051,0.00127659,0,0.0666667,-0.3221051712649998.573108921,
+1712649998.723195310,-2.14681,-1.24004,-0.0680199,0.000150058,-0.140694,0.00119922,0,0.0666667,-0.3221051712649998.723232344,
+1712649998.873317275,-2.14681,-1.24004,-0.0680199,0.000151427,-0.141978,0.00111597,0,0.0666667,-0.3221051712649998.873365682,
+1712649999.023461125,-2.14681,-1.24004,-0.0680199,0.000152112,-0.14262,0.00108041,0,0.0666667,-0.3221051712649999.023499617,
+1712649999.173584068,-2.14681,-1.24004,-0.0680199,0.000153479,-0.143902,0.00103988,0,0.0666667,-0.3221051712649999.173622560,
+1712649999.323710510,-2.14681,-1.24004,-0.0680199,0.000154162,-0.144542,0.000980021,0,0.0666667,-0.3221051712649999.323746961,
+1712649999.473836660,-2.14681,-1.24004,-0.0680199,0.000155526,-0.145821,0.000966117,0,0.0666667,-0.3221051712649999.473876902,
+1712649999.623965435,-2.14681,-1.24004,-0.0680199,0.000156208,-0.14646,0.00102357,0,0.0666667,-0.3221051712649999.624005969,
+1712649999.774090127,-2.14681,-1.24004,-0.0680199,0.000157569,-0.147737,0.00106862,0,0.0666667,-0.3221051712649999.774130078,
+1712649999.924217736,-2.14681,-1.24004,-0.0680199,0.000158249,-0.148375,0.00111583,0,0.0666667,-0.3221051712649999.924256812,
+1712650000.074355597,-2.14681,-1.24004,-0.0680199,0.000159608,-0.149648,0.00110795,0,0.0666667,-0.3221051712650000.074403421,
+1712650000.224490015,-2.14681,-1.24004,-0.0680199,0.000160286,-0.150284,0.00118314,0,0.0666667,-0.3221051712650000.224530841,
+1712650000.374632890,-2.14681,-1.24004,-0.0680199,0.000161642,-0.151555,0.00126415,0,0.0666667,-0.3221051712650000.374725913,
+1712650000.524859456,-2.14681,-1.24004,-0.0680199,0.000162318,-0.15219,0.00128455,0,0.0666667,-0.3221051712650000.524944023,
+1712650000.675087481,-2.14681,-1.24004,-0.0680199,0.00016367,-0.153457,0.00121758,0,0.0666667,-0.3221051712650000.675172048,
+1712650000.825302675,-2.14681,-1.24004,-0.0680199,0.000164345,-0.15409,0.00117079,0,0.0666667,-0.2842111712650000.825393949,
+1712650000.975527492,-2.14681,-1.24004,-0.0680199,0.000165694,-0.155354,0.00121616,0,0.0666667,-0.2842111712650000.975614100,
+1712650001.125749765,-2.14681,-1.24004,-0.0680199,0.000166367,-0.155985,0.0011577,0,0.0666667,-0.2842111712650001.125834915,
+1712650001.275972351,-2.14681,-1.24004,-0.0680199,0.000167711,-0.157246,0.00106305,0,0.0666667,-0.2842111712650001.276111449,
+1712650001.426252385,-2.14681,-1.24004,-0.0680199,0.000168383,-0.157876,0.00114053,0,0.0666667,-0.2842111712650001.426341617,
+1712650001.576473221,-2.14681,-1.24004,-0.0680199,0.000169723,-0.159132,0.00113743,0,0.0666667,-0.2842111712650001.576555455,
+1712650001.726689683,-2.14681,-1.24004,-0.0680199,0.000170393,-0.15976,0.00118093,0,0.0666667,-0.2842111712650001.726776291,
+1712650001.876913144,-2.14681,-1.24004,-0.0680199,0.000171729,-0.161013,0.00121736,0,0.0666667,-0.2842111712650001.877012000,
+1712650002.027164031,-2.14681,-1.24004,-0.0680199,0.000172396,-0.161639,0.00118472,0,0.0666667,-0.2842111712650002.027246848,
+1712650002.177380011,-2.14681,-1.24004,-0.0680199,0.000173729,-0.162888,0.00118799,0,0.0666667,-0.2842111712650002.177459329,
+1712650002.327596283,-2.14681,-1.24004,-0.0680199,0.000174394,-0.163512,0.00121916,0,0.0666667,-0.2842111712650002.327682892,
+1712650002.477820138,-2.14681,-1.24004,-0.0680199,0.000175722,-0.164757,0.00124341,0,0.0666667,-0.2842111712650002.477902955,
+1712650002.628034660,-2.14681,-1.24004,-0.0680199,0.000176385,-0.165378,0.00116129,0,0.0666667,-0.2842111712650002.628126517,
+1712650002.778313920,-2.14681,-1.24004,-0.0680199,0.000177708,-0.166619,0.00117145,0,0.0666667,-0.2842111712650002.778402570,
+1712650002.928534567,-2.14681,-1.24004,-0.0680199,0.000178369,-0.167239,0.00117384,0,0.0666667,-0.2842111712650002.928620300,
+1712650003.078752057,-2.14681,-1.24004,-0.0680199,0.000179688,-0.168475,0.00120156,0,0.0666667,-0.2842111712650003.078843039,
+1712650003.228978928,-2.14681,-1.24004,-0.0680199,0.000180346,-0.169093,0.00112874,0,0.0666667,-0.2842111712650003.229063787,
+1712650003.379197051,-2.14681,-1.24004,-0.0680199,0.00018166,-0.170325,0.0011095,0,0.0666667,-0.2842111712650003.379288325,
+1712650003.529385722,-2.14681,-1.24004,-0.0680199,0.000182316,-0.17094,0.00108477,0,0.0666667,-0.2842111712650003.529469997,
+1712650003.679597721,-2.14681,-1.24004,-0.0680199,0.000183625,-0.172167,0.00108061,0,0.0666667,-0.2842111712650003.679637088,
+1712650003.829726028,-2.14681,-1.24004,-0.0680199,0.000184279,-0.17278,0.00104604,0,0.0666667,-0.2842111712650003.829764812,
+1712650003.979849086,-2.14681,-1.24004,-0.0680199,0.000185583,-0.174003,0.00108798,0,0.0666667,-0.2842111712650003.979884371,
+1712650004.129970189,-2.14681,-1.24004,-0.0680199,0.000186234,-0.174613,0.00101155,0,0.0666667,-0.2842111712650004.130008973,
+1712650004.280097139,-2.14681,-1.24004,-0.0680199,0.000187533,-0.175831,0.00106254,0,0.0666667,-0.2842111712650004.280133882,
+1712650004.430223214,-2.14681,-1.24004,-0.0680199,0.000188182,-0.176439,0.00102951,0,0.0666667,-0.2842111712650004.430262873,
+1712650004.580351038,-2.14681,-1.24004,-0.0680199,0.000189476,-0.177652,0.00101661,0,0.0666667,-0.2842111712650004.580386906,
+1712650004.730471281,-2.14681,-1.24004,-0.0680199,0.000190121,-0.178258,0.00101095,0,0.0666667,-0.2842111712650004.730509482,
+1712650004.880612228,-2.14681,-1.24004,-0.0680199,0.00019141,-0.179466,0.000984505,0,0.0666667,-0.2842111712650004.880659469,
+1712650005.030763984,-2.14681,-1.24004,-0.0680199,0.000192053,-0.180069,0.000969288,0,0.0666667,-0.2842111712650005.030804226,
+1712650005.180892201,-2.14681,-1.24004,-0.0680199,0.000193336,-0.181272,0.000960705,0,0.0666667,-0.2842111712650005.180939442,
+1712650005.331036164,-2.14681,-1.24004,-0.0680199,0.000193976,-0.181872,0.00100667,0,0.0666667,-0.3221051712650005.331074949,
+1712650005.481160007,-2.14681,-1.24004,-0.0680199,0.000195254,-0.18307,0.00101509,0,0.0666667,-0.3221051712650005.481198499,
+1712650005.631284724,-2.14681,-1.24004,-0.0680199,0.000195891,-0.183668,0.00103061,0,0.0666667,-0.3221051712650005.631324092,
+1712650005.781408858,-2.14681,-1.24004,-0.0680199,0.000197163,-0.18486,0.00104498,0,0.0666667,-0.3221051712650005.781445893,
+1712650005.931534742,-2.14681,-1.24004,-0.0680199,0.000197798,-0.185455,0.00105974,0,0.0666667,-0.3221051712650005.931571777,
+1712650006.081656304,-2.14681,-1.24004,-0.0680199,0.000199064,-0.186643,0.0010127,0,0.0666667,-0.3221051712650006.081692173,
+1712650006.231775873,-2.14681,-1.24004,-0.0680199,0.000199696,-0.187235,0.00100173,0,0.0666667,-0.3221051712650006.231814074,
+1712650006.381896024,-2.14681,-1.24004,-0.0680199,0.000200956,-0.188417,0.000971277,0,0.0666667,-0.3221051712650006.381935975,
+1712650006.532020842,-2.14681,-1.24004,-0.0680199,0.000201585,-0.189006,0.000959253,0,0.0666667,-0.3221051712650006.532059334,
+1712650006.682143326,-2.14681,-1.24004,-0.0680199,0.00020284,-0.190183,0.00100463,0,0.0666667,-0.3221051712650006.682181527,
+1712650006.832266977,-2.14681,-1.24004,-0.0680199,0.000203466,-0.190769,0.000926351,0,0.0666667,-0.3221051712650006.832304303,
+1712650006.982387420,-2.14681,-1.24004,-0.0680199,0.000204714,-0.19194,0.00089476,0,0.0666667,-0.3221051712650006.982424455,
+1712650007.132511449,-2.14681,-1.24004,-0.0680199,0.000205337,-0.192524,0.000978409,0,0.0666667,-0.3221051712650007.132548484,
+1712650007.282631409,-2.14681,-1.24004,-0.0680199,0.000206579,-0.193689,0.000971347,0,0.0666667,-0.3221051712650007.282669318,
+1712650007.432759242,-2.14681,-1.24004,-0.0680199,0.000207199,-0.19427,0.000971158,0,0.0666667,-0.3221051712650007.432799484,
+1712650007.582884742,-2.14681,-1.24004,-0.0680199,0.000208436,-0.195429,0.00103298,0,0.0666667,-0.3221051712650007.582919152,
+1712650007.733006159,-2.14681,-1.24004,-0.0680199,0.000209052,-0.196007,0.00101848,0,0.0666667,-0.3221051712650007.733044652,
+1712650007.883129327,-2.14681,-1.24004,-0.0680199,0.000210282,-0.197161,0.000998235,0,0.0666667,-0.3221051712650007.883168111,
+1712650008.033253097,-2.14681,-1.24004,-0.0680199,0.000210896,-0.197736,0.00106163,0,0.0666667,-0.3221051712650008.033293631,
+1712650008.183376364,-2.14681,-1.24004,-0.0680199,0.00021212,-0.198883,0.00110045,0,0.0666667,-0.3221051712650008.183419522,
+1712650008.333508962,-2.14681,-1.24004,-0.0680199,0.00021273,-0.199456,0.00106078,0,0.0666667,-0.3221051712650008.333548913,
+1712650008.483635145,-2.14681,-1.24004,-0.0680199,0.000213947,-0.200597,0.00102402,0,0.0666667,-0.3221051712650008.483672179,
+1712650008.633758703,-2.14681,-1.24004,-0.0680199,0.000214555,-0.201166,0.000965161,0,0.0666667,-0.3221051712650008.633796029,
+1712650008.783880219,-2.14681,-1.24004,-0.0680199,0.000215766,-0.202302,0.00100398,0,0.0666667,-0.3221051712650008.783917254,
+1712650008.934006985,-2.14681,-1.24004,-0.0680199,0.00021637,-0.202868,0.000972837,0,0.0666667,-0.3221051712650008.934095635,
+1712650009.084226829,-2.14681,-1.24004,-0.0680199,0.000217574,-0.203998,0.000949887,0,0.0666667,-0.2842111712650009.084307897,
+1712650009.234436803,-2.14681,-1.24004,-0.0680199,0.000218175,-0.204561,0.00101149,0,0.0666667,-0.2842111712650009.234476754,
+1712650009.384561043,-2.14681,-1.24004,-0.0680199,0.000219373,-0.205684,0.0010289,0,0.0666667,-0.2842111712650009.384600119,
+1712650009.534687616,-2.14681,-1.24004,-0.0680199,0.00021997,-0.206244,0.000992854,0,0.0666667,-0.2842111712650009.534725234,
+1712650009.684810398,-2.14681,-1.24004,-0.0680199,0.000221162,-0.207361,0.000981248,0,0.0666667,-0.2842111712650009.684846266,
+1712650009.834932014,-2.14681,-1.24004,-0.0680199,0.000221756,-0.207918,0.00101184,0,0.0666667,-0.2842111712650009.834981004,
+1712650009.985064711,-2.14681,-1.24004,-0.0680199,0.000222941,-0.209029,0.000984676,0,0.0666667,-0.2842111712650009.985104370,
+1712650010.135193995,-2.14681,-1.24004,-0.0680199,0.000223531,-0.209583,0.000999382,0,0.0666667,-0.2842111712650010.135233363,
+1712650010.285316292,-2.14681,-1.24004,-0.0680199,0.000224709,-0.210688,0.00102042,0,0.0666667,-0.2842111712650010.285352744,
+1712650010.435437423,-2.14681,-1.24004,-0.0680199,0.000225297,-0.211238,0.000984065,0,0.0666667,-0.2842111712650010.435476207,
+1712650010.585559137,-2.14681,-1.24004,-0.0680199,0.000226468,-0.212337,0.000963806,0,0.0666667,-0.2842111712650010.585595589,
+1712650010.735680560,-2.14681,-1.24004,-0.0680199,0.000227052,-0.212884,0.000984273,0,0.0666667,-0.2842111712650010.735718469,
+1712650010.885803440,-2.14681,-1.24004,-0.0680199,0.000228217,-0.213976,0.000952735,0,0.0666667,-0.2842111712650010.885840767,
+1712650011.035926342,-2.14681,-1.24004,-0.0680199,0.000228798,-0.214521,0.000984479,0,0.0666667,-0.2842111712650011.035964835,
+1712650011.186038240,-2.14681,-1.24004,-0.0680199,0.000229955,-0.215606,0.000987991,0,0.0666667,-0.2842111712650011.186081107,
+1712650011.336166176,-2.14681,-1.24004,-0.0680199,0.000230533,-0.216147,0.000958061,0,0.0666667,-0.2842111712650011.336204377,
+1712650011.486289446,-2.14681,-1.24004,-0.0680199,0.000231683,-0.217226,0.000900327,0,0.0666667,-0.2842111712650011.486327064,
+1712650011.636414174,-2.14681,-1.24004,-0.0680199,0.000232257,-0.217764,0.000921967,0,0.0666667,-0.2842111712650011.636453542,
+1712650011.786539777,-2.14681,-1.24004,-0.0680199,0.000233401,-0.218837,0.000800684,0,0.0666667,-0.2842111712650011.786577104,
+1712650011.936661589,-2.14681,-1.24004,-0.0680199,0.000233972,-0.219372,0.000883642,0,0.0666667,-0.2842111712650011.936699207,
+1712650012.086782290,-2.14681,-1.24004,-0.0680199,0.000235109,-0.220438,0.000891447,0,0.0666667,-0.2842111712650012.086818158,
+1712650012.236904491,-2.14681,-1.24004,-0.0680199,0.000235676,-0.220969,0.000863855,0,0.0666667,-0.2842111712650012.236942692,
+1712650012.387025526,-2.14681,-1.24004,-0.0680199,0.000236806,-0.222029,0.000871535,0,0.0666667,-0.2842111712650012.387063436,
+1712650012.537147436,-2.14681,-1.24004,-0.0680199,0.000237369,-0.222557,0.000958014,0,0.0666667,-0.2842111712650012.537186512,
+1712650012.687273720,-2.14681,-1.24004,-0.0680199,0.000238492,-0.22361,0.000988933,0,0.0666667,-0.2842111712650012.687313379,
+1712650012.837399421,-2.14681,-1.24004,-0.0680199,0.000239611,-0.224659,0.00092467,0,0.0666667,-0.2842111712650012.837438788,
+1712650012.987523080,-2.14681,-1.24004,-0.0680199,0.000240168,-0.225182,0.000884944,0,0.0666667,-0.2842111712650012.987559240,
+1712650013.137642453,-2.14681,-1.24004,-0.0680199,0.00024128,-0.226224,0.00084285,0,0.0666667,-0.2842111712650013.137681237,
+1712650013.287764751,-2.14681,-1.24004,-0.0680199,0.000241834,-0.226744,0.000789407,0,0.0666667,-0.2842111712650013.287802953,
+1712650013.437892882,-2.14681,-1.24004,-0.0680199,0.000242387,-0.227262,0.000809928,0,0.0666667,-0.2842111712650013.437929917,
+1712650013.588015764,-2.14681,-1.24004,-0.0680199,0.000243489,-0.228295,0.000809845,0,0.0666667,-0.2842111712650013.588054840,
+1712650013.738140104,-2.14681,-1.24004,-0.0680199,0.000244586,-0.229324,0.000811764,0,0.0666667,-0.2842111712650013.738179472,
+1712650013.888266194,-2.14681,-1.24004,-0.0680199,0.000245133,-0.229837,0.000814022,0,0.0666667,-0.2842111712650013.888303520,
+1712650014.038391723,-2.14681,-1.24004,-0.0680199,0.000246224,-0.230859,0.000762338,0,0.0666667,-0.2842111712650014.038431383,
+1712650014.188518785,-2.14681,-1.24004,-0.0680199,0.000246767,-0.231369,0.000717226,0,0.0666667,-0.3221051712650014.188562527,
+1712650014.338648471,-2.14681,-1.24004,-0.0680199,0.000247309,-0.231877,0.000773305,0,0.0666667,-0.3221051712650014.338686380,
+1712650014.488790987,-2.14681,-1.24004,-0.0680199,0.00024839,-0.232891,0.000829193,0,0.0666667,-0.3221051712650014.488832688,
+1712650014.638929422,-2.14681,-1.24004,-0.0680199,0.000248929,-0.233396,0.000781615,0,0.0666667,-0.3221051712650014.638967623,
+1712650014.789061441,-2.14681,-1.24004,-0.0680199,0.000250003,-0.234402,0.000791172,0,0.0666667,-0.3221051712650014.789098184,
+1712650014.939183836,-2.14681,-1.24004,-0.0680199,0.000250538,-0.234904,0.00077134,0,0.0666667,-0.3221051712650014.939273653,
+1712650015.089382398,-2.14681,-1.24004,-0.0680199,0.000251604,-0.235904,0.00071539,0,0.0666667,-0.3221051712650015.090052233,
+1712650015.240194846,-2.14681,-1.24004,-0.0680199,0.000252666,-0.2369,0.000751086,0,0.0666667,-0.3221051712650015.240278539,
+1712650015.390409196,-2.14681,-1.24004,-0.0680199,0.000253196,-0.237396,0.000719424,0,0.0666667,-0.3221051712650015.390497263,
+1712650015.540642501,-2.14681,-1.24004,-0.0680199,0.00025425,-0.238385,0.00075578,0,0.0666667,-0.3221051712650015.540729110,
+1712650015.690868516,-2.14681,-1.24004,-0.0680199,0.000254776,-0.238878,0.000705747,0,0.0666667,-0.3221051712650015.690951917,
+1712650015.841086657,-2.14681,-1.24004,-0.0680199,0.0002553,-0.23937,0.000740916,0,0.0666667,-0.3221051712650015.841168017,
+1712650015.991302173,-2.14681,-1.24004,-0.0680199,0.000256346,-0.24035,0.000748809,0,0.0666667,-0.3221051712650015.991339208,
+1712650016.141425629,-2.14681,-1.24004,-0.0680199,0.000256866,-0.240838,0.000755643,0,0.0666667,-0.3221051712650016.141462664,
+1712650016.291547634,-2.14681,-1.24004,-0.0680199,0.000257905,-0.241811,0.000774634,0,0.0666667,-0.3221051712650016.291583794,
+1712650016.441668473,-2.14681,-1.24004,-0.0680199,0.000258422,-0.242296,0.000740156,0,0.0666667,-0.3221051712650016.441703466,
+1712650016.591789894,-2.14681,-1.24004,-0.0680199,0.000259453,-0.243263,0.000749979,0,0.0666667,-0.3221051712650016.591826638,
+1712650016.741909275,-2.14681,-1.24004,-0.0680199,0.000259967,-0.243745,0.000732085,0,0.0666667,-0.3221051712650016.741944852,
+1712650016.892031863,-2.14681,-1.24004,-0.0680199,0.00026099,-0.244705,0.000712447,0,0.0666667,-0.3221051712650016.892069190,
+1712650017.042155058,-2.14681,-1.24004,-0.0680199,0.000262009,-0.24566,0.000646661,0,0.0666667,-0.3221051712650017.042188593,
+1712650017.192272493,-2.14681,-1.24004,-0.0680199,0.000262517,-0.246136,0.000675837,0,0.0666667,-0.3221051712650017.192312444,
+1712650017.342398676,-2.14681,-1.24004,-0.0680199,0.000263529,-0.247085,0.000643946,0,0.0666667,-0.3221051712650017.342433961,
+1712650017.492521069,-2.14681,-1.24004,-0.0680199,0.000264033,-0.247558,0.000574093,0,0.0666667,-0.3221051712650017.492558687,
+1712650017.642643753,-2.14681,-1.24004,-0.0680199,0.000264536,-0.248029,0.000571024,0,0.0666667,-0.3221051712650017.642679621,
+1712650017.792769644,-2.14681,-1.24004,-0.0680199,0.000265539,-0.248969,0.00045018,0,0.0666667,-0.3221051712650017.792807554,
+1712650017.942896703,-2.14681,-1.24004,-0.0680199,0.000266038,-0.249437,0.000458294,0,0.0666667,-0.3221051712650017.942930821,
+1712650018.093016817,-2.14681,-1.24004,-0.0680199,0.000267033,-0.250371,0.000399107,0,0.0666667,-0.3221051712650018.093055310,
+1712650018.243140472,-2.14681,-1.24004,-0.0680199,0.000267529,-0.250836,0.000428482,0,0.0666667,-0.3221051712650018.243176340,
+1712650018.393246629,-2.14681,-1.24004,-0.0680199,0.000268518,-0.251762,0.000490516,0,0.0666667,-0.3221051712650018.393329447,
+1712650018.543458642,-2.14681,-1.24004,-0.0680199,0.00026901,-0.252224,0.00047376,0,0.0666667,-0.3221051712650018.543545251,
+1712650018.693683486,-2.14681,-1.24004,-0.0680199,0.000269991,-0.253143,0.000448858,0,0.0666667,-0.3221051712650018.693767471,
+1712650018.843912121,-2.14681,-1.24004,-0.0680199,0.00027048,-0.253602,0.000448338,0,0.0666667,-0.2842111712650018.844000188,
+1712650018.994134049,-2.14681,-1.24004,-0.0680199,0.000271454,-0.254515,0.00044846,0,0.0666667,-0.2842111712650018.994219492,
+1712650019.144352565,-2.14681,-1.24004,-0.0680199,0.000271939,-0.25497,0.000448679,0,0.0666667,-0.2842111712650019.144434509,
+1712650019.294564673,-2.14681,-1.24004,-0.0680199,0.000272906,-0.255876,0.000495307,0,0.0666667,-0.2842111712650019.294666738,
+1712650019.444804776,-2.14681,-1.24004,-0.0680199,0.000273387,-0.256328,0.000473128,0,0.0666667,-0.2842111712650019.444887595,
+1712650019.595041380,-2.14681,-1.24004,-0.0680199,0.000274347,-0.257228,0.000453949,0,0.0666667,-0.2842111712650019.595124490,
+1712650019.745257571,-2.14681,-1.24004,-0.0680199,0.000274825,-0.257676,0.00047574,0,0.0666667,-0.2842111712650019.745350304,
+1712650019.895479011,-2.14681,-1.24004,-0.0680199,0.000275778,-0.258569,0.000448803,0,0.0666667,-0.2842111712650019.895563870,
+1712650020.045749175,-2.14681,-1.24004,-0.0680199,0.000276726,-0.259458,0.000418479,0,0.0666667,-0.2842111712650020.045837242,
+1712650020.195966918,-2.14681,-1.24004,-0.0680199,0.000277198,-0.259901,0.000446133,0,0.0666667,-0.2842111712650020.196078023,
+1712650020.346207408,-2.14681,-1.24004,-0.0680199,0.000278139,-0.260783,0.000354167,0,0.0666667,-0.2842111712650020.346295767,
+1712650020.496425151,-2.14681,-1.24004,-0.0680199,0.000278608,-0.261223,0.000374248,0,0.0666667,-0.2842111712650020.496517010,
+1712650020.646651643,-2.14681,-1.24004,-0.0680199,0.000279076,-0.261661,0.000396016,0,0.0666667,-0.2842111712650020.646734753,
+1712650020.796863846,-2.14681,-1.24004,-0.0680199,0.000280007,-0.262535,0.000378342,0,0.0666667,-0.2842111712650020.796955121,
+1712650020.947090046,-2.14681,-1.24004,-0.0680199,0.000280934,-0.263404,0.000408464,0,0.0666667,-0.2842111712650020.947178989,
+1712650021.097308433,-2.14681,-1.24004,-0.0680199,0.000281396,-0.263837,0.000460383,0,0.0666667,-0.2842111712650021.097398833,
+1712650021.247527729,-2.14681,-1.24004,-0.0680199,0.000282316,-0.264699,0.000459459,0,0.0666667,-0.2842111712650021.247617837,
+1712650021.397801556,-2.14681,-1.24004,-0.0680199,0.000282774,-0.265129,0.000461955,0,0.0666667,-0.2842111712650021.397886999,
+1712650021.548018811,-2.14681,-1.24004,-0.0680199,0.000283687,-0.265985,0.000456465,0,0.0666667,-0.2842111712650021.548109211,
+1712650021.698244522,-2.14681,-1.24004,-0.0680199,0.000284142,-0.266411,0.000350532,0,0.0666667,-0.2842111712650021.698333465,
+1712650021.848473442,-2.14681,-1.24004,-0.0680199,0.000285048,-0.267261,0.000439192,0,0.0666667,-0.2842111712650021.848561801,
+1712650021.998710526,-2.14681,-1.24004,-0.0680199,0.000285499,-0.267684,0.000424507,0,0.0666667,-0.2842111712650021.998805009,
+1712650022.148936037,-2.14681,-1.24004,-0.0680199,0.000285949,-0.268106,0.000381845,0,0.0666667,-0.2842111712650022.149018564,
+1712650022.299151928,-2.14681,-1.24004,-0.0680199,0.000286846,-0.268947,0.00033205,0,0.0666667,-0.2842111712650022.299234746,
+1712650022.449371609,-2.14681,-1.24004,-0.0680199,0.000287293,-0.269366,0.000318832,0,0.0666667,-0.2842111712650022.449457635,
+1712650022.599592166,-2.14681,-1.24004,-0.0680199,0.000288183,-0.2702,0.000388174,0,0.0666667,-0.2842111712650022.599678775,
+1712650022.749808931,-2.14681,-1.24004,-0.0680199,0.000289068,-0.27103,0.000415709,0,0.0666667,-0.2842111712650022.749891166,
+1712650022.900024530,-2.14681,-1.24004,-0.0680199,0.000289509,-0.271444,0.000482984,0,0.0666667,-0.2842111712650022.900107348,
+1712650023.050238700,-2.14681,-1.24004,-0.0680199,0.000289949,-0.271856,0.00049082,0,0.0666667,-0.2842111712650023.050327351,
+1712650023.200459642,-2.14681,-1.24004,-0.0680199,0.000290825,-0.272678,0.000430166,0,0.0666667,-0.2842111712650023.200546251,
+1712650023.350681458,-2.14681,-1.24004,-0.0680199,0.000291697,-0.273495,0.000508655,0,0.0666667,-0.2842111712650023.350767776,
+1712650023.500897151,-2.14681,-1.24004,-0.0680199,0.000292131,-0.273902,0.000545735,0,0.0666667,-0.2842111712650023.500985510,
+1712650023.651168250,-2.14681,-1.24004,-0.0680199,0.000292996,-0.274713,0.000552631,0,0.0666667,-0.2842111712650023.651251652,
+1712650023.801377527,-2.14681,-1.24004,-0.0680199,0.000293427,-0.275117,0.000697735,0,0.0666667,-0.2842111712650023.801460346,
+1712650023.951597011,-2.14681,-1.24004,-0.0680199,0.000294285,-0.275922,0.000699751,0,0.0666667,-0.2842111712650023.951685953,
+1712650024.101808682,-2.14681,-1.24004,-0.0680199,0.000294712,-0.276322,0.000729655,0,0.0666667,-0.2842111712650024.101846884,
+1712650024.251935526,-2.14681,-1.24004,-0.0680199,0.000295564,-0.27712,0.000713918,0,0.0666667,-0.2842111712650024.251973144,
+1712650024.402055370,-2.14681,-1.24004,-0.0680199,0.000295988,-0.277518,0.000707845,0,0.0666667,-0.2842111712650024.402100862,
+1712650024.552185713,-2.14681,-1.24004,-0.0680199,0.000296411,-0.277915,0.00063214,0,0.0666667,-0.2842111712650024.552224789,
+1712650024.702312265,-2.14681,-1.24004,-0.0680199,0.000297253,-0.278704,0.000635502,0,0.0666667,-0.2842111712650024.702350758,
+1712650024.852438233,-2.14681,-1.24004,-0.0680199,0.000298091,-0.27949,0.000622344,0,0.0666667,-0.2842111712650024.852479351,
+1712650025.002565077,-2.14681,-1.24004,-0.0680199,0.000298508,-0.279881,0.000601836,0,0.0666667,-0.2842111712650025.002605611,
+1712650025.152699887,-2.14681,-1.24004,-0.0680199,0.00029934,-0.280661,0.000594835,0,0.0666667,-0.2842111712650025.152742171,
+1712650025.302834988,-2.14681,-1.24004,-0.0680199,0.000299754,-0.281049,0.000606105,0,0.0666667,-0.3221051712650025.302872606,
+1712650025.452960175,-2.14681,-1.24004,-0.0680199,0.000300166,-0.281436,0.000580545,0,0.0666667,-0.3221051712650025.453010915,
+1712650025.603103441,-2.14681,-1.24004,-0.0680199,0.000300989,-0.282207,0.000544385,0,0.0666667,-0.3221051712650025.603141351,
+1712650025.753228045,-2.14681,-1.24004,-0.0680199,0.000301807,-0.282974,0.000597542,0,0.0666667,-0.3221051712650025.753267704,
+1712650025.903367812,-2.14681,-1.24004,-0.0680199,0.000302214,-0.283356,0.000542551,0,0.0666667,-0.3221051712650025.903450630,
+1712650026.053584305,-2.14681,-1.24004,-0.0680199,0.000303026,-0.284117,0.00053232,0,0.0666667,-0.3221051712650026.053672956,
+1712650026.203866181,-2.14681,-1.24004,-0.0680199,0.00030343,-0.284496,0.000516088,0,0.0666667,-0.3221051712650026.203951916,
+1712650026.354134643,-2.14681,-1.24004,-0.0680199,0.000304235,-0.285251,0.000491474,0,0.0666667,-0.3221051712650026.354225335,
+1712650026.504353822,-2.14681,-1.24004,-0.0680199,0.000304636,-0.285627,0.000543828,0,0.0666667,-0.3221051712650026.504390857,
+1712650026.654479101,-2.14681,-1.24004,-0.0680199,0.000305036,-0.286001,0.000581778,0,0.0666667,-0.3221051712650026.654521677,
+1712650026.804608171,-2.14681,-1.24004,-0.0680199,0.000305832,-0.286748,0.000636136,0,0.0666667,-0.3221051712650026.804646956,
+1712650026.954731117,-2.14681,-1.24004,-0.0680199,0.000306624,-0.28749,0.000658923,0,0.0666667,-0.3221051712650026.954765236,
+1712650027.104850626,-2.14681,-1.24004,-0.0680199,0.000307018,-0.28786,0.000721557,0,0.0666667,-0.3221051712650027.104889703,
+1712650027.254975997,-2.14681,-1.24004,-0.0680199,0.000307804,-0.288597,0.000685725,0,0.0666667,-0.3221051712650027.255014199,
+1712650027.405098744,-2.14681,-1.24004,-0.0680199,0.000308195,-0.288964,0.000675105,0,0.0666667,-0.3221051712650027.405137529,
+1712650027.555225281,-2.14681,-1.24004,-0.0680199,0.000308974,-0.289694,0.000572315,0,0.0666667,-0.3221051712650027.555261150,
+1712650027.705346570,-2.14681,-1.24004,-0.0680199,0.000309362,-0.290058,0.000523304,0,0.0666667,-0.3221051712650027.705381855,
+1712650027.855468150,-2.14681,-1.24004,-0.0680199,0.000310135,-0.290782,0.000522364,0,0.0666667,-0.3221051712650027.855511601,
+1712650028.005598772,-2.14681,-1.24004,-0.0680199,0.00031052,-0.291143,0.000521173,0,0.0666667,-0.3221051712650028.005643389,
+1712650028.155733275,-2.14681,-1.24004,-0.0680199,0.000310903,-0.291503,0.000515363,0,0.0666667,-0.3221051712650028.155772935,
+1712650028.305864570,-2.14681,-1.24004,-0.0680199,0.000311668,-0.29222,0.000552286,0,0.0666667,-0.3221051712650028.305912395,
+1712650028.456013945,-2.14681,-1.24004,-0.0680199,0.000312428,-0.292932,0.000550537,0,0.0666667,-0.3221051712650028.456069061,
+1712650028.606180526,-2.14681,-1.24004,-0.0680199,0.000312806,-0.293287,0.000544279,0,0.0666667,-0.3221051712650028.606256637,
+1712650028.756352355,-2.14681,-1.24004,-0.0680199,0.00031356,-0.293994,0.000532592,0,0,11712650028.756443339,
+1712650028.906635582,-2.14681,-1.24004,-0.0680199,0.00031356,-0.293994,0.000484469,0,0,11712650028.906731232,

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h

@@ -82,6 +82,10 @@
 #include <vector>
 #include <algorithm>
 #include <cmath>
+#include <fstream>
+#include <ctime>
+#include <mutex>
+
 // services and actions
 #include <ipa_building_msgs/RoomExplorationAction.h>
 #include <cob_map_accessibility_analysis/CheckPerimeterAccessibility.h>
@@ -118,6 +122,7 @@
 
 
 #define PI 3.14159265359
+std::mutex log_mutex;
 
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 
@@ -288,6 +293,63 @@ protected:
 				mat.at<int8_t>(y,x) = map.data[y*mat.cols+x];
 	}
 
+	void fileinit(const std::string& directory)
+	{
+		std::time_t now = std::time(nullptr);
+		std::tm local_time = *std::localtime(&now);
+
+		// 根据当前时间构建文件名
+		std::stringstream filename_stream;
+		filename_stream << directory << "/data_" << std::put_time(&local_time, "%Y-%m-%d_%H-%M-%S") << ".txt";
+		std::string filename = filename_stream.str();
+		outfile.open(filename);
+        if (outfile.is_open()) {
+            std::cout << "start saving data" << std::endl;
+			outfile << "time,"\
+					<< "goalx,goaly,goaltheta,"\
+					<< "currentx,currenty,currenttheta,"\
+					<< "flag,"\
+					<< "linearx,anglez"\
+					<< "\n";
+        } else {
+            std::cerr << "Unable to open file " << filename << std::endl;
+        }
+	}
+	void loginit(const std::string& directory)
+	{
+		std::time_t now = std::time(nullptr);
+		std::tm local_time = *std::localtime(&now);
+
+		// 根据当前时间构建文件名
+		std::stringstream filename_stream;
+		filename_stream << directory << "/log_" << std::put_time(&local_time, "%Y-%m-%d_%H-%M-%S") << ".txt";
+		std::string filename = filename_stream.str();
+		outfile.open(filename);
+        if (outfile.is_open()) {
+            std::cout << "start saving data" << std::endl;
+        } else {
+            std::cerr << "Unable to open file " << filename << std::endl;
+        }
+	}
+
+
+	void writetofile(const geometry_msgs::Pose2D& map_oriented_pose,const geometry_msgs::Pose2D& current_pose,const bool flag_,const geometry_msgs::Twist& cmd_vel_)
+	{
+		ros::Time time=ros::Time::now();
+		outfile << time <<","\
+				<< map_oriented_pose.x << "," << map_oriented_pose.y << "," << map_oriented_pose.theta << ","\
+				<< current_pose.x << "," << current_pose.y << "," << current_pose.theta << ","\
+				<< flag_ << ","\
+				<< cmd_vel_.linear.x <<","<< cmd_vel_.angular.z<<"\n";
+	}
+	void writetolog(const std::string& string)
+	{
+		std::lock_guard<std::mutex> lock(log_mutex);
+		ros::Time time=ros::Time::now();
+		outlog << time <<","<< string <<"\n";
+	}
+
+
 	
 	// !!Important!!
 	//  define the Nodehandle before the action server, or else the server won't start
@@ -297,12 +359,12 @@ protected:
 	dynamic_reconfigure::Server<ipa_room_exploration::RoomExplorationConfig> room_exploration_dynamic_reconfigure_server_;
 	
 
-
 private:
 	void turn(int lr,int time);//rad/s ,0.1s
 	void gostraight(float v,int time);// 0.1s
     void publishZeroVelocity();//Publishes a velocity command of zero to the base
 
+	std::ofstream outfile,outlog;
 	
 	bool flag_=0;
 	bool flag_ultra_=0;

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp

@@ -72,10 +72,13 @@ int main(int argc, char **argv)
 		// std::string file_name;
 		// priv_nh.param<std::string>("image", file_name, "map.pgm");
 		std::string map_name;
+		//纯数字的map_name 不是string类型的读不上
 		priv_nh.param<std::string>("robot_env", map_name, "lab_ipa");
 		// image_path = env_pack_path + "/envs/" + map_name + "/" + file_name;
 	// --------------------修改地图路径------------------------------------------------------
 		image_path = "/home/firefly/pibot_ros/ros_ws/src/map/" + map_name + ".png";
+		std::cout<<image_path<<std::endl;
+
 	}
 
 	cv::Mat map_flipped = cv::imread(image_path, 0);//读入灰度图片

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_server.cpp

@@ -1,4 +1,4 @@
-/*!
+ /*!
  *****************************************************************
  * \file
  *
@@ -719,7 +719,12 @@ void RoomExplorationServer::navigateExplorationPath(const std::vector<geometry_m
 	// 1. publish navigation goals
 	std::vector<geometry_msgs::Pose2D> robot_poses;
 	geometry_msgs::Pose2D last_pose;
-	geometry_msgs::Pose2D pose;
+	geometry_msgs::Pose2D pose,posetmp;
+
+	//file init
+	fileinit("/home/firefly/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/data");
+	loginit("/home/firefly/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/log");
+
 	for(size_t map_oriented_pose = 0; map_oriented_pose < exploration_path.size(); ++map_oriented_pose)
 	{
 		// check if the path should be continued or not
@@ -738,6 +743,8 @@ void RoomExplorationServer::navigateExplorationPath(const std::vector<geometry_m
 		}
 		if(interrupted==true)
 			ROS_INFO("Interrupt order canceled, resuming coverage path now.");
+		
+		posetmp = exploration_path[map_oriented_pose];
 		// if(flag_||flag_ultra_||flag_collid_)
 		if(flag_)
 		{
@@ -745,6 +752,17 @@ void RoomExplorationServer::navigateExplorationPath(const std::vector<geometry_m
 			cancel_msg.id =  std::to_string(ros::Time::now().toSec());
 			cancel_pub_.publish(cancel_msg);
 			publishZeroVelocity();
+
+			double roll, pitch, yaw;
+			geometry_msgs::Pose2D current_pose;
+			tf::TransformListener listener;
+			tf::StampedTransform transform;
+			current_pose.x = transform.getOrigin().x();
+			current_pose.y = transform.getOrigin().y();
+			transform.getBasis().getRPY(roll, pitch, yaw);
+			current_pose.theta = yaw;
+			writetofile(posetmp,current_pose,flag_,cmd_vel_);
+			writetolog("bizhang");
 			// if(flag_collid_)
 			// {
 			// 	gostraight(-0.15,20);
@@ -768,15 +786,16 @@ void RoomExplorationServer::navigateExplorationPath(const std::vector<geometry_m
 			// {
 				while(flag_==1)
 				{
-					turn(-1,17);//1 16 ,-1 17
-					gostraight(0.15,20); //time 0.2m/s  20*0.1s
-					turn(1,16);
+					std::cout<<"--------------------------------"<<std::endl;
+					turn(-1,20);//1 16 ,-1 17
+					gostraight(0.15,40); //time 0.2m/s  20*0.1s
+					turn(1,20);
 					flag_ = 0;
 
 					ros::Duration sleep_rate(2.0);
 					sleep_rate.sleep();
 				}
-				gostraight(0.15,30);
+				gostraight(0.2,40);
 			// }
 			
 			
@@ -802,9 +821,8 @@ void RoomExplorationServer::navigateExplorationPath(const std::vector<geometry_m
 		{
 			temp_goal_eps = goal_eps_;
 		}
-		std::cout<<	map_oriented_pose <<std::endl;
 		
-		publishNavigationGoal(pose, map_frame_, camera_frame_, robot_poses, distance_robot_fov_middlepoint, temp_goal_eps, true); // eps = 0.35
+		publishNavigationGoal(pose, map_frame_, camera_frame_, robot_poses, distance_robot_fov_middlepoint, temp_goal_eps, false); // eps = 0.35
 		last_pose = pose;
 	}
 
@@ -1137,6 +1155,7 @@ bool RoomExplorationServer::publishNavigationGoal(const geometry_msgs::Pose2D& n
 		// try to get the transformation from map_frame to base_frame, wait max. 2 seconds for this transform to come up
 		try
 		{
+			
 			ros::Time time = ros::Time(0);
 			listener.waitForTransform(map_frame, camera_frame, time, ros::Duration(2.0)); // 5.0
 			listener.lookupTransform(map_frame, camera_frame, time, transform);
@@ -1155,14 +1174,16 @@ bool RoomExplorationServer::publishNavigationGoal(const geometry_msgs::Pose2D& n
 				near_pos = true;
 
 			robot_poses.push_back(current_pose);
+			writetofile(map_oriented_pose,current_pose,flag_,cmd_vel_);
+			writetolog("transform1178行");
 		}
 		catch(tf::TransformException &ex)
 		{
 			ROS_WARN_STREAM("Couldn't get transform from " << camera_frame << " to " << map_frame << "!");// %s", ex.what());
 		}
 
-	}while(mv_base_client.getState() != actionlib::SimpleClientGoalState::ABORTED && mv_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED
-			&& near_pos == false);
+	// }while(mv_base_client.getState() != actionlib::SimpleClientGoalState::ABORTED && mv_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED && near_pos == false);
+	}while(mv_base_client.getState() != actionlib::SimpleClientGoalState::ABORTED && mv_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED && near_pos == false && flag_ != 1);
 
 	// check if point could be reached or not
 	if(mv_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED || near_pos == true)
@@ -1170,6 +1191,13 @@ bool RoomExplorationServer::publishNavigationGoal(const geometry_msgs::Pose2D& n
 		ROS_INFO("current goal could be reached.");
 		return true;
 	}
+
+	if(flag_ == 1)
+	{
+		ROS_INFO("....bizhanging....");
+		return true;
+	}
+
 	// if the goal couldn't be reached, find another point around the desired fov-position
 	else if(perimeter_check == true)
 	{
@@ -1227,6 +1255,8 @@ bool RoomExplorationServer::publishNavigationGoal(const geometry_msgs::Pose2D& n
 void RoomExplorationServer::publishZeroVelocity()
 {
 	// ROS_INFO("stop!!");
+	writetolog("stop!!");
+
 	cmd_vel_msg.angular.z = 0;
 	cmd_vel_msg.linear.x = 0; 
 	cmd_vel_pub_.publish(cmd_vel_msg);
@@ -1237,13 +1267,13 @@ void RoomExplorationServer::turn(int lr,int time)
 	ros::Rate rate(10);
 
 	cmd_vel_msg.angular.z = lr;
-	// if(lr==-1)
-	// {
-	// 	std::cout<<"turn right"<<std::endl;
-	// }else
-	// {
-	// 	std::cout<<"turn left"<<std::endl;
-	// }
+	if(lr==-1)
+	{
+		writetolog("turn right");
+	}else
+	{
+		writetolog("turn left");
+	}
 	for (int i = 0; i < time; ++i) {  
 		cmd_vel_pub_.publish(cmd_vel_msg);
 		rate.sleep();
@@ -1270,14 +1300,20 @@ void RoomExplorationServer::stereo_CB(const ipa_building_msgs::dis_info_array::C
 	 for (const auto& obstacle_info :flag->dis)
     {
         float distance = obstacle_info.distance;
+		// std::cout<<"------------"<<distance<<"--------------"<<std::endl;
+		std::string distance_str = std::to_string(obstacle_info.distance);
+		writetolog(distance_str);
+
         // float width = obstacle_info.width;
         // float height = obstacle_info.height;
         float angle = obstacle_info.angle;
 
 		// if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
-		if(distance>10&&distance<45)
+		if(distance>10&&distance<60)
 		{
 			flag_ = 1;
+			std::cout<<"-------jiancewuti-----"<<flag_<<std::endl;
+			writetolog("检测到物体");
 		}
 	}
 	
@@ -1310,9 +1346,7 @@ int main(int argc, char** argv)
 	ros::Time::init();
 
 	ros::NodeHandle nh("~");
-
 	RoomExplorationServer explorationObj(nh, ros::this_node::getName());
 	ros::spin();
-
 	return 0;
 }

Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch

@@ -8,12 +8,12 @@
 	<!-- <arg name="robot_env" value="burger" doc="model type [burger, waffle, waffle_pi]"/> -->
     <!-- <arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/> -->
 	<!-- 小车参数 -->
-	<arg name="robot_radius" default="0.1"/>
-	<arg name="coverage_radius" default="0.1"/>
+	<!-- <arg name="robot_radius" default="0.1"/>
+	<arg name="coverage_radius" default="0.1"/> -->
 	<!-- 大车参数 -->
-	<!-- <arg name="robot_radius" default="0.3"/>
-	<arg name="coverage_radius" default="0.3"/> -->
-	<arg name="use_test_maps" default="true"/>
+	<arg name="robot_radius" default="0.3"/>
+	<arg name="coverage_radius" default="0.3"/>
+	<arg name="use_test_maps" default="false"/>
 
 	<!--  -->
 	<node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
@@ -27,6 +27,6 @@
 	<!-- <node pkg="ipa_room_exploration" type="talkerUltraSound" name="talkerUltraSound" required="true"/> -->
 
 	<!-- <include file="$(find ipa_room_exploration)/ros/launch/clean_work.launch"/> -->
-	<include file="$(find pibot_navigation)/launch/nav.launch"/>
+	<!-- <include file="$(find pibot_navigation)/launch/nav.launch"/> -->
 
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch

@@ -37,8 +37,8 @@
     <node pkg="tf" type="static_transform_publisher" name="base_imu_to_base_link"
         args="0 0.0 0 0 0.0  0.0 /base_link /imu_link 40" />  
 
-    <!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="1 -1 0 0 0 0 /map /odom 50" />  -->
-    <node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" /> 
+    <node pkg="tf" type="static_transform_publisher" name="fix_map" args="0 0 0 0 0 0 /map /odom 50" /> 
+    <!-- <node pkg="tf" type="static_transform_publisher" name="fix_map" args="-2 -2 0 0 0 0 /map /odom 50" />  -->
 
     <!-- <node pkg="tf" type="static_transform_publisher" name="fix_odom" args="0 0 0 0 0 0 /odom /base_link 50" /> -->
     <!-- <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="1.0 1.0 0.0 0 0 0.0 /map /odom 1000"/>     -->
@@ -66,10 +66,10 @@
     <group ns="/" if="$(arg use_odom)" >
         <!-- robot pose ekf -->
         <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
-            <!-- <param name="output_frame" value="odom" /> -->
-              <param name="output_frame" value="odom" />
+            <param name="output_frame" value="odom" />
+            <param name="output_frame" type="bool" value="false" />
             <param name="base_footprint_frame" value="base_link"/>
-            <param name="freq" value="2000.0"/>
+            <param name="freq" value="200.0"/>
             <param name="sensor_timeout" value="1.0"/>  
             <param name="odom_used" value="true"/>
             <param name="imu_used" value="true"/>
@@ -82,7 +82,8 @@
         <!-- odom type adapter -->
         <node pkg="pibot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
             <remap from="input" to="robot_pose_ekf/odom_combined"/>
-            <remap from="output" to="odom"/>
+            <remap from="output" to="odom_imu_ekf"/>
         </node>
     </group>
+
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py

@@ -67,8 +67,8 @@ class OdomEKF():
     def pub_ekf_odom(self, msg):
         odom = Odometry()
         odom.header = msg.header
-        odom.header.frame_id = '/odom'
-        odom.child_frame_id = 'base_link'
+        #odom.header.frame_id = '/odom'
+        #odom.child_frame_id = 'base_link'
         odom.pose = msg.pose
         odom.twist = self.twist
         self.ekf_pub.publish(odom)

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch

@@ -12,7 +12,7 @@
   <node name="map_server" pkg="map_server" type="map_server" args="$(find pibot_navigation)/maps/$(arg map_name)"/>
 
   <include file="$(find pibot_navigation)/launch/include/move_base.launch.xml" />
-
+  <include file="$(find upbot_location)/launch/upbot_location.launch" />
   <!-- <include file="$(find pibot_navigation)/launch/include/amcl.launch.xml" /> -->
   
 </launch>

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml

@@ -33,6 +33,7 @@ DWAPlannerROS:
 # Goal Tolerance Parameters
   yaw_goal_tolerance: 0.2 
   xy_goal_tolerance: 0.15  
+  latch_xy_goal_tolerance: true
   # latch_xy_goal_tolerance: false
 
 # Forward Simulation Parameters

Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/rviz/view_navigation.rviz

@@ -4,12 +4,12 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
-        - /Grid1
-        - /RobotModel1
+        - /TF1
+        - /TF1/Status1
         - /TF1/Frames1
         - /TF1/Tree1
-        - /Map1
         - /Global Map1/Planner1
         - /Local Map1/Planner1
       Splitter Ratio: 0.5
@@ -99,6 +99,8 @@ Visualization Manager:
           Value: true
         odom:
           Value: true
+        odom_combined:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -113,6 +115,8 @@ Visualization Manager:
                 {}
               imu_link:
                 {}
+        odom_combined:
+          {}
       Update Interval: 0
       Value: true
     - Alpha: 1

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   pibot_msgs
   geometry_msgs
+  tf2_ros
 )
 
 # 寻找OpenCV库
@@ -18,7 +19,7 @@ find_package(Boost REQUIRED)
 # catkin_package(
 # #  INCLUDE_DIRS include
 # #  LIBRARIES upbot_location
-# #  CATKIN_DEPENDS roscpp rospy std_msgs
+# #  CATKIN_DEPENDS roscpp rospy std_msgs tf2_ros
 # #  DEPENDS system_lib
 # CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
 # )

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h

@@ -3,13 +3,13 @@
 * Current Version    : V1.0
 * Author             : logzhan
 * Date of Issued     : 2022.09.14
-* Comments           : 导航的矩阵计算
+* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 ********************************************************************************/
 /* Header File Including -----------------------------------------------------*/
 #ifndef _H_MAT_
 #define _H_MAT_
 
-#define MAT_MAX 15 //决定了能处理的最大矩阵
+#define MAT_MAX 15 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
 
 #include <string.h>
@@ -20,55 +20,60 @@ class Mat
 {
 public:
 	Mat();
-	Mat(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
-	void Init(int setm,int setn,int kind);//kind=1单位阵，kind=0零矩阵,其它不初始化内容。
+	Mat(int setm,int setn,int kind);//kind=1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
+	void Init(int setm,int setn,int kind);//kind=1<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>kind=0<><30><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ݡ<EFBFBD>
 
 	void Zero(void);
-	//这些关键数本应该作为private的。但是为了方便，我也做成了public
-	int m;//行数
-	int n;//列数
-	double mat[MAT_MAX][MAT_MAX];//矩阵数据内容
+	//<EFBFBD><EFBFBD>Щ<EFBFBD>ؼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊprivate<EFBFBD>ġ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD><EFBFBD>㣬<EFBFBD><EFBFBD>Ҳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>public
+	int m;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	int n;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	double mat[MAT_MAX][MAT_MAX];//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
-	//特殊的矩阵
-	Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分
-	void FillSubMat(int a,int b,Mat s);//填充子矩阵
+	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>
+	Mat SubMat(int a,int b,int lm,int ln);//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	void FillSubMat(int a,int b,Mat s);//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӿ<EFBFBD><EFBFBD><EFBFBD>
 
-	//向量专用
-	double absvec();//这个是向量的长度。不是个别元素的绝对值。
-	double Sqrt();//向量长度的平方
-	friend Mat operator ^(Mat a,Mat b);//叉乘
+	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
+	double absvec();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD>صľ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>
+	double Sqrt();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȵ<EFBFBD>ƽ<EFBFBD><EFBFBD>
+	friend Mat operator ^(Mat a,Mat b);//<EFBFBD><EFBFBD><EFBFBD>
 
-	//运算
+	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 	friend Mat operator *(double k,Mat a);
 	friend Mat operator *(Mat a,double k);
 	friend Mat operator /(Mat a,double k);
 	friend Mat operator *(Mat a,Mat b);
 	friend Mat operator +(Mat a,Mat b);
 	friend Mat operator -(Mat a,Mat b);
-	friend Mat operator ~(Mat a);//转置	
+	friend Mat operator ~(Mat a);//ת<EFBFBD><EFBFBD>	
 	friend Mat operator /(Mat a,Mat b);//a*inv(b)
 	friend Mat operator %(Mat a,Mat b);//inv(a)*b
 
-	//MAT inv();//逆矩阵
+	//MAT inv();//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 
 private:
-	// 为了用高斯消元法，做的一些函数
-	// 交换两行
+	// Ϊ<EFBFBD><EFBFBD><EFBFBD>ø<EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һЩ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 	void RowExchange(int a, int b);
-	// 某一行乘以系数
+	// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
 	void RowMul(int a,double k);
-	// 对某一行加减另一行的倍数
+	// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD>
 	void RowAdd(int a,int b, double k);
-	// 交换两列
+	// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 	void ColExchange(int a, int b);
-	// 某一列乘以系数
+	// ijһ<EFBFBD>г<EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
 	void ColMul(int a,double k);
-	// 对某一列加减另一列的倍数
+	// <EFBFBD><EFBFBD>ijһ<EFBFBD>мӼ<EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>еı<EFBFBD><EFBFBD><EFBFBD>
 	void ColAdd(int a,int b,double k);
 	
 
 };
 
+
+
+
+
+
 #endif
 
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h

@@ -1,3 +1,5 @@
+#ifndef ALIGN_H
+#define AlIGN_H
 #include <cmath>
 #include <ros/ros.h>
 #include <opencv2/opencv.hpp>
@@ -11,39 +13,37 @@
 #include "lighthouse.h"
 #include "Mat.h"
 
-#ifndef ALIGN_H
-#define AlIGN_H
+
 namespace uwb_slam{
     class Align
     {
     public:
         Align(){
-            imuPos.Init(2, 1, 0);
+            imu_odomPos.Init(2, 1, 0);
             uwbPos.Init(2, 1, 0);
             syncPos.Init(2, 1, 0);
-            imuStartPos.Init(2, 1, 0);
+            imu_odom_StartPos.Init(2, 1, 0);
             Rotate.Init(2,2,0);
             uwbStartPos.Init(2,1,0);
         };
         void Run();
-        void wheel_odomCB(const nav_msgs::Odometry& wheel_odom);
         void imuCB(const pibot_msgs::RawImu& imu);
-        void odomCB(const nav_msgs::Odometry& odom);
+        void odom_imuCB(const nav_msgs::Odometry& odom);
 
     public:
         ros::NodeHandle nh_;
-        ros::Subscriber wheel_odom_sub_;
         ros::Subscriber imu_sub_;
-        ros::Subscriber odom_sub_;
+        ros::Subscriber odom_imu_sub_;
         Imu_odom_pose_data imu_odom_;
         Uwb_data uwb_data_;
         ros::Time imuDataRxTime, uwbDataRxTime, odomDataRxTime;
-        Mat imuStartPos;
-        Mat imuPos;
+        Mat imu_odom_StartPos;
+        Mat imu_odomPos;
         Mat uwbPos;
         Mat syncPos;
         Mat Rotate;
         Mat uwbStartPos;
+        double qx, qy, qz, qw;
         ros::Time odom_tmp_ ;
         bool write_data_ = false;
         cv::Mat img1;

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h

@@ -2,7 +2,8 @@
 #include <opencv2/opencv.hpp>
 #include <mutex>
 #include "uwb.h"
-#include "align.h"
+// #include "align.h"
+#include "senddata.h"
 
 #ifndef MAPPING_H
 #define MAPPING_H

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h

@@ -1,34 +1,38 @@
+
+#ifndef SENDDATA_H
+#define SENDDATA_H
 #include <ros/ros.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/Quaternion.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <mutex>
 #include "uwb.h"
-#ifndef SENDDATA_H
-#define SENDDATA_H
+#include "align.h"
+
 
 
 namespace uwb_slam{
-
     class Senddata
     {
         public:
         Senddata(){};
-        void publishOdometry( std::shared_ptr<uwb_slam::Uwb>uwb);
-        void Run(std::shared_ptr<uwb_slam::Uwb>uwb);
-        void odomCB(const nav_msgs::Odometry& odom);
-
+        void publishOdometry();
+        void publishtf();
+        void Run();
 
+        public:
+        std::shared_ptr<uwb_slam::Align> align_;
         std::mutex mMutexSend;
+
         private:
-        ros::Publisher position_pub_;
-        ros::Subscriber odom_sub_;
         ros::NodeHandle nh_;
-        nav_msgs::Odometry odom_;//odom的消息类型
-        nav_msgs::Odometry sub_odom_;//odom的消息类型
-
-
+        ros::Publisher position_pub_;
+        tf2_ros::TransformBroadcaster broadcaster;
+        nav_msgs::Odometry odom_imu_uwb_;//消息类型 发布融合uwb imu 里程计信息
+        geometry_msgs::TransformStamped transformStamped;//坐标变换消息
         
     };
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/uwb.h

@@ -28,9 +28,12 @@ namespace uwb_slam{
         int pre_seq = -1;
         int cur_seq = -1;
         int AnchorNum = 3;
-        int AnchorPos[3][3]={-240, 400, 113,\
-                                240, 400, 113,\
-                                -400, -240, 113};//基站坐标，序号+三维坐标
+        int AnchorPos[3][3]={-245, 240, 50,\
+                                240,240, 50,\
+                                -245, -115, 50};//基站坐标，序号+三维坐标
+        // int AnchorPos[3][3]={-240, 400, 113,\
+        //                         240, 400, 113,\
+        //                         -400, -240, 113};//基站坐标，序号+三维坐标
         uint8_t tmpdata[13];
         int d[3];
      

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch

@@ -0,0 +1,5 @@
+<launch>
+
+    <node pkg="upbot_location" type="upbot_location_node" name="upbot_location" output="screen" />
+
+</launch>

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp

@@ -3,7 +3,7 @@
 * Current Version    : V1.0
 * Author             : logzhan
 * Date of Issued     : 2022.09.14
-* Comments           : 矩阵运算库
+* Comments           : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 ********************************************************************************/
 /* Header File Including -----------------------------------------------------*/
 #include "Mat.h"
@@ -28,7 +28,7 @@ int mini(int a,int b)
 	
 }
 
-//不要在成员函数内调用构造函数
+//<EFBFBD><EFBFBD>Ҫ<EFBFBD>ڳ<EFBFBD>Ա<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD>ù<EFBFBD><EFBFBD>캯<EFBFBD><EFBFBD>
 Mat::Mat()
 {
 	Init(1,1,0);
@@ -51,7 +51,7 @@ void Mat::Init(int setm,int setn,int kind)
 	if(kind==1)
 	{
 		int x;
-		//C原有的max min会导致两次运行自变量。有附带操作的东西不要直接放到max里面。
+		//Cԭ<EFBFBD>е<EFBFBD>max min<69>ᵼ<EFBFBD><E1B5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD>Ҫֱ<D2AA>ӷŵ<D3B7>max<61><78><EFBFBD>档
 		int xend = mini(this->m, this->n);
 		for(x=0;x < xend;x++){
 			mat[x][x] = 1;
@@ -184,12 +184,12 @@ Mat operator ~(Mat a)
 
 Mat operator /(Mat a,Mat b)
 {
-	//高斯消元法
+	//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
 
 	int x,xb;
 	for(x=0;x<b.n;x++)
 	{
-		//首先找到最佳的列。让起始元素最大
+		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		double s=0;
 		int p=x;
 		double sxb;
@@ -202,19 +202,19 @@ Mat operator /(Mat a,Mat b)
 				s=sxb;
 			}
 		}
-		//同时变换两侧矩阵
+		//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		if(x!=p)
 		{
 			a.ColExchange(x,p);
 			b.ColExchange(x,p);
 		}
 
-		//这一列归一
-		double k=1/b.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
+		//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ
+		double k=1/b.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		a.ColMul(x,k);
 		b.ColMul(x,k);
 
-		//把其它列归零
+		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD>
 		for(xb=0;xb<b.n;xb++)
 		{
 			if(xb!=x)
@@ -232,11 +232,11 @@ Mat operator /(Mat a,Mat b)
 
 Mat operator %(Mat a,Mat b)
 {
-	//高斯消元法
+	//<EFBFBD><EFBFBD>˹<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>
 	int x,xb;
 	for(x=0;x<a.m;x++)
 	{
-		//首先找到最佳的行。让起始元素最大
+		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD>ѵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		double s=0;
 		int p=x;
 		double sxb;
@@ -249,19 +249,19 @@ Mat operator %(Mat a,Mat b)
 				s=sxb;
 			}
 		}
-		//同时变换两侧矩阵
+		//ͬʱ<EFBFBD>任<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		if(x!=p)
 		{
 			a.RowExchange(x,p);
 			b.RowExchange(x,p);
 		}
 
-		//这一行归一
-		double k=1/a.mat[x][x];//这一句不要嵌套到下面两行中，否则会因为更新不同步导致计算错误。
+		//<EFBFBD><EFBFBD>һ<EFBFBD>й<EFBFBD>һ
+		double k=1/a.mat[x][x];//<EFBFBD><EFBFBD>һ<EFBFBD>䲻ҪǶ<EFBFBD>׵<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 		a.RowMul(x,k);
 		b.RowMul(x,k);
 
-		//把其它行归零
+		//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><EFBFBD><EFBFBD>
 		for(xb=0;xb<a.m;xb++)
 		{
 			if(xb!=x)

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp

@@ -6,11 +6,10 @@ namespace uwb_slam{
         imuDataRxTime = ros::Time::now();
         uwbDataRxTime = ros::Time::now();
 
-        wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this);
         imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this);
-        odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this);
+        odom_imu_sub_= nh_.subscribe("odom_imu_ekf",10,&Align::odom_imuCB,this);
 
-        std::ofstream outfile("data.txt",std::ofstream::out);
+        std::ofstream outfile("/home/firefly/pibot_ros/ros_ws/src/upbot_location/data.txt",std::ofstream::out);
         if(outfile.is_open()) {
             std::cout << "start saving data" << std::endl;
             outfile << "imuDataRxTime,odomDataRxTime,uwbDataRxTime,"\
@@ -18,7 +17,7 @@ namespace uwb_slam{
                     << "wX,wY,wZ,"\
                     << "qX,qY,qZ,qW,"\
                     << "vXY,angleV,"\
-                    << "imuPosX,imuPosY,"\
+                    << "imu_odomPosX,imu_odomPosY,"\
                     << "uwbPosX,uwbPosY,"\
                     << "syncPosX,syncPosY,"\
                     << "lightHousePosX,lightHousePosY,lightHousePosZ,"
@@ -28,7 +27,7 @@ namespace uwb_slam{
         }
         
         Mat prevPos(2, 1, 0);
-        Mat detPos(2, 1, 0);
+        Mat deltaPos(2, 1, 0);
         Mat predPos(2, 1, 0);
         Mat R(2, 2, 1);
         Mat Q(2, 2 ,1);
@@ -44,8 +43,8 @@ namespace uwb_slam{
         Mat I = F;
         bool imuStartFlag = 1;
         bool printFlag = 0;
-        double qx, qy, qz, qw;
-        float roll, imuStartRoll;
+
+        float yaw, imuStartyaw;
         while(1){
             // int key3 = cv::waitKey(1);
             // if(key3 == 's'){
@@ -54,40 +53,40 @@ namespace uwb_slam{
             //     printFlag = 0;
             // }
             if(imuDataRxTime!=imu_odom_.imu_data_.imu_t_){
-                std::cout << "imu received" << std::endl;
-                imuPos.mat[0][0] = imu_odom_.pose_[0] * 100;
-                imuPos.mat[1][0] = imu_odom_.pose_[1] * 100;
+                // std::cout << "imu received" << std::endl;
+                imu_odomPos.mat[0][0] = imu_odom_.pose_[0] * 100;
+                imu_odomPos.mat[1][0] = imu_odom_.pose_[1] * 100;
                 qx = imu_odom_.quat_[0];
                 qy = imu_odom_.quat_[1];
                 qz = imu_odom_.quat_[2];
                 qw = imu_odom_.quat_[3];
-                roll = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
+                yaw = -atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
 
-                std::cout<<"roll:"<<roll<<std::endl;
-                if (imuStartFlag == 1 && imuPos.mat[0][0] != 0 && imuPos.mat[1][0] != 0) {
-                    imuStartRoll = roll-PI/2;
-                    imuStartPos = imuPos;
+                //std::cout<<"yaw:"<<yaw<<std::endl;
+                if (imuStartFlag == 1 && imu_odomPos.mat[0][0] != 0 && imu_odomPos.mat[1][0] != 0) {
+                    imuStartyaw = yaw-PI/2;
+                    imu_odom_StartPos = imu_odomPos;
                     imuStartFlag = 0;
                 }
-                //std::cout << imuStartPos.mat[0][0] << " " << imuStartPos.mat[1][0] << std::endl;
-                roll = roll - imuStartRoll;
-                imuPos = imuPos - imuStartPos;
-                detPos = imuPos - prevPos;
-                prevPos = imuPos;
+                //std::cout << imu_odom_StartPos.mat[0][0] << " " << imu_odom_StartPos.mat[1][0] << std::endl;
+                yaw = yaw - imuStartyaw;
+                imu_odomPos = imu_odomPos - imu_odom_StartPos;
+                deltaPos = imu_odomPos - prevPos;
+                prevPos = imu_odomPos;
                 if (uwbDataRxTime != uwb_->uwb_data_.uwb_t_) {
-                    std::cout << "uwb received" << std::endl;
+                    // std::cout << "uwb received" << std::endl;
 
-                    Rotate.mat[0][0] = cos(roll);
-                    Rotate.mat[0][1] = -sin(roll);
-                    Rotate.mat[1][0] = sin(roll);
-                    Rotate.mat[1][1] = cos(roll);
+                    // Rotate.mat[0][0] = cos(yaw);
+                    // Rotate.mat[0][1] = -sin(yaw);
+                    // Rotate.mat[1][0] = sin(yaw);
+                    // Rotate.mat[1][1] = cos(yaw);
 
                     uwbPos.mat[0][0] = uwb_->uwb_data_.x_;
                     uwbPos.mat[1][0] = uwb_->uwb_data_.y_;
 
-                    uwbPos = uwbPos - Rotate * uwbStartPos;
+                    // uwbPos = uwbPos - Rotate * uwbStartPos;
 
-                    predPos = F * syncPos + detPos;
+                    predPos = F * syncPos + deltaPos;
                     Z = H * uwbPos;
                     P = F * P * (~F) + Q;
                     Kg = P * (~H) / (H * P * (~H) + R);
@@ -95,15 +94,15 @@ namespace uwb_slam{
                     P = (I - Kg * H) * P;
                     uwbDataRxTime = uwb_->uwb_data_.uwb_t_;
                 } else {
-                    syncPos = syncPos + detPos;
+                    syncPos = syncPos + deltaPos;
                 }
                 imuDataRxTime = imu_odom_.imu_data_.imu_t_;
                 odomDataRxTime = odom_tmp_;
 
                 std::cout << "syncPos:" << syncPos.mat[0][0] << " " << syncPos.mat[1][0];
-                std::cout << " uwbPos:" << uwbPos.mat[0][0] << " " << uwbPos.mat[1][0];
-                std::cout << " imuPos:" << imuPos.mat[0][0] << " " << imuPos.mat[1][0];
-                std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
+                std::cout << " uwbPos:" << uwbPos.mat[0][0] << " " << uwbPos.mat[1][0] << std::endl;
+                // std::cout << " imu_odomPos:" << imu_odomPos.mat[0][0] << " " << imu_odomPos.mat[1][0]<< std::endl;
+                // std::cout << " lightHousePos:" << lighthouse_->data.x_ << " " << lighthouse_->data.y_ << std::endl;
 
                 
                 outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\
@@ -111,7 +110,7 @@ namespace uwb_slam{
                         << imu_odom_.imu_data_.w_[0] << "," << imu_odom_.imu_data_.w_[1] << "," << imu_odom_.imu_data_.w_[2] << ","\
                         << qx << "," << qy << "," << qz << "," << qw << ","\
                         << imu_odom_.vxy_ << "," <<imu_odom_.angle_v_ << ","\
-                        << imuPos.mat[0][0] << "," << imuPos.mat[1][0] << ","\
+                        << imu_odomPos.mat[0][0] << "," << imu_odomPos.mat[1][0] << ","\
                         << uwbPos.mat[0][0] << "," << uwbPos.mat[1][0] << ","\
                         << syncPos.mat[0][0] << "," << syncPos.mat[1][0] << ","\
                         << lighthouse_->data.x_ << "," << lighthouse_->data.y_ << "," << lighthouse_->data.z_ << ","\
@@ -120,27 +119,11 @@ namespace uwb_slam{
             
         }
 
-        // outfile.close();
+        outfile.close();
         // std::cout<< "Data written to file." << std::endl;   
     }
     
-    
-    
-    void Align::wheel_odomCB(const nav_msgs::Odometry& wheel_odom)
-    {
-        imu_odom_.vxy_= wheel_odom.twist.twist.linear.x;
-        imu_odom_.angle_v_ = wheel_odom.twist.twist.angular.z;
-        // imu_odom_.pose_[0] = wheel_odom.pose.pose.position.x;
-        // imu_odom_.pose_[1] = wheel_odom.pose.pose.position.y;
-        // imu_odom_.pose_[2] = wheel_odom.pose.pose.position.z;
-        // imu_odom_.quat_[0] = wheel_odom.pose.pose.orientation.x;
-        // imu_odom_.quat_[1] = wheel_odom.pose.pose.orientation.y;
-        // imu_odom_.quat_[2] = wheel_odom.pose.pose.orientation.z;
-        // imu_odom_.quat_[3] = wheel_odom.pose.pose.orientation.w;
-
-    
-        return;
-    }
+//imu原始数据，不知道有没有纠正零偏的   
     void Align::imuCB(const pibot_msgs::RawImu& imu)
     {
         imu_odom_.imu_data_.imu_t_ = imu.header.stamp;
@@ -155,7 +138,8 @@ namespace uwb_slam{
         return;
     }
 
-    void Align::odomCB(const nav_msgs::Odometry& odom)
+//imu里程计融合位姿
+    void Align::odom_imuCB(const nav_msgs::Odometry& odom)
     {
         odom_tmp_ = odom.header.stamp;
         imu_odom_.pose_[0] = odom.pose.pose.position.x;
@@ -165,18 +149,12 @@ namespace uwb_slam{
         imu_odom_.quat_[1] = odom.pose.pose.orientation.y;
         imu_odom_.quat_[2] = odom.pose.pose.orientation.z;
         imu_odom_.quat_[3] = odom.pose.pose.orientation.w;
+
+        imu_odom_.vxy_= odom.twist.twist.linear.x;
+        imu_odom_.angle_v_ = odom.twist.twist.angular.z;
     }
 
 
-
-
-
-
-
-
-
-
-
 };
 
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp

@@ -4,7 +4,6 @@
 #include <iostream>
 #include <ros/ros.h>
 #include <thread>
-#include "senddata.h"
 
 
 
@@ -24,11 +23,7 @@ int main(int argc, char** argv)
     mp->align_ = align;
     align->uwb_ = uwb;
     align->lighthouse_ = lighthouse;
-
-    // //  control data fllow in system
-    // std::thread rose_trd ([&system]() {
-    //     system->Run();
-    // });
+    sender->align_ = align;
 
     // uwb serried read
     std::thread uwb_trd([&uwb]() {
@@ -40,8 +35,8 @@ int main(int argc, char** argv)
         mp->Run();
     });
 
-    std::thread sender_trd ([&sender, uwb]() {
-        sender->Run(uwb);
+    std::thread sender_trd ([&sender]() {
+        sender->Run();
     });
 
     std::thread align_trd ([&align]() {

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp

@@ -109,7 +109,7 @@ namespace uwb_slam
                 if (key2 == 'q') {
                     //TODO: save
 
-                    std::string pngimage="/home/firefly/Project_Ros11/Project_Ros1/src/upbot_location/Map/output_image.png";//保存的图片文件路径
+                    std::string pngimage="/home/firefly/pibot_ros/ros_ws/src/upbot_location/map/output_image.png";//保存的图片文件路径
                     cv::imwrite(pngimage,img);
                     readPos = false;
                     
@@ -120,7 +120,7 @@ namespace uwb_slam
                     break;
                 }
         
-                this->feedPos(cv::Point2d(align_->imuPos.mat[0][0], align_->imuPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
+                this->feedPos(cv::Point2d(align_->imu_odomPos.mat[0][0], align_->imu_odomPos.mat[1][0]), cv::Point2d(align_->uwbPos.mat[0][0], align_->uwbPos.mat[1][0]), cv::Point2d(align_->syncPos.mat[0][0], align_->syncPos.mat[1][0]));
                 //this->feedPos(cv::Point2d(uwb_->x, uwb_->y));
 
                 //uwb xieru

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp

@@ -3,73 +3,69 @@
 
 namespace uwb_slam
 {
-    void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){
-    // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹
-        ros::Rate loop_rate(10);
-    // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息
-    // 主题被设置为"uwb_odom"，队列大小为50
-        position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
-    // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到
-    // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10
-        odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
+    void Senddata::Run(){
+
+        ros::Rate loop_rate(100);
+        position_pub_=nh_.advertise<nav_msgs::Odometry>("odom",10);
         while(ros::ok()){
-        // 按照10Hz频率发布uwb信息
-            publishOdometry(uwb);
+            // publishOdometry();
+            publishtf();
             ros::spinOnce();
-        // 用于控制循环速率
             loop_rate.sleep();
         }
 
 
-    }
-    void Senddata::odomCB(const nav_msgs::Odometry& odom){
-    // 这个地方接收的是轮速里程计的信息
-    // 包含位置和姿态
-        sub_odom_ = odom;
-        return;
     }
 
-	/**---------------------------------------------------------------------
-	* Function    : Senddata::publishOdometry
-	* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问
-	* Date        : 2023/12/13 zhanli@review
-	*---------------------------------------------------------------------**/
-    void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
+    void Senddata::publishOdometry()
     {
 
         std::mutex mMutexSend;
         ros::Time current_time = ros::Time::now();
 
         // 设置 Odometry 消息的头部信息
-        odom_.header.stamp = current_time;
-        odom_.header.frame_id = "odom";  // 设置坐标系为 "map"
-        odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
+        odom_imu_uwb_.header.stamp = current_time;
+        odom_imu_uwb_.header.frame_id = "odom";  // 设置坐标系为 "map"
+        odom_imu_uwb_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
 
+        // odom_imu_uwb_.header.frame_id = "odom";  
+        // odom_imu_uwb_.child_frame_id = "map";  
         // 填充 Odometry 消息的位置信息
-        odom_.pose.pose.position.x = uwb->uwb_data_.x_;
-        odom_.pose.pose.position.y = uwb->uwb_data_.y_;
-        odom_.pose.pose.position.z = 0.0;
+        odom_imu_uwb_.pose.pose.position.x = align_->syncPos.mat[0][0]/100;
+        odom_imu_uwb_.pose.pose.position.y = align_->syncPos.mat[1][0]/100;
+        odom_imu_uwb_.pose.pose.position.z = 0.0;
         
 
         // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
-        // tf2::Quaternion quat;
-        // quat.setRPY(0, 0, uwb->theta);  // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
-        // odom.pose.pose.orientation.x = quat.x();
-        // odom.pose.pose.orientation.y = quat.y();
-        // odom.pose.pose.orientation.z = quat.z();
-        // odom.pose.pose.orientation.w = quat.w();
+        odom_imu_uwb_.pose.pose.orientation.x = align_->qx;
+        odom_imu_uwb_.pose.pose.orientation.y = align_->qy;
+        odom_imu_uwb_.pose.pose.orientation.z = align_->qz;
+        odom_imu_uwb_.pose.pose.orientation.w = align_->qw;
 
-        odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
-        odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
-        odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
-        odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;
 
         // 发布 Odometry 消息
-        position_pub_.publish(odom_);
+        position_pub_.publish(odom_imu_uwb_);
 
 
     }
 
+    void Senddata::publishtf()
+    {
+        transformStamped.header.stamp = ros::Time::now();
+
+        transformStamped.header.frame_id = "odom"; // 父坐标系为 "odom"
+        transformStamped.child_frame_id = "base_link"; // 子坐标系为 "base_link"
+        transformStamped.transform.translation.x = align_->syncPos.mat[0][0]/100; // 设置坐标变换的平移部分
+        transformStamped.transform.translation.y = align_->syncPos.mat[1][0]/100;
+        transformStamped.transform.translation.z = 0.0;
+        transformStamped.transform.rotation.x = align_->qx; // 设置坐标变换的旋转部分（使用四元数表示）
+        transformStamped.transform.rotation.y = align_->qy;
+        transformStamped.transform.rotation.z = align_->qz;
+        transformStamped.transform.rotation.w = align_->qw;
+
+        broadcaster.sendTransform(transformStamped);
+    }
+
 
     
 

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp

@@ -60,7 +60,7 @@ namespace uwb_slam{
     void Uwb::Serread(){
         try {
         boost::asio::io_service io;
-        boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径
+        boost::asio::serial_port serial(io, "/dev/uwb"); // 替换成你的串口设备路径
 
         serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
         serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
@@ -92,6 +92,8 @@ namespace uwb_slam{
         d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
         d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;
         d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852;
+        std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
+
         for(int i=0; i<2; i++)
         {
             b.mat[i][0] = (this->d[0]*this->d[0]-this->d[i+1]*this->d[i+1])\

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ranging.h

@@ -17,28 +17,28 @@ private:
     // rknn_handle hdx;
     cv::VideoCapture vcapture;
     Detection yolov5s;
-   
-   /* new stereo
+/*  2-stereo
     Mat cam_matrix_left = (Mat_<double>(3, 3) << 
-        4.809895643547006e+02, 0, 0,
-        0,4.807599168204821e+02, 0,
-        3.362108165786334e+02, 2.298502481932070e+02, 1);
+     4.791153893499601e+02, 0, 0,
+     0,4.798785696902847e+02, 0,
+     3.195422157433843e+02, 2.355081129251360e+02, 1);
     Mat cam_matrix_right = (Mat_<double>(3, 3) << 
-        4.903260126250775e+02, 0, 0,
-        0,4.900310486342847e+02, 0,
-        3.230124997386542e+02, 2.346297967642670e+02, 1);
-    Mat distortion_l = (Mat_<double>(1, 5) <<0.113688825569154,-0.106166584327678, 0,
+     4.851946576927952e+02, 0, 0,
+        0,4.848412081674973e+02, 0,
+       3.139237321746737e+02, 2.316610217516937e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) <<0.091215339806518,-0.088601421082219, 0,
         0, 0);
 
-    Mat distortion_r = (Mat_<double>(1, 5) <<0.121425307936153,-0.141892782717707, 0,
+    Mat distortion_r = (Mat_<double>(1, 5) <<0.086266675232625,-0.070869167707634, 0,
         0, 0);
     Mat rotate = (Mat_<double>(3, 3) << 
-        0.999996295879846, 8.723884080433472e-04, 0.002578209660240,
-        -8.682590894537506e-04,0.999998339366207, -0.001602308016931,
-        -0.002579603213718,0.001600063527818,0.999995392711370);
+    0.999999240684794, -5.243648990073547e-05, -0.001231210888060,
+      5.272610271745578e-05, 0.999999970951700, -0.002653487630467,
+       0.001231198519510, -2.352594617066257e-04,0.999999214401287);
     Mat trans = (Mat_<double>(3, 1) << 
-         -60.348359844102470,-0.030699794141365, 0.495248628081046);
+      -61.636981845981540, -1.107000282904877, -1.084419989523733);
 */
+/*
     Mat cam_matrix_left = (Mat_<double>(3, 3) << 
     4.869084743604942e+02, 0, 0,
     0,4.859921620798602e+02, 0,
@@ -58,7 +58,27 @@ private:
     0.012484284819374, 0.002658381435011,0.999918534502034);
     Mat trans = (Mat_<double>(3, 1) << 
        -60.319997608188590, -0.019664800882533, -0.638993708428792);
+*/
+// 3_stereo:
+    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+    4.926816600424309e+02, 0, 0,
+        0,4.926464996604736e+02, 0,
+     2.968913145354954e+02, 2.282943456903496e+02, 1);
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+     4.936169711204374e+02, 0, 0,
+        0,4.934446944784490e+02, 0,
+       2.874782972788689e+02, 2.301583475160467e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) <<0.109321356376779,-0.134909702605162, 0,
+        0, 0);
 
+    Mat distortion_r = (Mat_<double>(1, 5) <<0.103636140273929,-0.111420134027036, 0,
+        0, 0);
+    Mat rotate = (Mat_<double>(3, 3) << 
+    0.999971802318130, -3.077374593154837e-04, -0.007503323682689,
+     -3.351245877303192e-04,0.999993286165984, -0.003649015547566,
+      0.007502150367846, 0.003651427202043,0.999965191803818);
+    Mat trans = (Mat_<double>(3, 1) << 
+     -60.284286962670556, -0.093489637356761,-0.264272235561136);
     cv::Mat mapX1, mapX2, mapY1, mapY2, q, Z;
 
     int imgw, imgh;
@@ -77,4 +97,9 @@ public:
     std::vector<int> muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates);
     std::vector<cv::Mat> get_range();
     void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k);
+    Ranging& operator=(const Ranging& other) {
+    if (this != &other) {
+    }
+    return *this;
+}
 };

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/detection.cc

@@ -196,6 +196,6 @@ detect_result_group_t Detection::outputParse(cv::Mat netInputImg)
     post_process((int8_t *)outputs[0].buf, (int8_t *)outputs[1].buf, (int8_t *)outputs[2].buf, height, width,
                 box_conf_threshold, nms_threshold, pads, scale_w, scale_h, out_zps, out_scales, &detect_result_group);
 
-
+    rknn_outputs_release(ctx, io_num.n_output,outputs);
     return detect_result_group;
 }

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc

@@ -10,12 +10,28 @@
 #include "opencv2/imgcodecs/imgcodecs.hpp"
 #include <ctime>
 #include <queue>
+#include <cstdlib>
 #include "pibot_msgs/dis_info.h"
 #include "pibot_msgs/dis_info_array.h"
 
 #define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
 
-Ranging r(11, 640, 480);
+Ranging r(40, 640, 480);
+// int findCameraIndex();
+// Ranging r(findCameraIndex(), 640, 480);
+
+// int findCameraIndex() {
+//     cv::VideoCapture cap(0);
+//     if (cap.isOpened()) {
+//         return 0;
+//     }
+//     cap.open(40);
+//     if (cap.isOpened()) {
+//         return 40;
+//     }
+//     return -1;
+// }
+
 std::queue<std::vector<cv::Mat>> frameInfo;
 std::mutex g_mutex;
 
@@ -49,20 +65,7 @@ void *ranging(void *args)		// ranging线程
 			}
 			
         dis_pub_.publish(dis_array);
-		// if (!dis_array.dis.empty())
-		// {
-		// 	for (const auto& data : dis_array.dis)
-		// 	{
-		// 		ROS_INFO("distance: %.2f, width: %.2f, height: %.2f, angle: %.2f",
-		// 				data.distance, data.width, data.height, data.angle);
-		// 	}
-		// }
-
-		
 		}
-		// else{
-		// 	 std::cerr << "Info matrix is empty!" << std::endl;
-		// }
 		g_mutex.lock();
 		for (uchar i = 0; i < frameInfo.size(); i++)		// 只保存当前最新的图片帧信息
 		frameInfo.pop();
@@ -75,6 +78,10 @@ void *ranging(void *args)		// ranging线程
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "stereo");
+	// if (argc >= 2) {
+    //     index = atoi(argv[1]);
+	// 	r = Ranging(index, 640, 480);
+    // }
 	ros::NodeHandle nh; 
 	pthread_t tids[1];		// 执行ranging线程
 	int ret = pthread_create(&tids[0], NULL, ranging, NULL);

Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ranging.cc

@@ -301,7 +301,7 @@ std::vector<Mat> Ranging::get_range()
 
         if (detect_result_group.count<=0)
         {
-            std::cout<<"detect nothing"<<std::endl;
+            // std::cout<<"detect nothing"<<std::endl;
         }
 
         // detction box transfor to our format
@@ -313,26 +313,30 @@ std::vector<Mat> Ranging::get_range()
         {
             detect_result_t *det_result = &(detect_result_group.results[i]);
 
-            // if(strcmp(det_result->name, "vase") == 0 || strcmp(det_result->name, "suitcase") == 0)
-            if(strcmp(det_result->name, "vase") == 0|| strcmp(det_result->name, "suitcase") == 0||
-                // strcmp(det_result->name, "mouse") == 0|| strcmp(det_result->name, "keyboard") == 0||
+            if(strcmp(det_result->name, "vase") == 0)
+            // if(strcmp(det_result->name, "vase") == 0|| strcmp(det_result->name, "suitcase") == 0||
+            //     strcmp(det_result->name, "person") == 0|| strcmp(det_result->name, "keyboard") == 0||
             //     strcmp(det_result->name, "mouse") == 0|| strcmp(det_result->name, "bowl") == 0||
-                strcmp(det_result->name, "cup") == 0|| strcmp(det_result->name, "bottle")==0)
+            //     strcmp(det_result->name, "cup") == 0|| strcmp(det_result->name, "bottle")==0)
             {
                 count++;
                 sprintf(text, "%s %.1f%%", det_result->name, det_result->prop * 100);
                 // printf("%s @ (%d %d %d %d) %f\n", det_result->name, det_result->box.left, det_result->box.top,
                 // det_result->box.right, det_result->box.bottom, det_result->prop);
+                if(det_result->prop <=0.2)
+                    continue;
                 
                 int xmin = det_result->box.left;
                 int ymin = det_result->box.top;
                 int xmax = det_result->box.right;
                 int ymax = det_result->box.bottom;
                 
+
                 rectangle(Rframe, cv::Point(xmin, ymin), cv::Point(xmax, ymax), cv::Scalar(256, 0, 0, 256), 3);
                 putText(Rframe, text, cv::Point(xmin, ymin + 12), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
                 
                 // (x,y) (x,y) conf 
+                
                 detBoxes.at<float>(count-1, 0) = xmin;
                 detBoxes.at<float>(count-1, 1) = ymin;
                 detBoxes.at<float>(count-1, 2) = xmax;

Docs/UWB解算原理.md

@@ -0,0 +1,108 @@
+# UWB 定位原理
+
+#### 1.三维坐标转二维坐标
+
+​		在割草机器人项目中，割草机器人目前只考虑二维平面的定位。但是UWB测量的距离是三维距离，所以我们根据机器人的高度`carH`和`UWB`标签的高度`UwbH`计算出水平距离`dxy`。
+
+```cpp
+// dxy^2 = di^2 - (UwbH = carH)^2 
+for(int i=0; i<3; i++){
+	this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * 
+                     (AnchorPos[i][2] - CARHEIGHT));
+}
+```
+
+![UWB高度修正](./image/UWB高度修正.png)
+
+#### 2.多项式拟合
+
+​		UWB的定位是存在波动的，所以会根据UWB计算距离的规律对计算的距离进行多项式拟合，可以起到滤波提高精度作用。下面的计算实际是收集不同实测距离下，UWB的实际输出距离，利用3次多项式拟合得到的结果。
+
+​		下面的计算跟标签的位置以及高度无关，主要跟UWB的硬件设备的特性有关。
+
+```cpp
+d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
+d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;
+d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852;
+```
+
+#### 3.位置求解
+
+​		UWB位置求解采用如下图示：
+
+![UWB位置示意图](./image/UWB位置示意图.png)
+
+​		UWB的定位可以用下面公式描述, 其中$(x,y)$是割草机器人上面的UWB的位置，另外三个坐标点是3个UWB标签的位置，可以有如下的公式。
+$$
+d_1^2 = (x_1 - x)^2 + (y_1 - y)^2   \space\space\space\space\space (1)\\
+d_2^2 = (x_2 - x)^2 + (y_2 - y)^2   \space\space\space\space\space(2)\\
+d_3^2 = (x_3 - x)^2 + (y_3 - y)^2   \space\space\space\space\space(3)\\
+$$
+​		$(2)-(1)$以及$(3)-(2)$消去二次项，可得：
+$$
+d_1^2 - d_2^2 = \left[ -2(x_1 - x_2)x + x_1^2 - x_2^2 \right] + \left[ -2(y_1 - y_2)y + y_1^2 - y_2^2 \right] \\
+
+d_1^2 - d_3^2 = \left[ -2(x_1 - x_3)x + x_1^2 - x_3^2 \right] + \left[ -2(y_1 - y_3)y + y_1^2 - y_3^2 \right]
+$$
+​		整理为矩阵形式：
+$$
+-2 \begin{bmatrix}
+x_1 - x_2 & y_1 - y_2 \\
+x_1 - x_3 & y_1 - y_3
+\end{bmatrix}
+\begin{bmatrix}
+x \\
+y
+\end{bmatrix}
+=
+\begin{bmatrix}
+(d_1^2 - d_2^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2) \\
+(d_1^2 - d_3^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2)
+\end{bmatrix}
+$$
+​		整理可得：
+$$
+\begin{align*}
+A &= -2\cdot \begin{bmatrix}
+x_1 - x_2 & y_1 - y_2 \\
+x_1 - x_3 & y_1 - y_3
+\end{bmatrix}\\
+b &= \begin{bmatrix}
+(d_1^2 - d_2^2) - (x_1^2 - x_2^2) - (y_1^2 - y_2^2) \\
+(d_1^2 - d_3^2) - (x_1^2 - x_3^2) - (y_1^2 - y_3^2)
+\end{bmatrix}\\
+X &= \begin{bmatrix}
+x\\
+y
+\end{bmatrix}
+\end{align*}
+$$
+​		矩阵A对应的代码：
+
+```cpp
+for(int i=0; i<2; i++){
+    A.mat[i][0] = -2*(this->AnchorPos[0][0]-this->AnchorPos[i+1][0]);
+    A.mat[i][1] = -2*(this->AnchorPos[0][1]-this->AnchorPos[i+1][1]);
+}
+```
+
+​		矩阵b对应的代码：
+
+```cpp
+for(int i=0; i<2; i++)
+{
+	b.mat[i][0] = (this->d[0]*this->d[0]-this->d[i+1]*this->d[i+1])\
+     - (this->AnchorPos[0][0]*this->AnchorPos[0][0]-this->AnchorPos[i+1][0]*this->AnchorPos[i+1][0]) 
+     - (this->AnchorPos[0][1]*this->AnchorPos[0][1]-this->AnchorPos[i+1][1]*this->AnchorPos[i+1][1]);
+}
+```
+
+​		那么，上述矩阵可以通过$X=(A^T\cdot A)^{-1}A^T*b$ 求解UWB的位置。
+
+```cpp
+Mat AT=~A;
+uwbPos=(AT*A)%AT*b;
+this->uwb_data_.x_ = uwbPos.mat[0][0];
+this->uwb_data_.y_ = uwbPos.mat[1][0];
+```
+

Docs/uwb安装说明.md

@@ -0,0 +1,10 @@
+# uwb安装说明：
+
+地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示：
+
+![uwb安装说明](./image/uwb安装说明.png)
+
+注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。
+小车半径：a
+标签高度：h
+BC标签距离d