From 8bc0cf6dc6b315ec6a516eab025a69a88190f3c6 Mon Sep 17 00:00:00 2001 From: MikuSamaLOVEReady <543664432@qq.com> Date: Tue, 9 Apr 2024 10:18:22 +0000 Subject: [PATCH] =?UTF-8?q?4.9=20=E8=8D=89=E5=9D=AA=E9=AA=8C=E8=AF=81?= =?UTF-8?q?=E5=85=A8=E8=A6=86=E7=9B=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CMakeLists.txt | 2 +- .../include/Mat.h | 0 .../include/align.h | 0 .../include/lighthouse.h | 0 .../include/mapping.h | 0 .../include/senddata.h | 0 .../include/type.h | 0 .../include/uwb.h | 0 .../package.xml | 6 +- .../src/Mat.cpp | 0 .../src/align.cpp | 0 .../src/lighthouse.cpp | 0 .../src/main.cpp | 0 .../src/mapping.cpp | 0 .../src/senddata.cpp | 0 .../src/uwb.cpp | 0 .../ros/data/data_2024-04-09_08-06-34.txt | 226 ++++++++++++++++++ .../room_exploration_action_server.h | 64 ++++- .../src/room_exploration_action_client.cpp | 3 + .../src/room_exploration_action_server.cpp | 80 +++++-- .../ros/test/room_exploration_client.launch | 8 +- .../pibot_ros/ros_ws/src/map/2_3.png | Bin 0 -> 1100 bytes .../pibot_ros/ros_ws/src/map/2_4.png | Bin 0 -> 1172 bytes .../pibot_ros/ros_ws/src/map/3_2.png | Bin 0 -> 1027 bytes .../pibot_ros/ros_ws/src/map/400_600.png | Bin 0 -> 4932 bytes .../pibot_ros/ros_ws/src/map/40_.png | Bin 0 -> 1531 bytes .../pibot_ros/ros_ws/src/map/4_2.png | Bin 0 -> 1001 bytes .../pibot_ros/ros_ws/src/map/5_3.png | Bin 0 -> 1061 bytes .../pibot_ros/ros_ws/src/map/600_.png | Bin 0 -> 4781 bytes .../pibot_ros/ros_ws/src/map/800.png | Bin 0 -> 5608 bytes .../launch/bringup_with_imu.launch | 2 +- .../src/pibot_bringup/scripts/odom_ekf.py | 4 +- .../rviz/view_navigation.rviz | 10 +- .../ros_ws/src/upbot_location/include/Mat.h | 55 +++-- .../ros_ws/src/upbot_location/include/uwb.h | 9 +- .../ros_ws/src/upbot_location/map/2.png | Bin 0 -> 44760 bytes .../src/upbot_location/map/output_image.png | Bin 0 -> 46982 bytes .../ros_ws/src/upbot_location/src/Mat.cpp | 30 +-- .../ros_ws/src/upbot_location/src/align.cpp | 52 ++-- .../src/upbot_location/src/senddata.cpp | 27 +-- .../ros_ws/src/upbot_location/src/uwb.cpp | 19 +- .../ros_ws/src/upbot_vision/CMakeLists.txt | 13 +- .../include/upbot_vision/ranging.h | 53 ++-- .../include/upbot_vision/ultrasonic.h | 24 -- .../ros_ws/src/upbot_vision/src/detection.cc | 2 +- .../ros_ws/src/upbot_vision/src/main.cc | 35 +-- .../ros_ws/src/upbot_vision/src/ranging.cc | 14 +- .../ros_ws/src/upbot_vision/src/ultrasonic.cc | 46 ---- 48 files changed, 528 insertions(+), 256 deletions(-) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/CMakeLists.txt (93%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/Mat.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/align.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/lighthouse.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/mapping.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/senddata.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/type.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/include/uwb.h (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/package.xml (92%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/Mat.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/align.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/lighthouse.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/main.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/mapping.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/senddata.cpp (100%) rename Code/MowingRobot/pibot_ros/ros_ws/src/{upbot_following => FollowingCar}/src/uwb.cpp (100%) create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/data/data_2024-04-09_08-06-34.txt create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/2_3.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/2_4.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/3_2.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/400_600.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/40_.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/4_2.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/5_3.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/600_.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/800.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/2.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/output_image.png delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h delete mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc 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-----"< --> - - + - + + 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 0000000000000000000000000000000000000000..c04969c29aa05a58d6e6c1090d38fc7fbed3ffb9 GIT binary patch literal 1100 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LWDJzX3_Dj44$bmTqYAaG__Z#e4t& literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..aa3a2dd9d531702c0f2e1c52933ae2b0b09194bf GIT binary patch literal 1172 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LWhJzX3_Dj44$JjlynAaH0y z%KwG8Mc;7>ZSxVCsbLhDvgm}%#7-q)SGBHD0gA&zr0R}`yyll36YYXt6e`y)x$xNS zm&d~6yL3j|%nh6O!`ppKw_|empluF}-HbWbG$C{pCfi ub3T6bTQ|Yj?%qk!p{ST1fjDZ%Bu)EYjhYifFWWo^(5ID49 z!~YA{MJpC*l}-4vSK(!~}GeyuC1KLD(oc z8UmvsFd71*Aut*OqaiRF0;3@?8UmvsFd71*Au!THV04^ebew?};|#dc)jejV*D>r* Tw>~fc4Uc)c`njxgN@xNA(jW$% literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..5fefeec88bc277cfa9abd64ae06e93da127f1357 GIT binary patch literal 1531 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LU&dAc};R4~3hXvoOGz;SrP zwfz|z4%qdn2CetltMs+&iuQ122uZ*43K}Iw!(cQW4C`z#T+1)Gzqn8CJULOwf3^Xr ORQGiCb6Mw<&;$UonNxfK literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..78f86b21a222813b0ecf3912c56390e2db577fe1 GIT binary patch literal 1001 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#Ns0c)B=-R4~3hXvoN5Akbp) zL4TL&N9Mz-Mw+9L9w8v~eTQo9Fd*m=R-<+pY09_PYfijydhTja9`{JzX3_Dj44$+{nvdz;oE( z=l{i*xoy;(6^%T8#ZCCKSK(!~ zjnSfVw0;<^6i1s5qwS^9M&fA8c(ng8+G!f?DUNoJM+Xr`M^1*>aM9>^B6%aJ?-|XM VRm9XQ>(W3&xSp - + 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 87b9862..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/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/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/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/map/2.png b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/2.png new file mode 100644 index 0000000000000000000000000000000000000000..6c7d1fcb31f4d1467bb9e37f97485677f77697cf GIT binary patch literal 44760 zcmeHw3s{q9mcO@(g$e{hiIsvT3JFx1)OCQ0Ttk3>kOT`9Da@p`?#Na)bRD5Z@J9I( zQ;{*PwIVl(*pbVyT?VWab~?$nO?Py6r;9UNh5diiJnr)_qZ|5A(IWhR=K};4trwh` zt;{nr;mdbE(BG>27{^PHtv;R55bzaTWBel9+eJP>;v~bP(-_?BV?D5m%zlbh*+W(KrAD23Q zZx1baqw+!l_ut-+$3INI-ENsyelYxfVo-=Ta0m_%2WK4{>c5|WeXK+fVv zf*_5}s_TOW?kOQTi4L^R(9Rj!dB6UChIUSClJQ-x@BT8ONq3#0oe!P~O>(F+wDUfb z^GT4Lp`FtT$r;)?osgWNo%gF3P4bcxkes2N7yeq*&aiL(Taeb^udn@G*4IA&RDL{8 zE1UZt-+tEm`~S-d^aLG!^*a6vCsfeh3H-H9sG#?9>Z7WH{_=X1?woLmxf68s%ZY$~ zFQ-1DbaYxFISJ^GC>@3j*5AG!=AUVnRKB_9{eFp9+ zAvuW-{JB-opwP|w@m_$xY@L6i5PzRRd`fsuq6Uv@A^u)?<#)kz5?h_2pwk-fXDH~j z#`_rxI-T);hJv1%&UimVK`%^ajGv*PUr%R@|8rB&8QS@<`bfUac%|#-fA)?vj>r!S zR(wD`wDpfu(`;Dt#&5!N4pGBruRAr83|-84h1x^QJhhu1_Z%&LJka7o-fBWpW&wox`Jy)LQcxZ9m=-2L?-0IW)U$1=A^%L$PK3qmsn3Z$* z_Xq#!Ux#Zpa!)yXzZt-=#wE*DtuoG=@B3}`KfLKM_iXkXN|ox=&o$MlPkf=@nITmM z%-?K@XcQdxnA>`92hGe*!NlbNWK}&Z=-;+4unybcq^(AwhVen;ZbV>g|8iLhX z2LyqV?u?ulKhw8%XKaiAO6(>&ZS>PSN#_{bK4a2!yfjzQY4ZYCQAELUJYsc=rg2~c z!|U_p?PNr@y8Nhuromt-`?bW=l9AI`Iywy5KA}{tKP8+~F3>tzW9a$+jQf`fkz9_S(kOhSu&6!!^s~6_OHx*2m2BF}zKhc?_>vtuJ;H z;U;J4=?OYKUFEFriZBGH#62O#@n+uf`&iKLob_2!HqJ2XZ6;rLH@I0M-yzT*F*qo|ae!*KhT~ zVxM=AYL>`{7SS@fFudY-)l7u>^+bblFB>8SOp6u-bZ7J`41-=8bJbG$kfX+&U}Vc& zMNJ}k=&=eu@Og;vx$R$pkE@T9joE|5me&JJN@qIq0s*Qw&d@$rWB!xaa&>{N8V9OA z#&(_6Z)I%~|HFun_}G~26qOt8DCC&sKFh&S%lz@A&X(_oZa9n8!p zX2u5#wM$^%A+Tp@%=J9caExK_Zxk(IafWv#BcCx?PhS}MMc9e2yzpIt{R-!*DjA*+ zeh-ndiC7VMoRpQc^a{=&>=S4mjBE@X*UQ$Cmq)=;Mb6~v3GU*}8uK=#ey-F_#ADL4 zVI|Bbco*AH-WGNRrIV05FM{>l2^xP}sL<%o%-w+LSYp#IIme_eh z9y1H0lCs%Y_oZ`qXE{$X0Km;wv(S@ zQ<&L;_Y0GUNa!`s)4{@@a%@83o84dFsrY;v;5f6+r<3bQ1t+ zm=xPncDk$sbuJA6ki! zJ@~g7?@3vml+VD+jr%u3g=5TYen*gVm0WIqFPTFD)TwH}s^&oLuL{FFuf`Y7m+5 zI*Uf9;G7XLtAcMG`JCCh3{K`Jmt`jDXLip2&i9zkX&s6`>evF1#p%7)eY{p z8W#MowcC@N08F0?D+RT|oY~-+ncynhq9om~;pHxiEqJS(?e9k1h2xqO=WasWZ5-Hg zdFav99qQ1U8zV0wz!&<7P@kV@m^IOBW2Bnjx6XKgZA;N# zFf&Bdu6klMLAdNsTKDQ(hpW^LmsR@gbs^#`vYfGN3&G*f?aNb%llgtk;Bf;RKg>D;Os^$J-BNYJ3yj zdQR=EUyUhTfkp461^XqTw8FQifr3{Z<@d#=B8mhMnJ@$o~6gL2x>L(Kc_sb(H7(mU~lC}JWB-j9KZ|XGem)@ZHcU&f;io) zRB)S+RBDW`30)7eaA?fbWpMrRcEi|jCH?V^HXXZs66>1J4^-WQV3Ac9x_$=|ga>Ai z;|>h-_J&YiIN#=Bxm2CW#|#$AYWQiG`7(9W`obSlyz=!`$I+sdfrt#8M-oQy$w^{K zQ)a2eCH=CRj^|stL8+n)8cNwI*nPqg9N9FPDdk}eAhb?e) zKJ}sN8=f&W?N&HB14EU_aU!o9N2*e z^P_Wsuz?&bn>+kkhb~Xu(7F%TZx0wvhLQ~VNGP9zzkG_}mBDDjjf^%GnY0VU z4SGeoV&KB#>&H3v)aEy7)o+Y1Q)_9KL5$ zL0THSJ^>ak^F+wjoTThaq-H^WDBJ^Abp}vw?L5-2)Gm>CfVLNa`E#dl$2q>_jlz$B zq|6qk4rL-^^8x~hG(Tpm#f9MOm*pYi?yCtL95B2M?xLb6LFExPv^YIW798NK%sFy@?%`#xpo#ACC7BneWI@+q+{WRv6gsBZ%*GkpEsuBv9 zFC{$Mr=_enUHB$0T;of;XEgD|he56JOaHJ^bwl$~d5417<#QVIg%oFfo13T!ler42 zNnbAPW?(yp1P$g0a`OeYMty)7R)`G1hX@>!->PIoexQ_Ht}&PD1fDu=s(55SuNkQn z7m$l)o5pTqlPy7Z;zqA#W;P*99-!DJwgmcWN)R6c{qZZzCsZWg(Z7+rG&nGgvj)=} z9x=Txc9qaUtp#uJLI~1j*xh*V3y!}HRgnXsKhN%V8&uYY`g-`?V0CyEV;Y>CT}YRB zL95Gix5yd=Fl)jj%*KUWEAm6+6H{8TuQl?v&%2E=-ePcs?O1buH5l$b5SQ`-{xwuoH1Nv8z z4IxV?R}aPF;EqpUpaej-Kvw8LBV-0Xpf`CBZxXmH?@8^lIlN8Q$fbBFv=Rs7$g=tT zZE(35$6{T&-I%97HyeJK;sXma!XO&LA?#iKh)nWvI0(EkvR%yE;}A9^poVV%?~FhT z&F4!DXSTvV2QPx`f3D)1&l~@{Yd^DVS;P;%9xgp?`_;$Q>A(Exw~-ACz-|k@U9#Wd+rFkm-PC1e#tz9K{%wqGDk+2Ya0 zWmtu-Qub9d(VZKII%+2L$@SkGvq_yJd<+ZUwSM4#1NcEWei5vtr?)-%w zQ>!OCld^Ae=B6=wjLJvEKu8d`8#C-ifi8Wi{G2i%2{D4SrO_`ld7=`T|30p^aND%} zj?By5)L#3Yv2sG2y2a93H56~05cDzXBN$u?Bob&va9%!1aBM-Yq8>ScMpB~c21T3UU8?coXLB0hY$V%+2}8c0lub4>2N@o8M>y_iL%t^<_P3CY zLd4GWz7RJRE!`rp+yO|S_3+76z(%$^Nvz37Gx^1_1~{EwczD3vpK#lQ6e|pKiOOaq=AU-4}nhK!&--twpAMESOBv1269a zO4Ztl{B3}NrK}Y~qG-@j3KxD>zkyrKg=VHqG=~?fF@LM$1sOF+?PGN5H62xv_nR_;fa!Vpk` z5FAl(DhfO}+#k;FO0uTH!y1WfIKT*z3!=pFd^yy!12|))8I=j+CjDjPCr^#N{nd?h znlq{g$tLEUQJ-Ek=okKfR!Y$)hc~0CC^HR%cJB^m>HcqbUEgd{5nvY zq>%DNS+)FJJUFZ7oImV+N1hUtPiB~9JH&3a?sc?zlpv!$tM5&KQOjV|MM1UVrY#tbJQft7bwAn|@~ z91KsJnKeg;RQ8pCQb#QmBPS5=6*$wDVSn4qTuvb`rx)1W0Et8cbEb~3G5?x}HI+!& zm6B%66B`NVSr#|F5%v}n%35xIn~l0|w1loN^w0@!O;3-eB5J06;pi%IHc~N-#8nh| zsBW66`jk0{NUe>g8^MW^=-7qwG@z+N+IlIKV3;6VS1)YCZ;n z2+{+O(+$CQ0jeaRgO^}}Icrzq(S68(@%?<{z3)Ri3|7Z2Az*nL>4n!wEPFZ61qTAd zr^1G-i0T^N3j2Rjder+XJTfB}Au!jE#syR+!akWc+*GA6202yfDLsoP`T)m+a5ePz zsQl5B+(g4~QPedE>^r%85jmeDWk>n>DqDs&0q?7@|H{acMH^XfhPN$|vl)14j1E5& z$vtioI>__*aNb=hs23Fl(OcEP1I3@Dq!A@>_~2Eh!F*7%8YBB_9nXo5YDm%M7%oy$ z=UuK4DLM zBC61t1XNq`vxvmtUMZVR{I)Uu0VXE_lXc@IOJeULPQp0pbWUjZ z=aJI8BP#Dnir|Jk)fB<=ZVBxK|K`LZKaiPbfbmSN7eAM*#*5#VhUCnQrUUjp_*hZ{ ze}kaZxHa1FE&|(_v*8RnWM6%}gN3G=T*uFbkt1*EdKwTbSDN0Js^vKs!f9IRmIv-7JWnQ%8Jv zCy1j*pnB@`ZOWYP&If6A{$ynLO4{__iYP@y6=b<)S!!XpX;5d>oS$uKrwVY`F3dA| z?QIE?&V%Z6i==KLdd~3IjjS_B zB`6PJt@DU_C>V+>)Tz;e_kgEyca-Gfo@XVU#1U-ai?q2$_7jzUs{SDqs@Wd_;WnB$E(+eHVNoB?1(1tPD!QZyts=SfRZMFG-2kzDkeOSC4A_+7M?o5&pOWnSEjqgo?x=j+Y1+iTfYP!XB zo;9wjxlvo&yZ*)@*@YfAmch@d6~uDR8V_*L)Abv#S5A%WJNGu)I{=czRn+XNuoT{q zsj4#rIxF+k9Z4rO%SI$wn0aK3@AbXD=XN`Mx9I* zrx_6I0hemrHVs8^6_j55Q^UJwEA?#<`_ZiNio)(=I|OXH0Grz(`9V|K36{uX2G<&M z=z@cJ%8ny>C)!ONFU2C*ErjGPO;9AUmY7kOBA{97Dq z=`oFn@iSkILeCB64gTP=tLO`cbiQ1pW;?Pc zSJE>&Ns#^@nlkE{p+M3O&yuqiBLiCLABLnl(x?ujDktC0{Wqbe!n&CnUR~vM*%kET7 zy80@b-rz{KJ#>el#O#%-5jCLbD3Tl3xb? z=YdNM&H*x_sd1y1a@u9`hOIG#rT6-c<;u`mru0k5REPIbaDa2RuLkBy!Au}#a9RLb z09Hfki#z!|n95#G)U4s>)^((5tG)PLmCs6?G1fqIsw^Z4oyu!#{|30e91HP+$ADb- zX*m`pllWTez`@Q!5`l5QYF>abN5F~1SYv9CT-(uS0-a+CmnV2qQbgL=B>>S6-0Bi1 zK)CDCme&C}B6^zZM~<*K!7I-M~MCgDpi;>Vcs) z7>N`NDh#b<>(J3t1Jx?H#o9q=jSl1@$o65Z87}LloBJT^ zfM*OqvU5W!drP8y3r#u5crDmD`!Q?$`yN9?ogWyHkHhY{9{zj8>MSl`crjSI>9DO z0Ju%M%Q_^Me;A2h>R>?vha5D4>(@kT+MNPOLxsTvFGa~mPsizw|-sVqAQc|Si8 zxI_h^<1LU;$T|aX-Ds>pnV0kpIvr%85?7=lpj@?qI{}niHRfoNzu*%LgGdTIC{-qS z${U=f!`+ zrq++8;V4xNiA;m(7)*k~UcgNc+Ji{;gKU7CBbZ%6U6d1s!}mnVhvwSqAgFhOAcmEq zZ8wY?N)M`Dz36XvT}iss0^VGGaXn{(=pyQNS*n4az6*9yJ?#NtC)C~ks8;FQXL zypa~&9?!7~?Yisw(R~J@V3j`L{=v#|E}E_-pw=h`B)}sg;R=5X6BR&4Zw1KCAPe%b zjr?x6=N`G+@|`>X5;lt~tdElrYo76sbS+EBF@kv1H#M1^?dT-!Y7!DP8Hrf?HF-X@3e zg?tbl>=Oz9&}(Ijp0sES4J#nn;@*cWtyZ774G#eXd%ah0>>0HJJsW=ayY zStE77A`W|DQup&N<0b${nV4j43nDu@rn@%)LD87h)+Z@{@c`?s4m}F;eDe9s6lY`s zOnLu&OMHlj_5EOR8Q+h98NyFie~O+*NRyD@$x~mJU6d)bm?y}ckj1Yy8QJZ0&+dh= z%03GwZG{PdEkUQp`}vRCiroT+u@MIOaQ;Fm`)l~+SOeVwgwMQ|_6dZ>z!_BKiM{cz zWq52(<0i^-J*_pt@0SnNLq`Fl03*cG&PEiw z$lo93h^-A45obLaeQ$%RH15tr?SsWiy$iLM6I?v%XK@|iy6_=t~UkOB=6j!22V8;Sk zhdXgUPj@OoBEr@)BeKr!1 zg_@P*vf-u?_ng#ejGsr-A6K-7~*k8S(z3Zf2GQh zPx~WK4qss!gv^cx6Wq)J^$mu1*~|pi!t^rweoZ5xZk+ne0B_)KZ3A@ z1=yYjmq8Wx&I&ZppmqVGhE=1aD>Pv#@R*+S5m#I&f&cYJzlF6`gI!9`K)z*isTma( zf_ee`&_#)bHcoAy#d#d_9%NX&KzQlC7pVEZcaOS(I+cV}!c9iU_E$-ms<|{(%_e?T z;z@B4tnLpu5fsEH1|tmxaqIiTf@y3Vy8-@VB)&wA(max3L`T3rQ(Qj>wSF!~;KC>S zreLhRhN9)5>s2pJ!VOWBXOqM6N@LSFo7!rMTswI;pmpt^!2&QnD4kSv7n$`kjL z3GC=FSq1c^qHutXE^W{v=Gc^>U;&kNUi!0$CmDUa2}qZc@B*kPR;uv~)W6`}hqa29 zN<OTkJo_>X?IY?(%f+l{@thQ~=VJ&8)Zx{QPR{0f zFMSRo5)w=;?zBN+FG0MjR97cD+6pX@pa>%LiZ6R8^)IdnJWc`OWI+9LtZQ%LSt78~ zWJ7!XElTY;#&qiOTX3om@ghmYLNPiHz$#+25CzT}r>F@ydL(m4!9BK7M`igGabf%s zQ>Vl?d2Ft~5xZ#IZIFG?E>VmY4-}k;aD5KZB}y_}3C5uU=@BPceL%5;CH0z*b}$}Q z>(JB0_0EE9tQn(hg_{5_2#N_vGuB3j@XwAh!wpzpOJo7ow?aSg0w2v6*o4>3HBK*v z4qsfFcLJDXQH;6_N`EOigx_XNp`Ht8Ya>laD^WJ6xZ>_+^d>1nkh}bpKn-7fHbs91 z3)2>VH=Td zYyukuCP9R?>o6W3IPO_AeyeG#;3hP<6wzLq-revFpdgSFhOjTO4TPu<8ym1ypc$$4 zwas|7P9G_I0{eL5|jy{RjFEWzO%Bazjh8~yU^5_M=D7qB##6A8JK9JAIvpY-`WJ&K(0+bidD>_RosQZ zO6qC5A6Rz~>Tb8Te0_X*~~hj={en!Uiiry&eWo4lTzcQ zM~5MSm>Ms~Re%>Dj)Ijs^8S#7{j8#k;$;7BPxMzv5mK-l)ky{ zND{$x?%6{#Eb^TgbMQt~=r~M1ZF_Udczr|EhapXYL!1(0_(B07C@WPJ$Zz2hHd7B5 z63`(PIce*{rIpWn!E1A>LzHC8jg3F32B3m^J!xm__=pyF<(F@Q|8-D?RECkoRd`6RY0~>C1T}}w?x~YFe&xhD76;Ye~Zo)Bxq=^C09i1 zQ11>#Tc00jts4ZZ25avA#2q+jKg!1jrE4#9^aUM8&x6zUQdbW+IGzj~R#)gArVm| z>42_H1`LUaLOq6xPCq~S5ZcV)328&MlzoA6*ExXZVBYvGev6=0>&59(4klDXG}p4w z!tTc|HRYo*$HHXb3PeCm%vrj3SK zh{H%E@P+}{T#gbMcKp)~#va`9Zl1W)14IZlnm#@8M)(j>XCVNp-}J{5N)K!=8-2j2zEuFKWktyDNUb&E-c~% zl|GbgW6jKJcTpHN_`~b^q28snzCT7bzSfni3yZ;%xT?`h&_R(5f=kx(`+&6pH#)J3 zHg$Wdk}QjnO0FY$DZLj>ZfJpk6YVBa)mTjJG+5|agyeH5XRil6KZ^?Nd*J0q1a>>w z##Z=XY)W|tfe8|7IQ=CHRJ@+RQD@6R)FTvdy_;Ne?e4!k1oIOKZ6RKteL5{ZglDrd?n23-@_oD4~wK~ZXNsVijlqHtZ zb3*t4{NfZnGE|c6Ov=i41WLF|bXEC#<2~ca)4;lhyeRHq!_#(74W0Uj{TtlqirVVU?UnJy16k?}dbjjJR=N+e4@> zT;Kb+RKD2~Z&I=~QHS=NI}oKSg&+xaFLo4EORL*&?b4V#8K1lW?+MBTwu|>$9~#q$ z6}kb|<1D60Qi?2icV6^yqzX6c$~WAW%O-A%@W}%hJ#$o{NwV}w>RHaqZRNZ5p}a76 z*$L+Wr(MtEj~4yLXY28!Zv_ipej@o#hZkW(1+<3s*l_{!`XZ*>&xEaw2icex^q#iG z`XFl%b~Hv^Ii9cTY|KAexL$k-$J8C-UZptud9h`0ze;=hN^3*#g^Dh9mcAu@tXG;{cESM_}kWDi{vGJc0xf) zp`VmRvilM?T5Lzb)Ep-%@-yM)$!47Sd)lAZg>4-{2v{AGPP9Sy*73XEQimD@+S4J$ zdp#~xe4UF2B|~j?#a(L)*B1hfn~d8mWuPs763eAJKF-mlW6*pvGvaMC4W6R^T`es7 zq3tLO&wwo{Xb&;6K`Au0XXwo?iG2A!*lt(zjO8p+<|s)I!@DpSb(UUvf#h_^g%5tg zR=ifk^s9$J!lIE_ASr&kzbXG2#JD1VsU`&{(s}MJIB$5q&6K|k;li5cfj>GlObz!~ z0!c~#vB6k$ChZs4w;NeV4tTl@h;P+EJb4&Ddc?W<@Z%n!;n#oIG5bfbK5x;nVr36M z_X~Ywqu`bw;bKedW(znC{V0r-AT7A%oT;vRz+mMalgPOxb>?^1OK|B^9~Z|#a(qVC~$9Rdtb z`)Mp87lxj(9%fgmdid7E8Z2N`LCVlG3VS(YFJbO1l_jc>uq18CyEj(-6wJo1cHE$&V2fMQ(| zRUk2z!MVobn#1fTc*O+ma5SNKq>z?+gm23Cma-z-i*^Sodx_}tZ&R{p`7V3_T%9JT z7pgJu?QaT3Uo9;Xxuw(1K+am+aA?ECt*$JB&janmeNMpF7w#q_2(N;zyjB@WO%QL0 zeLOCRXq?4i1lK}mYeG#2|3VDi*XJ}I8q&2#pe zf8Kxn1Zp1QCt92BoNy?$G)rDMKe9TNs|5i z&{~E4F^W`tYnw+Pu)A$~j)2vl$v{8EJ9&WZtZ~k*<4wI<(iGt)UUoqxT)WCoox=$t|4&5Ytf{QY0? zwKK*(W9&1=K4a`NS<$^a=^5WS<2z@3=Zx=s2)^@Qg|h#gHOu*>cv;D6uh|SOd literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..76abbc62a5d06fd5074884d1fcb6c2cf4f727de1 GIT binary patch literal 46982 zcmeHw4O~-qmbTw0Rv?g6TB#_A6QkhPZ+KE$m_Z>&uk-}~|S@vBzb%Q@gun6ySZa~0lTb0@AOqri$ zAi1CCf6jBB^PF=Nts*XJlH=o!BSwsv6cZh(95G__f6;&AN8y#m+*V=-UzbPxyVMc&@iPsA~KtNv|Va==mF{b!EtfsV;T+TX?{2ORavPXICdtvZA# zIlv|Rk9uOmPbUw-szZp916;EIsF&vbbEoot$0WKk{b3XLU0rej9Y~$>lORaFR&{yU z#C=zn96$$757W+J+WDY(KTJD^w8;2A=zFkAXuwg2Y3IXNLIVJGn07v3aX!E$hiT`K zy5unJ97>lQrkxL}7Y#tkewQ4kovl9>wKMpy|Nif<$bVyyY-_d`n0_ek|@cf(~iEAEux~n(v1x=uqbS zVG7zfl=*&`g0>E2jvuC=*M>63|6M5PKOXJueE!LI_yN)Hrvbyc{U0i~dv5)oZLhQC zGoE_t^PciKt1_Pvzk5Dl+e`aJqfe{LmOgTk&08T+_>Ws>3O(U|^vCj|DD}kYODj@O zTzS!ipZK-y-Q_3gh0~YLZdiJpfA93hLqB}8&HIcR+AA;gf9XD_Y6Q+dPTTe__ixzK zmp*m#?)HP9yO2k&(r3{0@5@cJZcVxPD{V z$xCOKsExEF-9i4BR)olPZ)gR%hwOVV!QMGxEA?1nzFRX@YwnP~*)f zjhZ6(BOd~7pVBc$NYuBUQ5!dF1z3|D0WDyZ&Zw#4H%yx*Yr;vVXbSs%p*l|2;UTdE zYXu=;x{k>b%cn-oeEWHByp4<5go{ucuV@4}LUkSE)W!u`!8G1&<~L%w<6ZKiarilE z<5-CWZgX?0+8Cu3T;@IN>`Hh`HN9UIIn7dG)W9Roz3HAFf-|_$-JNd6G2t9H+;Hr55ZlI*?RR^H zj`5OMZW%Qpi*y|Zqh_;6ZG22(!O}Jg^02Ar&#~zWtdEUexmdC7% zpT?Dn9r81ji?}3VE4ME^D^Z-UELO!iT9r0OYl7`jbC~TVxy@?S>`^+gleL8z3UPp6 zcZ@D_8r*Ls;n6b^%Qmc$E_1X&k(^kpYAz3~s;kZ{REpD*aIhfd%>0aFwUt7%A)INB zWw>cl%>C^l+L8J0BXR52GDQi~z@#O3h<)?h7wNm|bzJy+ ziQ=Ap?g|g_jE^gfxUlrHx@nTxLI+2!txOTeUS?{_IJq*3ZIuq%I!4yF*T;OY$7 zDlDFPj%kj1hV1vWbQ=(On>U*cmc1E@ReA2V2?(s8Nh}Dg`4N1KVyzIAPbN$i74gCM zm_$8O_9}w<<~ZIzq)aeY8W@aS+Rhp^H8W|!f+Jy5SW@ER9W2*nI!i3LqFbujq8c~v zWG@7D^>1-oBE~ew%f)ussL6Aez#&}oG}jZ<#$E7%Qg;LrVros4&Rru|6RCq6Zrp>F z>5NxW?uDi$~2?pAIkHkJvUAeR_GSYA8$do21NBo?e@4KQ_cBLXxA zC=}$^9IgY9pSV5=kbcXjOYNSsj;^h`jtdmcY{x zqzdwusf|g%)D1`*K=eHKlNdqD+3kD3F4mLX&mGdAYl@piVtQe#SW&M0^pL4 zEao`Ww8hR;VP0?RK}+EeVClf*^h#IXLU*m8%vDfER>^h!a;&R+lu4WMf{zTi;nC~G zy}WQZ-;T~Gc$?&i1=f%FJo8f9>kuuIvVu{s;0wLqI^lyF)!|hYA-j$)?oq9!E1(6&e z*G_RO8%seU$bFTF;fFYdR%^ReIe=lS)Vo})U$ZfamB|i|uf7-USA6*u*a15~pSMTNs zf_8$+Qg5~C=0^^>=k_a$-6;F= z0kKwX1n{1nAhO90U zNGWA5wIV%I2FFH}r9sXK!2p}iuzrmf)=y&T)-F4FZilj1Bfxd7Xgds_J+b*C_)#oz zvuIuIqY}$JA_3be6s}g$qN?eTal{;md1gr znD#R!WI@0-i04~`wjRQ^(X=^S^5mtLN5fll-$13tv^pAUD_8k}am)eaE(COpW|NS< z7wbBdYU3A5_Ik28_i4n(FOeE9otat(U*O3IjLU+88VTNe8ojXm4boi zFHg*;K)STT(`!hn!~H8Dwot}iBlb}O@HQZ{T}yFv@ovyW4^|i^%Job8>WjM+E>&2H zfh^>q>Pp_)*Eq{&wQ(|t_o6Xg>D*sw9IK zXC?A93TP@7Pt^)08l$Hpd!M*If!_{HKfq8d+!6(}IP50bTY-Hpt+=|D9P&_=2jMi3 z@`g;xB+mfS>;g5|JWVDCBD&hoqc*~6i}5l5`Gk_Km*FZ#MNW<63F#Pq;YS<56Y4d} zrYHD(3|*i}M8w1vY7G1Y7?^}u3R#$SoWH@;6Y;2?->+EZ=frKwCnQJDC1YaI=5i1!dl_ae7O z?U9@)E<*XC&pu%(Y>zj!aofYC;$(f@(agej*0Q&~oc}XOBxWZiEhEj_;ZOKvS2 z5}~V|Cd-~9u{6My7U@P9GEw-GM+cM(TjH2=s5TbyXd18R-nVQz>Y*HsDD{iUAZgeG zR|}4Qog=c*@fRTXR;E!{>MeUU18E%S5ZYTVqY&Q>_v6b|Ik5pC-`P~T%@>u}o&^8`R7LU%V^KHi;}cpa+!5CZq~6mtwH(KvI3C33o?fv<+e(znCy3@P=* zH_w??L>GzjJT zAlx+60+|0Cl{RN%07tMWv?YNia+)i#rb|yMa6FMI4?$BYyjs;2FRP0~C~mhDDM2-n zil@n5Z9f~Hbp+19IE*#NkoZvu+IafF&?w3d=CE|xcPE0uS!5@-&k~xo5O;T5$G34$ z%XP+DZkgD&H(Ywk;$!ZKeMHRd`6Rlt)R?I#R!wg{rkH7h&Ltz=se8|}nPJfz5+0rL zhO{SaZH<5Yn1vU-e8C{Xq{1|BS(CFt;gs)gOO}J+Uxxndf+`RM zJs#q9sBw`lQ00XC*4$7uP>8K)D>{V;lgMooynH{7Wv@SO=!oSkWx`VA!8LYLk78wt z7@?yEXLa?)Uj=3SsE4WcIB$l4Dq?W^ir*#A<)fx;|~1&8UzAg;Lc`WnEMsyvAP|u5T#=D z9xM#f0pf;2XQ#LTMsgr}S6B47;POmLNFWrYWkP_B;7kN_?u`ubOf?Kn@-{MmOZ-T0 ziPuz35IX__nUe|BltbkefE5xO0uxvW2@Ra(KgoXVcK7sD&a#tPYeJz0E=(%-R)Ez= z6!%n;Mak1~gfjx~jR)v@-P7lA;W{B!Ie4+Iwz61wb-FBD<8H%!`{wI1XULky8#-iw zFH+Xe$jk1Z5`IVSnPX3b4p1EL7Tff8klsU1?*#}7?X5VhKv8d8)RVLM0y29;+?NZ_ zzum8^@9kC8bK!#|r*dI$2?RFUZSY`QBP(Ib+y}|x-;qLrp&MU?L)XBgwBsY2rb3_M z5CmP1%G1k*o>=+A8RD;ZD4S**I;NYK+lon1MG3N^Z3+oHD56QmL{?md>=eL}?C+p9 zZcwq?!$%2PWX#ujAaL`60wIf~LI}rDqo{8LF*P_+N;X0MbH+ORfF1=UK^Q=7Gsn;o zZyxK7pb3XRf(;==fE9*7u_$V&VKn%*G;Rle<-! z&X;B2B|M5iuGNw&x;~o{e z(jbJ|TDaB=4G)UerCLE+g$LlUvNoL{0U53u+AIXVHN0BvW_{0MzfqF2o+Vv*9%DLz z#ks5v0yR8+X$hGfCPpc+#%r9ghTm-Jr4L1SHIhhTLQ9v!1 zJPi~mL{l(`{YGuLtZ9~^HVXzooja|ar1!A|1UMfWLQyEMjjviJNR-&KHU$MIr|@gQ znIf~w6(w@rTA&K<5&#uv#L~v&_X&o~a7Gd8$N=bF47HER)BnI(sa(y+- zTqR7?dpH93rG^fGM4+m{(W(%qC3bo4uT>P&4ssh(^`Fp&AxIu8x4B9zRd_vK#e&Qo z5u2V+f;Z;tMo>d?^`W6j+uu?fjO=8JG-Zi@qGpSqlV2jP;NS`@>pD{;( zLuP9v=9?eDL4e;qF;ZbY`U0v?#?N(Sr&J0k;RCm1UBG^ z2hoB_N_GKhF#!~qR}-8}HRZy|MokNhi&L=R+u%q*{zN)TYU84q`A88#3EFaG=BFu(ivP(^iV%0B!K0v$<+xE0%mW zV?qnfYpEs_*j1xqr&M{c^Wg(!#iIJ%s1?qUBgj*sRouz9dhNf(5FO8m~t)CB#QdX+j5DeTmqFR-%ZcrDj~j zjfGO?9M>LsGTA_?CH1jNP~YLw_PN`>|e9%9lu)6!_$g3h)by_ z&H$KBpqzngUDgX*WLXL6N4lyk6RgLVwGjBN`H(zej=+B-_qae#wYPl=0 z!rF|b$I+W}Aaq3zfIxS4vU!u~rc7Pw!5*rpD^~;`M`N-TizgMD&kKQXJr&uZ`c-~$ zoU?di0gB@^y)b}RZGn{J5!^fKR{{v9Iv9w-(BrCWsUcoc817^uKSp|i;B>sEve##B zKo4+|(-ct)=)#$B`UA1J8F5z%_T?s_gOieR11{%a==jK}NkhmTLh%9j<}A-}mM!R8 zxBZE;&@f6hod4@LGsIlP$sCQc>nW-%py*UbFx^ftOH#R=Xks!dXC&%G)6EGeF=J3p zu=Qc&?ad28N27GhZY*M)$W!GH=czU?*xB^moww5#AID|4SO2JpE+%Ra;`KUGD7Y!P z*v`beH*FNNMc+A(W(Wjmq3TEHtv?1H0zB85$c%2}ZE>(#C^);^c7-whQEXB=nO^oa zxa5s@$#Stf*3DPQGZt7C+4FnX1TvK;nrfi>ZIe*vpy;y&5O4cnpD6X#M)}EL5c+1+ zk%yGUVv}dt>y*7An{AFkuj?S>xDzxX)gs5#$ySX3%JdB?36-o{uUn`WbLtOJVB@X@ z&XoV{LUw&g#<68BSml;TCRC?jArXDOOS!5*c|1wH)^xT^=r{ptHA+WR=BeoI+?u`g z`1L4|lC#LxkiU@vRx-F8inz8XQB0K1P|sM3;gRu?@IC}ww-8;En{r4N`M^E}yiY1z zt2Uk&n_dnu(BMarz#CSee)7vx{4d>9VJna>v>0oj0~27-bSU#cu=wx*T62J(XlXuXT7iRLf{cszy|MlNQ5+7l7B}z_*UD~hPtJe5 zAbL@ERUjG!zMN$u1YT5G!Ldv=+QOsEJK60SOS1uGm|pmX=d4imdwg8Lby0FH0AW5u zRhX>Fm86zzO@=bNSlQ)4PSGejvyi#;N5=H23e_wR;K=CL&!gV_6eZmuUkFV~IPL zPl2ccO(zGsw?AXrgGx&>6+10JyRvN&;wkpe36Rid$ht6}^fAsQHjT9^g7m^0v2+7! z*coXOE3jGR><&>vtT4FQcNdAHC|I}a_I zVv*eT3~^^?xICQ>zg^qr+NTS#<4&Z8;=<-*!&A;*t|x;*7&xIff)$fHHP*wRY6jqWFO)m3ty+^oqEka6>?M!D2dAM}ZmjyBlGsJy<+eaH5`Zc0K=G&3qKK zn;8>EORi$5;&jeL_y*YYc+T3vZTkzDvPdH3ViqK~k^*xMH5dq+2-Y^6F=0@!IpWV@#<`TzZsvaV;mB~WD+fk= z`1@M{&r}6pe?bvA&r$r}yEN1ZQyVAXs9HfIB(LeRIz*_qY}SSLt;*PD$$erQ9|Gqe#$j zI%Z)#gHa=iO$&^g6i7EQ%v6Bo0qdD})XNWSO_*oe40V&nQ`BYa*<9s3Q<{%#5oc*d zM*%YN5g{DqJyg_%%2^jqWgie-IMq~m3P}(I&+rv{gE^%3m357kpM*W*#OP=@ZFf(% zZ_CoQhmrKgN7kXoP%bnd3G|V9h~G!Uu45yG3c8;i2@ns<*RyToqMk13n+4DcF0_hG zr)FKKS_T*L79T?XBu9A*z2>jj`Cyzcnb55Wf%Ma~2mx0$UDkyn2@OVEz^n_Smcsi2 z(KX4@!v7_&58? znzWyU8BfjHl7d^ZLT4+z&+iU@TOxj zCL5|84GR35XF?OO;wu(Slqm*8)$fg(DPm7*QaAf#xX_sPtP2^=bfdp8A91Il(oA&b zF>q9MOh8B5<3)|;It9Bm;S)`4{yYG?iE5okglbOYS0v}7zy*bUuI^Ss{^Z+!ZCQ(2*;lyAKA+R1B{x< zeRNRZKU?XatV*UTE9K{=RC5oqp4lh7nBgic#VgXob%6^1*?e;br}EIOuk7m(rPD|Q znk>mG6khN2a00$jB0Ll2Ha>PHGxu=u$qoSsaoVO~JHiFfKGa%!XvTN7t-P_Y4sCrENn89u|TB<9re z5l0NhDgAK~PShTq4n>c`fPSn-wn1q6;Pt?Y^{gk7A-c){n#o9p0I~UoT9EBk=mNc= z$o9k$UIa-%{}Imr+;RNaiu8j=7}~^#EXZo3(F7Ko;xTSAU7}>6{T>7uR0wZ?%mWX0 zpaXW{RFDoR7c@FWr8a(vqF65s*`s`f9?@Vn0+b#jDfxZN1(Z2@VU!nHYBgDEKBm;U z)wad7QpvCymH>7LRI({$!V48Z$RZ_sn4oMld6g>RB~ucQARs-Xkr6gg%Jevf3Neg- z?1!moK!1m3a1k1)7cTg&TJ7*DCix+|A>Y6I+emt?(_~+9uvep;sYa#cC2Pvh_*EvZ z7_137WC8A%0sJnj)(g$m9hj6{ejtjH<=P|b*#JD}5yb@KAuE@;dwYaatgEyXhXrV6 zptC_UK@6g$9Z6VhDy>>i8O4kraD4C@M4spd9C8tZQKTY(_Q`}ek$IsB3H56^W$!NZ z9x^7SB+WuRAflb0M?{BcPWewo;q@lWlh7>YF6%TZxh>nMSrf$^=&2Ge!2AWd&8Z-c zTyc`b^0-|873<*U{gPb2UcP9atIH|>W(5nDe&JIRYsOlS0ukB+hCb` zt6?mwjck4Xq!;C!DkQTeABOg+VY{zC|%T=HGl+LwQSm;&T~ zz_tp>BTT;&OY~a1EM1;e@flbi?q-U$Je+~iMYOoBj*9RG(E{r( z3(|=kJ|Jr4)Sdr<#5L=}mq4wM!c+bYSk|n^@lb|0o>OfJXS&f8!4Uo|3=~l6C4h5) z1BSr0XJe7uf5wr@a9PFHM=}Sj$a9k<@P`HD56^(H!G2aLpxK|_CL{vepc{=@+2seW z;EeUJ;psG-&wM0PDW}HEsq8nEn92eR-6Oite#pMeISV9~|Hqi-f_7FSXb{W?QnEZ? z1qcwh<&)VWq=jTqGz|B>WIoaXJ#7xW9=MO-+KuD*!x$i}zKB){PUR)H6^Ts|h>mZF zPDY&!a-b<$ob)$vb`!A_gvvMQ(8n<)`N}RIxy=)qH}1Woia2<|H*6bo#{U4O4Db*s z?p1JiefWp3DPc@2x7yCP?(NX-M_n*+cH86A#E|#~Ndb7UPFRIIVuY`U&jUFF72%oG zpz-Ni5U_i+Ld1uV&=4N?c>9jCebO)-_U`P^!?yexUjD<#A}%Z zn}V_^KGCF*B$Hj)0>SX25-= zNlk}|&JLTVvt^4|L{0$Lx>&qsY zGzr-kd^W*02DQI(R%w-oD0dH~;%VR{8S?jz5*174>tBp5&&$t96j!mkGIoXUf@^&; z#WWXhO_1Bpi(fEipp@InSvYsx3=XiINKL%C60TxCrmm=~Q8bHRGS7wbKqLB?vuc5+fZTD zfQjmsZPW{MweZ!HEEg&&VT}Wj9YEJ~r#7^oXr=$Fk&AO^!(K5gcDiOz1vr5%S zQ=W(k>o;Ozn!7oRGpcKFL;hmwi%$7hPsi|Wcee})Sx z7tA3`BOar`-1YpFq^3xwn#yCu)ses|0*exiBBJvN?RpO-^^R$f*}al9nML%VEniY0 zCfs1~tT8aS7jC)+idU4>4;D}xQRx{7Qs$Yixlvid*J_X@vK`2jkQQ~BD1;DB;hpjA z)tbq0d6d3&u>_I)eG@M|pTOqznm<)5s4qhD#b$b;11KWIbiOisgGMRUMm!9PnR*W3yXrn8d1di8d41I7*9itC8U7kVaO4F z;wwgJh|mX)(~}eRnpIe|3#Ym~=%gkjH6IG5JPoSjV!qU_r)TqPkjjWdRx5%l6^^Kr zqVSXt7!y`|723wogi(4R>*PQ!=gDZY8@c{2cgk7Yu~u-ptS4P5yC+c1enG3HdW9lB z6E3RMKpVpm>N$74a0`EcucLICOZh8atEEB&#R~3oYXTcfLv9f0AzD)~BGFSUmi~?k zwn8N!sbo7QIRFMgm|ak_+|%(`pev#V*s*Fcno<;$7+CB;-+_Nt1iA^t0Z;xffC(lh zQL2Ywo)cE#RY|B9DrDfry3A4XbW9=P`EFVkjLG7o5paKaL>OWRBn=k?w^4(=Ax9t> z(Knb(cff$SCja~>?>cN9tn1{f}w#1SEj7wI(H`$Om zgHPqK+Y@BBEE);Qbmz~B(iAceWEqSAL}4la$dd;EA$WTLsmP;w!Rbc1a7`R)u2Wr) zV~I0h$K6=vMZ%{c9U<_c7e9heIf3TrM#2iv0N0G4r4jT&1GujEaOT{BZu-b0#n|$N zQNLc-y}i0A`cMC}W5XZ(a{lMIDI&e0Gx~U7YaKc!dkn(d07Hi!2Q+GW?4Lk``Wz1p zcV)|MZj7@@+PWQV`63&3;vs*;!?``znIQeGGsQZN7vVVyjQ~H6;*o&Dc16%pdgaa^ zBTcFNsd1C^K?U@C<}N56y~p7XhR3{XP^q0(BjekDgfq!~zZ;ef!KDwncUZ2ggj3^F zgY2Ky1Az!r9f$Do0fCtAbhoA9KHVf1d}avr=$x%;BRXdi%lmkq8iR+ATJ?dg*+EN- zH|4A}@3UZD8{MFDT4mP<90c>xc#;+E<#Z@p+u9X%A^gVC*!UcpP1Rh1bT;nG7i${=xoC=Cfdzg^L_ zinnt2uJ+}&y?8dU{a2Q-teuvL=E;ueC1SF6oZ8qa#e-Oz&K(l97GJFhV$PwEE@H7j zD{+;L<-&SWPi!@~gdjBbCz>#@nuejjDD1e2clGS~IBiQLtbw4NVl`M6bCxeOf*u8% z#59Tmm{?m`*IVJ8XVn7lZhtGH28gi*tAL@^f~ssRRKuR1BVt07=M-Jb%xm-8AH}Du z#Iw1E?A7`pVJTB8GDq0_1f$tCh;|IEZ&zjmI|0KHZr;IW z%8?o}%j*)m#-X|@6XGKe>hhFzKy7rp&!brz$BdI$vV-9Jh+xLQq4PAv6wZ)|3Jq=e z^NJE-BRZ@|v-Hp){=WqxOca{m zaea#X2Y<*@-RyBrcJ#p8tL3a^vxtH*nBIdOwH>*A~E^7(ULS%AF zVYr@OVe2J$3(sODl=ydts*TdttIwOHD3lak?{fGIAQkwxwEhWzr&MSdf`4L z4&DQ*V~x>gFO`3?76Ur`POvtF?7;93FF`O##z*SsJnr2Z3rehcVSFSZ6fR3g{xFt!qzNino*p zG2M&gwt$P84mU*RVy47?w5<50#n<8h5%Ur4WkEtvT>Q+%_#d4s9=?mr-Mvh1Te>c? z>xoDT$H6aSG90i(=jrko_5FA@cs)*v@5FaJOg!hbWv-W!0P5SwirhMuM3Vu0r=R)U z&u@7p=L`F00Ur8b_dEb1^glRD{1K!-lm*DLi1LAkf1WX=#UOtJM=+R*37#_H8dN&u z5>7Dnc_kad5;LJxd;@bxAsTl?ers{*Z{b_}~I|2CoB&~F&d;omsvi*KalWS;+fz_4IR#yv_?9Ir0oRw!+ae+^oy=)kGEnFCj5ZtEX9X7 z+?k`1lri88?|EENho_|NZ&Rk8jzYVaBVeeDo}jw@_mA&4SQ(EEK94+%2W^9U<2Fy! zn(b@o_!rI+f;n@HGMG`MV3E;mm$t+(!5+ME!F2=5ur_#OQ}eFdyWo+1A4A8dJfFoT za~&VE@B6}leG4pGd}Li{=iWXloyHi#Y?&euMx5;B*&#v1*bQO=#_EjMFjEuo=Oy!7v-QgOP zJ>WXda$EhK-hfJ|BQi@IjPAnV1O2G}MD$OU+tTs54G43Hh+%+PM1=I}r@WoJ{sT$_ zxH%@hUX!$=S;b+0^<_~(1v&xU1hrX)@N~u`6|{E3FcF@6Y7<0DbiT)t$FT+XW}W` z(Wtn=uO@b6IRkV&XB)jk0Yz z>7smm1|vSv+iQu+!nDX5SgO|>isvKm!;NzNtuZ!W1i1mkZdyZ!-dz@FDYK6Rq2k1W zvYp{u^W?TF5dFONe9M;t`{6g)m)jZh=m7O`a@#9VTjK24p7s$O`Jl2mi7`3Qq$rO~ zAlLAKz#65U6%=2cdEW^@Zo@49V5Y$TX&Y-8ac8^}SHNr%Jq2**Tlb4Xf5QXtP>`Oi zBwog+#cxo3T-6I(ee6W5Q-CGeikQyA1Fz^-@cA4HMrWLy->Wy7#;@gJy&={d0sV+J zlJJcI?-DzpE!Hy~%@ zL2@U8dm*>BM|+RbjO~ z%b`<2nCExvQUbI;!XZ5&AuFb$Dh!4UgQX4j@NdZSMcKIk>Px*RBw#%M zO(*KYGqu;MQ3qcJ7~rXY{tN+t?T~`@8tl*Kg5qmh6uEQkPSW{Dco)A}S2J22f15br zb-%yOp@%Mzu|#vw_Zn&U?$$3*({;||KdP9wPm2-8gHpmTKr@hx7&O_uxE_g(w_tG# z32ac#9bY-d!TV$1C~A&2jIZY)Kh(=rwH4 z&#(-)Alae;${VWynd%yf*;EjsURVo3N!&G$l_#o()G8~^XqTTA#a3F(f)rd>(BUl~$TANSd{?3EY3%HrRhJm3oe2ONC( z8`i%6t5pVl>0i!{+l%BtUnh8}*ZQ5Yu&QTS%3tjZ*foCGvb<$`F7_En_jYUNzpZ$Y z_uQaHXV+d!1@T{dwdPlSmTQ7fNByGL!-H~P#a{Kl{=MD9OG1)|zhw2EU%J-0>$Yo7 z8VnkCn}7coE6w^L>}&16^+9jWGsCRvhghuNxSh;x?o^u451j)%clfIqJKyb3(F4A9e!$my z-v^iue*<{-%Dej6fTD8{?r!L-S6hcmRHUpyt~L<)1{uv>WCnddqrdA?{E(1ev*N`b z<=;^lxw!Al#t|bX3t}R}5{Fi)8NaoA>FqMKW=5|*l2tMksj3fnhnfC^f0=dB?Tpph zvcoQP!QD6fgfsuuBYo9o*VBXWq0ikOENcyar}ecT>33S+s(3bVo^IOsZPPC9&;38y zWBilWKJs&C(O(tM{bT%iuD9QkHgSRdH*f;J2OveoYVQU}TONI>w=eMS+kEF>Q^T43 z!}j06aqSHoEKuK}Qm6dG-2PB6)NLCKn{y~4VAz~P3L`^c&aPuAy(QU$kMXx{@X)B)j~K(|9D*b?Y);3m!`?ZhurzGWVRQa?liaX(4twXYcm70Z=y0(A f=!5-=^P3-CEBqf*(=SHge=!T=BCErHzU}`5%?QOD literal 0 HcmV?d00001 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; - //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作 + // Rotate.mat[0][0] = cos(yaw); // Rotate.mat[0][1] = -sin(yaw); // Rotate.mat[1][0] = sin(yaw); @@ -85,8 +85,7 @@ namespace uwb_slam{ uwbPos.mat[1][0] = uwb_->uwb_data_.y_; // uwbPos = uwbPos - Rotate * uwbStartPos; - //后期如UWB摆放在imu正上方即删除 - //卡尔曼更新过程 + predPos = F * syncPos + deltaPos; Z = H * uwbPos; P = F * P * (~F) + Q; @@ -95,32 +94,32 @@ namespace uwb_slam{ P = (I - Kg * H) * P; uwbDataRxTime = uwb_->uwb_data_.uwb_t_; } else { - syncPos = syncPos + deltaPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新 + 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 << " imu_odomPos:" << imu_odomPos.mat[0][0] << " " << imu_odomPos.mat[1][0]<< 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 <<","\ - // << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\ - // << 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_ << ","\ - // << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n"; + outfile << imuDataRxTime << "," << odomDataRxTime << "," << uwbDataRxTime <<","\ + << imu_odom_.imu_data_.a_[0] << "," << imu_odom_.imu_data_.a_[1] << "," << imu_odom_.imu_data_.a_[2] << ","\ + << 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_ << ","\ + << uwb_->d[0] << "," << uwb_->d[1] << "," << uwb_->d[2] << "\n"; } } - // outfile.close(); + outfile.close(); // std::cout<< "Data written to file." << std::endl; } @@ -156,15 +155,6 @@ namespace uwb_slam{ } - - - - - - - - - }; 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 e295b0a..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 @@ -1,41 +1,22 @@ - /**--------------------------------------------------------------------- - * Function : Senddata::publishOdometry - * Description : 发布UWB IMU 里程计融合的定位信息 一个发布给导航 一个发布给tftree 坐标变换使用 - * Date : 2024/3/19 linyuehang - *---------------------------------------------------------------------**/ - #include "senddata.h" namespace uwb_slam { void Senddata::Run(){ - // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 - ros::Rate loop_rate(10); - // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 - // 主题被设置为"uwb_odom"，队列大小为50 - position_pub_=nh_.advertise("odom",10); - // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到 - // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10 + ros::Rate loop_rate(100); + position_pub_=nh_.advertise("odom",10); while(ros::ok()){ - // 按照10Hz频率发布uwb信息 - publishOdometry(); - publishtf(); + // publishOdometry(); + publishtf(); ros::spinOnce(); - // 用于控制循环速率 loop_rate.sleep(); } } - - /**--------------------------------------------------------------------- - * Function : Senddata::publishOdometry - * Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问 - * Date : 2023/12/13 zhanli@review - *---------------------------------------------------------------------**/ void Senddata::publishOdometry() { 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 6d41146..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 @@ -45,14 +45,22 @@ namespace uwb_slam{ while(1){ this->Serread(); + // std::cout<<"s"<x<d[1], &tmpdata[5], sizeof(uint16_t)); memcpy(&this->d[2], &tmpdata[7], sizeof(uint16_t)); // std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl; - // 如果距离过大说明数据无效 if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) { return; } - // 修正车子和标签的高度差 - // d[i]是三维距离， 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)); } - // 多项式拟合，用于提高精度，注释之后不影响 - // 在不同的距离采集数据，然后拟合出一条曲线 - // 这个地方的参数纯粹和设备特性有关，和标签位置无关 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])\ -(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]); } - // 构造线性最小二乘求解位置 Mat AT=~A; uwbPos=(AT*A)%AT*b; this->uwb_data_.x_ = uwbPos.mat[0][0]; diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt index 95f81b0..f39ac6e 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenCV REQUIRED) find_package(Threads REQUIRED) -find_package(Boost REQUIRED COMPONENTS thread chrono) # generate_messages( @@ -42,13 +41,10 @@ add_library( include/upbot_vision/preprocess.h include/upbot_vision/ranging.h include/upbot_vision/Timer.h - include/upbot_vision/ultrasonic.h - src/detection.cc src/postprocess.cc src/preprocess.cc src/ranging.cc - src/ultrasonic.cc ) add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -57,14 +53,13 @@ target_link_libraries(head ) add_executable(main src/main.cc) -add_executable(ultrasonic src/ultrasonic.cc) +# add_executable(sub_dis src/sub_dis.cc) add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ultrasonic - ${Boost_LIBRARIES} Boost::thread Boost::chrono - ${catkin_LIBRARIES} - ) +# target_link_libraries(sub_dis +# ${catkin_LIBRARIES} +# ) target_link_libraries(main head ${catkin_LIBRARIES} 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/include/upbot_vision/ultrasonic.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h deleted file mode 100644 index 6ff8f5d..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h +++ /dev/null @@ -1,24 +0,0 @@ -#include -#include -#include -#include -#include - - -class UltrasonicNode { -private: - ros::NodeHandle nh; - ros::Publisher ultrasonic_pub; - boost::asio::io_service io; - boost::asio::serial_port serial; - -public: - UltrasonicNode() : nh("~"), serial(io), ultrasonic_pub(nh.advertise("ultrasonic", 1)) { - std::string port_name; - nh.param("port", port_name, "/dev/ttyUSB0"); - serial.open(port_name); - serial.set_option(boost::asio::serial_port_base::baud_rate(9600)); - } - void readUltrasonic(); -}; - \ No newline at end of file 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/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc deleted file mode 100644 index 02e946b..0000000 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc +++ /dev/null @@ -1,46 +0,0 @@ -#include "upbot_vision/ultrasonic.h" - - -void UltrasonicNode::readUltrasonic() { - try { - while (true) { - // 发送数据 - serial.write_some(boost::asio::buffer("\xA0", 1)); - // ROS_INFO("Data sent: 0xA0"); - - // 读取数据 - char data[3]; - boost::asio::read(serial, boost::asio::buffer(data, 3)); - sensor_msgs::Range range_msg; - range_msg.header.stamp = ros::Time::now(); - range_msg.header.frame_id = "ultrasonic_sensor"; - range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; - range_msg.field_of_view = M_PI; // Assuming 180 degrees - range_msg.min_range = 0.0; // Minimum range in meters - range_msg.max_range = 5.0; // Maximum range in meters - range_msg.range = ((data[0] << 16) + (data[1] << 8 ) + data[2]) / 1000.0; // Convert to meters - ultrasonic_pub.publish(range_msg); - ROS_INFO("Received ultrasonic data: %f mm", range_msg.range); - - // 延迟一段时间 - boost::this_thread::sleep_for(boost::chrono::milliseconds(100)); - } - } catch (std::exception& e) { - ROS_ERROR("Error reading ultrasonic data: %s", e.what()); - } -} - - -int main(int argc, char** argv) { - ros::init(argc, argv, "ultrasonic_node"); - UltrasonicNode ultrasonic_node; - ros::Rate rate(10); // 10 Hz - - while (ros::ok()) { - ultrasonic_node.readUltrasonic(); - ros::spinOnce(); - rate.sleep(); - } - - return 0; -}