diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt similarity index 93% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/CMakeLists.txt index 728b454..63fddec 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/CMakeLists.txt +++ b/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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/Mat.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/Mat.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/align.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/align.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/lighthouse.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/lighthouse.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/mapping.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/mapping.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/senddata.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/senddata.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/type.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/type.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/include/uwb.h rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/include/uwb.h diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml similarity index 92% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml index 8bdc0fa..7415ed8 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/package.xml +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/package.xml @@ -1,8 +1,8 @@ - upbot_following + FollowingCar 0.0.0 - The upbot_following package + The FollowingCar package @@ -19,7 +19,7 @@ - + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/Mat.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/Mat.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/align.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/align.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/lighthouse.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/lighthouse.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/main.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/main.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/mapping.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/mapping.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/senddata.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/senddata.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp similarity index 100% rename from Code/MowingRobot/pibot_ros/ros_ws/src/upbot_following/src/uwb.cpp rename to Code/MowingRobot/pibot_ros/ros_ws/src/FollowingCar/src/uwb.cpp diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/data/data_2024-04-09_08-06-34.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/data/data_2024-04-09_08-06-34.txt new file mode 100644 index 0000000..818d722 --- /dev/null +++ b/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, diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h index 9669503..7f205ab 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/include/ipa_room_exploration/room_exploration_action_server.h +++ b/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 #include #include +#include +#include +#include + // services and actions #include #include @@ -118,6 +122,7 @@ #define PI 3.14159265359 +std::mutex log_mutex; typedef actionlib::SimpleActionClient MoveBaseClient; @@ -288,6 +293,63 @@ protected: mat.at(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 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 @@ -296,13 +358,13 @@ protected: actionlib::SimpleActionServer room_exploration_server_; dynamic_reconfigure::Server 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; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp index f508352..ca19456 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/src/room_exploration_action_client.cpp +++ b/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("image", file_name, "map.pgm"); std::string map_name; + //纯数字的map_name 不是string类型的读不上 priv_nh.param("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< 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::vectordis) { float distance = obstacle_info.distance; + // std::cout<<"------------"<5&&distance<45&&cmd_vel_.linear.x!=0) - if(distance>10&&distance<45) + if(distance>10&&distance<60) { flag_ = 1; + std::cout<<"-------jiancewuti-----"< --> - - + - - + + + @@ -27,6 +27,6 @@ - + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png new file mode 100644 index 0000000..4c1dfbe Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png new file mode 100644 index 0000000..5dbb712 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_3.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_3.png new file mode 100644 index 0000000..c04969c Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_3.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_4.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_4.png new file mode 100644 index 0000000..aa3a2dd Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/2_4.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png new file mode 100644 index 0000000..9116421 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/3_2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/3_2.png new file mode 100644 index 0000000..2787d15 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/3_2.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png new file mode 100644 index 0000000..dd6b454 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_600.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_600.png new file mode 100644 index 0000000..2cdda03 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/400_600.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/40_.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/40_.png new file mode 100644 index 0000000..5fefeec Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/40_.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/4_2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/4_2.png new file mode 100644 index 0000000..78f86b2 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/4_2.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png new file mode 100644 index 0000000..7dff090 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/5_3.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/5_3.png new file mode 100644 index 0000000..b9adb42 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/5_3.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/600_.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/600_.png new file mode 100644 index 0000000..1052959 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/600_.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/map/800.png b/Code/MowingRobot/pibot_ros/ros_ws/src/map/800.png new file mode 100644 index 0000000..c937289 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/map/800.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch index 43eb274..dcc1a30 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/launch/bringup_with_imu.launch @@ -37,8 +37,8 @@ - - + + @@ -66,10 +66,10 @@ - - + + - + @@ -82,7 +82,8 @@ - + + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py index 9db038d..718671a 100755 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_bringup/scripts/odom_ekf.py +++ b/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) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch index e9faaac..1f4188c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/launch/nav.launch @@ -12,7 +12,7 @@ - + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml index f10afd8..184977a 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/params/dwa_local_planner_params_apollo.yaml +++ b/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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/rviz/view_navigation.rviz b/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/rviz/view_navigation.rviz index 938a4ce..d5617f9 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/pibot_navigation/rviz/view_navigation.rviz +++ b/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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt index 26392cc..35820e3 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/CMakeLists.txt +++ b/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 # ) diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h index 0586c6c..f8cc745 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/Mat.h +++ b/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 : �����ľ������ ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #ifndef _H_MAT_ #define _H_MAT_ -#define MAT_MAX 15 //决定了能处理的最大矩阵 +#define MAT_MAX 15 //�������ܴ����������� #include @@ -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��λ��kind=0�����,��������ʼ�����ݡ� + void Init(int setm,int setn,int kind);//kind=1��λ��kind=0�����,��������ʼ�����ݡ� void Zero(void); - //这些关键数本应该作为private的。但是为了方便，我也做成了public - int m;//行数 - int n;//列数 - double mat[MAT_MAX][MAT_MAX];//矩阵数据内容 + //��Щ�ؼ�����Ӧ����Ϊprivate�ġ�����Ϊ�˷��㣬��Ҳ������public + int m;//���� + int n;//���� + double mat[MAT_MAX][MAT_MAX];//������������ - //特殊的矩阵 - Mat SubMat(int a,int b,int lm,int ln);//获取矩阵一部分 - void FillSubMat(int a,int b,Mat s);//填充子矩阵 + //����ľ��� + Mat SubMat(int a,int b,int lm,int ln);//��ȡ����һ���� + void FillSubMat(int a,int b,Mat s);//����Ӿ��� - //向量专用 - double absvec();//这个是向量的长度。不是个别元素的绝对值。 - double Sqrt();//向量长度的平方 - friend Mat operator ^(Mat a,Mat b);//叉乘 + //����ר�� + double absvec();//����������ij��ȡ����Ǹ���Ԫ�صľ���ֵ�� + double Sqrt();//�������ȵ�ƽ�� + friend Mat operator ^(Mat a,Mat b);//��� - //运算 + //���� 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);//ת�� friend Mat operator /(Mat a,Mat b);//a*inv(b) friend Mat operator %(Mat a,Mat b);//inv(a)*b - //MAT inv();//逆矩阵 + //MAT inv();//����� private: - // 为了用高斯消元法，做的一些函数 - // 交换两行 + // Ϊ���ø�˹��Ԫ��������һЩ���� + // �������� void RowExchange(int a, int b); - // 某一行乘以系数 + // ijһ�г���ϵ�� void RowMul(int a,double k); - // 对某一行加减另一行的倍数 + // ��ijһ�мӼ���һ�еı��� void RowAdd(int a,int b, double k); - // 交换两列 + // �������� void ColExchange(int a, int b); - // 某一列乘以系数 + // ijһ�г���ϵ�� void ColMul(int a,double k); - // 对某一列加减另一列的倍数 + // ��ijһ�мӼ���һ�еı��� void ColAdd(int a,int b,double k); }; + + + + + #endif diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h index b11132c..601f30b 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/align.h @@ -1,3 +1,5 @@ +#ifndef ALIGN_H +#define AlIGN_H #include #include #include @@ -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; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h index ac39d27..64b5e60 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/mapping.h @@ -2,7 +2,8 @@ #include #include #include "uwb.h" -#include "align.h" +// #include "align.h" +#include "senddata.h" #ifndef MAPPING_H #define MAPPING_H diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h index efef283..f582139 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/senddata.h @@ -1,35 +1,39 @@ + +#ifndef SENDDATA_H +#define SENDDATA_H #include #include #include #include #include +#include +#include #include #include "uwb.h" -#ifndef SENDDATA_H -#define SENDDATA_H +#include "align.h" + namespace uwb_slam{ - class Senddata { public: Senddata(){}; - void publishOdometry( std::shared_ptruwb); - void Run(std::shared_ptruwb); - void odomCB(const nav_msgs::Odometry& odom); - + void publishOdometry(); + void publishtf(); + void Run(); + public: + std::shared_ptr 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;//坐标变换消息 + }; } diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/uwb.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/uwb.h index 4718fed..482662c 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/include/uwb.h +++ b/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]; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch new file mode 100644 index 0000000..f726f40 --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png new file mode 100644 index 0000000..67d7af0 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/2.png new file mode 100644 index 0000000..6c7d1fc Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/2.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/output_image.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/output_image.png new file mode 100644 index 0000000..76abbc6 Binary files /dev/null and b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/output_image.png differ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp index 8bbf4da..4192cb5 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/Mat.cpp +++ b/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 : ��������� ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include "Mat.h" @@ -28,7 +28,7 @@ int mini(int a,int b) } -//不要在成员函数内调用构造函数 +//��Ҫ�ڳ�Ա�����ڵ��ù��캯�� 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ԭ�е�max min�ᵼ�����������Ա������и��������Ķ�����Ҫֱ�ӷŵ�max���档 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) { - //高斯消元法 + //��˹��Ԫ�� int x,xb; for(x=0;xuwb_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_ << "," <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; } - - - - - - - - - }; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp index a8e5068..5d7d868 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/main.cpp @@ -4,7 +4,6 @@ #include #include #include -#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]() { diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp index a6240b7..d81a85d 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/mapping.cpp +++ b/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 diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp index 89c2ee8..59045c0 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/senddata.cpp @@ -3,73 +3,69 @@ namespace uwb_slam { - void Senddata::Run(std::shared_ptruwb){ - // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 - ros::Rate loop_rate(10); - // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 - // 主题被设置为"uwb_odom"，队列大小为50 - position_pub_=nh_.advertise("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("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_ptruwb) + 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); + } + diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp index 2a17887..85526a7 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/uwb.cpp +++ b/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])\ diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ranging.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ranging.h index 3af5a21..c63787f 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ranging.h +++ b/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_(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_(3, 3) << - 4.903260126250775e+02, 0, 0, - 0,4.900310486342847e+02, 0, - 3.230124997386542e+02, 2.346297967642670e+02, 1); - Mat distortion_l = (Mat_(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_(1, 5) <<0.091215339806518,-0.088601421082219, 0, 0, 0); - Mat distortion_r = (Mat_(1, 5) <<0.121425307936153,-0.141892782717707, 0, + Mat distortion_r = (Mat_(1, 5) <<0.086266675232625,-0.070869167707634, 0, 0, 0); Mat rotate = (Mat_(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_(3, 1) << - -60.348359844102470,-0.030699794141365, 0.495248628081046); + -61.636981845981540, -1.107000282904877, -1.084419989523733); */ +/* Mat cam_matrix_left = (Mat_(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_(3, 1) << -60.319997608188590, -0.019664800882533, -0.638993708428792); +*/ +// 3_stereo: + Mat cam_matrix_left = (Mat_(3, 3) << + 4.926816600424309e+02, 0, 0, + 0,4.926464996604736e+02, 0, + 2.968913145354954e+02, 2.282943456903496e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << + 4.936169711204374e+02, 0, 0, + 0,4.934446944784490e+02, 0, + 2.874782972788689e+02, 2.301583475160467e+02, 1); + Mat distortion_l = (Mat_(1, 5) <<0.109321356376779,-0.134909702605162, 0, + 0, 0); + Mat distortion_r = (Mat_(1, 5) <<0.103636140273929,-0.111420134027036, 0, + 0, 0); + Mat rotate = (Mat_(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_(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 muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates); std::vector get_range(); void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k); + Ranging& operator=(const Ranging& other) { + if (this != &other) { + } + return *this; +} }; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/detection.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/detection.cc index b8ecf6e..e9f648b 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/detection.cc +++ b/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; } \ No newline at end of file diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc index 7438b1a..3c310b3 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/main.cc @@ -10,12 +10,28 @@ #include "opencv2/imgcodecs/imgcodecs.hpp" #include #include +#include #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> 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); diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ranging.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ranging.cc index 96589c2..e1255ca 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ranging.cc +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ranging.cc @@ -301,7 +301,7 @@ std::vector Ranging::get_range() if (detect_result_group.count<=0) { - std::cout<<"detect nothing"< 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(count-1, 0) = xmin; detBoxes.at(count-1, 1) = ymin; detBoxes.at(count-1, 2) = xmax; diff --git a/Docs/UWB解算原理.md b/Docs/UWB解算原理.md new file mode 100644 index 0000000..4ed46ea --- /dev/null +++ b/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]; +``` + diff --git a/Docs/image/UWB位置示意图.png b/Docs/image/UWB位置示意图.png new file mode 100644 index 0000000..19a3be3 Binary files /dev/null and b/Docs/image/UWB位置示意图.png differ diff --git a/Docs/image/UWB高度修正.png b/Docs/image/UWB高度修正.png new file mode 100644 index 0000000..5c8659b Binary files /dev/null and b/Docs/image/UWB高度修正.png differ diff --git a/Docs/image/uwb安装说明.png b/Docs/image/uwb安装说明.png new file mode 100644 index 0000000..699bb7e Binary files /dev/null and b/Docs/image/uwb安装说明.png differ diff --git a/Docs/uwb安装说明.md b/Docs/uwb安装说明.md new file mode 100644 index 0000000..6f3bfcc --- /dev/null +++ b/Docs/uwb安装说明.md @@ -0,0 +1,10 @@ +# uwb安装说明： + +地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： + +![uwb安装说明](./image/uwb安装说明.png) + +注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 +小车半径：a +标签高度：h +BC标签距离d \ No newline at end of file