From e73ef2a5a802ccebe8eccebdb5eff135315b86c1 Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Thu, 14 Mar 2024 08:48:40 +0000 Subject: [PATCH 1/6] ultrasonic --- .../ros_ws/src/upbot_vision/CMakeLists.txt | 13 ++++-- .../include/upbot_vision/ultrasonic.h | 24 ++++++++++ .../ros_ws/src/upbot_vision/src/ultrasonic.cc | 46 +++++++++++++++++++ 3 files changed, 79 insertions(+), 4 deletions(-) create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h create 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_vision/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt index f39ac6e..95f81b0 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,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(OpenCV REQUIRED) find_package(Threads REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread chrono) # generate_messages( @@ -41,10 +42,13 @@ 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}) @@ -53,13 +57,14 @@ target_link_libraries(head ) add_executable(main src/main.cc) -# add_executable(sub_dis src/sub_dis.cc) +add_executable(ultrasonic src/ultrasonic.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/ultrasonic.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h new file mode 100644 index 0000000..6ff8f5d --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h @@ -0,0 +1,24 @@ +#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/ultrasonic.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc new file mode 100644 index 0000000..02e946b --- /dev/null +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc @@ -0,0 +1,46 @@ +#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; +} -- 2.37.2.windows.2 From b0f5f708ebdf70b5203b7f53fd6a0c7c2f47f11f Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Mon, 18 Mar 2024 17:34:02 +0800 Subject: [PATCH 2/6] 3.18 --- .../ros_ws/src/upbot_location/src/align.cpp | 14 ++- .../ros_ws/src/upbot_location/src/uwb.cpp | 15 ++- Docs/UWB解算原理.md | 108 ++++++++++++++++++ Docs/image/UWB位置示意图.png | Bin 0 -> 33768 bytes Docs/image/UWB高度修正.png | Bin 0 -> 22677 bytes Docs/image/uwb安装说明.png | Bin 0 -> 14066 bytes Docs/割草机融合定位：.md | 9 ++ 7 files changed, 132 insertions(+), 14 deletions(-) create mode 100644 Docs/UWB解算原理.md create mode 100644 Docs/image/UWB位置示意图.png create mode 100644 Docs/image/UWB高度修正.png create mode 100644 Docs/image/uwb安装说明.png create mode 100644 Docs/割草机融合定位：.md diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp index 941d989..8cb47b9 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/src/align.cpp @@ -38,8 +38,8 @@ namespace uwb_slam{ Mat Kg(2, 2, 0); Mat Z(2, 2, 0); - uwbStartPos.mat[0][0]=9; - uwbStartPos.mat[1][0]=0; + uwbStartPos.mat[0][0]=9;//表示UWB设备相对于imu在x方向上有9cm的距离，后期如UWB摆放在imu正上方即删除 + uwbStartPos.mat[1][0]=0;//表示UWB设备相对于imu在y方向上有0cm的距离 Mat H = F; Mat I = F; bool imuStartFlag = 1; @@ -65,7 +65,8 @@ namespace uwb_slam{ std::cout<<"roll:"<uwb_data_.uwb_t_) { std::cout << "uwb received" << std::endl; - + //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作 Rotate.mat[0][0] = cos(roll); Rotate.mat[0][1] = -sin(roll); Rotate.mat[1][0] = sin(roll); @@ -86,7 +87,8 @@ namespace uwb_slam{ uwbPos.mat[1][0] = uwb_->uwb_data_.y_; uwbPos = uwbPos - Rotate * uwbStartPos; - + //后期如UWB摆放在imu正上方即删除 + //卡尔曼更新过程 predPos = F * syncPos + detPos; Z = H * uwbPos; P = F * P * (~F) + Q; @@ -95,7 +97,7 @@ namespace uwb_slam{ P = (I - Kg * H) * P; uwbDataRxTime = uwb_->uwb_data_.uwb_t_; } else { - syncPos = syncPos + detPos; + syncPos = syncPos + detPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新 } imuDataRxTime = imu_odom_.imu_data_.imu_t_; odomDataRxTime = odom_tmp_; 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..6d41146 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,15 +45,7 @@ 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; @@ -98,6 +96,7 @@ namespace uwb_slam{ -(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/Docs/UWB解算原理.md b/Docs/UWB解算原理.md new file mode 100644 index 0000000..ae176dd --- /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)); +} +``` + + + +#### 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的定位可以用下面公式描述, 其中$(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 0000000000000000000000000000000000000000..19a3be38d1c3fdef4d41b6bc0da6ab65999445ba GIT binary patch literal 33768 zcmXtfWmsEHuyESaLV@C1tayRo4h4!k6qn%c?ht5kch}%!$V6Fg$=yjBS;4|NZ!#`LT-__Qu=! zw)g?|!icxPWL5U8ApM_zFFUk7hhg;`1TzcRhXiOR?*IT~Z%_FIsC`|YMm9<65FeZ; zY_eZ^&REmAjHr7C6?dt=8P)y&j&y*JD@j!`$^6y!Y}Bk}{5OR(X;rui9spoUrdbZ> zjl^;MPTH5G!g)B6w~#j4&{3gm>!*hb0O&FCx@R{C+H71=62{VDN#11@o2ee8kS6}$ zY%XSC>2$I;uQCOi6gLHK50gmf@KGJjQUuk=q}18}t~}j}WAEWHb(xA!mqHjX5RHr` zQv$f=Q&3avY)1Ki>k|Tw(9RV=q7tt}W2Mdcgb+x>u~gU&rms@Ra9V6F7t{WiB>q-x zjvToiIdWr|61>FJ{{dP`Q=iIVn;iX>5(xhv`Tptk)Rg6n-8@ezW=lvZwpkMQNh+9U z+-b?>u5&4S{NI|atk^vH8>@2!g+Eg)2uLaZh>`I6R%~%_^MCs)VnSXiV7C@fB*T!D z2=(c4w3E_OUP??{$m7)TZ1w=Z_vM2zOt8-?d%~DKakfY@^TuR98cUuchES%&CLT3p zhxmU+bouFP1S~%cgs{Y@3prGtmY)ZI6Q!(A9nHb%K~w*Q`oDvE46VX}1UFNKHBIc14`AiwjFEbyEa}L`6|m2@Dp$y4pyP9z+@@ zR!qeh}a4l(ZMhh~mxh z;GT6H_#0^&l`GiL*vQw4Q9+Xl%`RBF@t}h5l?s9mFm-Pfx2@Yt z9G4F`x2xi&fWq3mfnb2nkYCrxM2E)wOB^5#Ewum`+gXz$G$zXp{dIHjeX;x zY9VK6Q>-;1A&KJsY8o?wtv>eDJKBE%2-1It0#+1;8P2~uw8Dz2MFU z(fRDwqS%N6u0ww{U`hv`Sa!|{@BbdVr5!il*khNzg;*PwAeu1z%@$u2qh0quAi-=N zBLVMKIzIrQH6d2}QKiM2`l}@<{IkB|JJhd&t)F0w z=Yla_VNqtA>?CXY$puU8C;63-4id#Z0ATkeE`UF+{AE|7J$b_2u3#Wo@3SCsVt8xg z&n$4%qI+AoAeNv5uRDL_Hlb9sTE#e{n*YOcSCtn4&=kJ2VH3F;!hDDRW*{%l5cI*^ zBB={B|2mD7%DBf~lRggH9E_yH4%3L5!xr#!QYbKdK?sD`j+ipx)%Om2f_a^M7ZuK( z)Q|9b7o$6?gQYr_qc^&sKpI0W||_>GE-S=Vi(cT z--(X2Q!Y&_V;XPVnI{)6!DuZ(lWN-4N_?Ez4*+CARf0HBHgD|wFw@Dy_tt}-pGAcf zm%nVtp2*<`(8EN9LU%cT{V7r+)V2;?gR=ZTE(d-~6t&10>-p2FS9V>b?-Hqqn0nUw z(dcf?d?3bvZfrBIZH3_-H3CW>W7)J3c0HWuh{S)oBK%oUPrsuZ&Gt`RIGH|B=l)M) z&B{(<1C{Z-()%Urn|asX?1xWuG1}9b3YCZbNACbXp+@!0qU z$-wu%uC*&xOS%%|p?@D-)lmfPU<9lb%FvmF>$!Rd@JmxPC$Up0c&Rn zBKd;vznA?gFych$b#YCG88?ZF@$mgb8rmcMaXx^aUCnax*4Nh#EO(cb`K8D!r!B#A zuZchW&u5n_*<`G;2u;2=G!fspDAZsAAvb1ob06Ie6NrBshBZw4S1J*R*MoNE@tunb zpA0WO(BPhVMQ3Crok0eg^ihn)Rt;ygiK5Ah;-(3M;^M6V56iA<)d0Yx{4YY_v@Rh5 z*%v$9fBhVMUwmIPVC%>jkgofLM1pUvs>GQ%IypbBxz@2`LMQym1JMo33m*XZkk;4F zMmNgBOoLv$q5&joNwUNayRBW4?$=%!ju)d>J zL~C%V`Lx}$zW0KUFU&}GDp{17iynn5Mg+^-Rf4c8d2R>yBKFC%>`|MsPOx`1G?p~# z8`FJcB(?l7O!_?=pvH3+Qup4M>n-4$clV^5fQRHh(_tuM0%Nu8hxTW{kFc0}dl%_48OLcvd7|9L|Ara}d z)0T_OF}hIQoCQZ}8OPH(q^Tu*2GsGkrF>g1mGBUud9|FcXFEePjN$WWquU&E%Qly} zUiIcQz}jBW&QtNUBDdC14~reEexe;37S5l92|cP*r~!-i%;_NQO7P^u8aT`|@zKHyl`?n(;Av zYuO}YX;Mo&HqKQo@LE2dW8P#S${tYOT*N4Jd_%OMERv(;OhSp=CWkb}$;PmTHWZ ze0eW{;iB}6JWn%$P#b!Teg@CCdR)^3AMmaZ`@8CUJ`dgacw?)A)LC*Lu54|Hm*}`7G4-Htu@oHXEXf6dF$X@z=k`uI0F23y#TghU1WVq42@p{ zMb>kPl)2qx<7YN1i8cD_30$CY?d|=`U>&C6`IW9yR$>6?$S*SkmKT^LBRpx@!{SdeXMkMHXwOT=M0kk9VT3p@^^P zd58o7c_yRHfuRbRzBs?N|y+VkD%OlIcKBi)3$QAh4a`4rWjBbpFLs)|J!Pwp6m$mHN1;3;rQMi)4rKKkq8yH}T+sk|w%z4e{f z(#2yGauyUp=*SQX-hLha)Kdqq>$N#`$gl0r^GU(~pUgDu))Yr9h)qX>(FZAPPr0+;b>(!R7 zZs+x5wn^cIq+x)hxqEpdluBKbZaB_0Gj=6FCcW-^gKl^ki~&^&6lXtmEC_4WQ(kCK z`^d%)^ryeclZLN-0Nn7GMsR0A_Dm)+IQxhCGQ(UAK599;sJh$dcOPBdzjK#VRdrEq zf6K~>z-oO8sMKurj_e;=Atv>=t5`IjEh$WjOPRyv_s|Csb9xLNQD+f@F6UZE?#nzC zDlELLhNkgLb-W_%=)zJ?NHJW@e*VJ3wp`o70fOD;auVs~RTI;jBh(<{1!a{JTnUzS zNQl8%tJ$o+4R||C)N(576q%oA20UK*ihkPLpT}TifM?Tb^f9gqT*cJtb|{VGXIEBp zgMvP~Nid?yPO9!2?cz3u^pKH}znoOGBlx~<{MnWmB)20xm9+VMjnfw-`MUN|NZlU; zFd0T6#si2n% zxb0qkv}Z`_ZYsEVs3ZZmw7ZX9S9YRwdH>p05D`iS^8AdAt>j6`E;DE5=b1o)g|&YR zi*M&k#0;YgqbK^9kqL-r14J(l=8-})`uZFQy4VwmS6__fVj=O(+drrTF5L815^6O^ zYU84a%+dU*Ml%VH-FCSkQ;Zx~r2cqm8g!L|vI4Fq6a8O6tj^PaW3F+2+|Da0{do8q z@riVEaLy8fV<)g=|8fFfI&qd<XDF87(p^fK4Y5R7Cv|Wu+$>q zy?K?Ih|vJN#GgUe{>}nxsr8s9{nN-|klMgSgbMt8TVr7$?tQngV|vjs4na%y?}MMX z`0gBsPJ3I2+Xh<8x$RV(^t|MyI$YgrQ!A&RaADTestLhdfuM+f*wEegFBmm!mlc6NpCn@4h` zpFc$Q-BcMn?U80#y!y$8hqEm$Y4p>W`(8>e4I?!3p^v(i+8bZ@Z4=aLK{+Mgemza$ zZR)Ah+*8;ebRsu8Em zXr4I-w;Cn3oo}0eMaHoZ%(n&^YRrq(s>t?Z9)`ZquGMjgS4Ypxk>F}-d$BRMFJhpb z^r+QoY$sOW(UdH|u4ONWPET7frq#Dd?c?EX?H_yXVuZ=5XsiA*#p3c850J7it;jzb zJ=gVlo@wY+Z2w0N^qvoHO>D_)zXM$*R>s-BOj zQ4^!i8uThTHBzxP36o+L&OOG_r9R;B-(S4N*%B>5yPiTEL%rz>-mA7V|i5wt0N`(!xRpbW32L`O#`E$TMcSY$dMsa*)AXsO6_5q6D?3-?H;9ap=D zVm7bK4dfdO1US7OF8k*%Cpw$=65Q1orss?@-JlTNcz*`3>r(kd4~^xcYq?XtSlgS( zENpK5Atlr?RM$RQ#e*s;fAv5Pj--7xx<0{NA)*Q_QpCt8XHw@|Q=ex@$92AnMbcF6 zYhKN|=4K>$;zxR)+iE@<%eY294M9T&Bwg*s(cG{A1HB(b0}*Xs>RfdkCnxMBUm(}*hJJGcjIY@McfcLL>HPpf@u|PBj@XHmQGfFDLCdE`G_~sT^-Y_H8l-7RxA6cFhQ` zB+-d5*$1lu$5@w>!IV3Lw)-l{O79l_no-j$Wq|^g&I2zF+n1uDkwBQ4D9v`+N7D6r zNjiVFE!QxKnuPFewS)UqX;tA2y9q8u#fyr z!e*KZ5g)>p1!AD4@y~S}PoTRj3|4)R#2+eT88MYXHl4uTPqJvEj{J-i)ns2LzScoc zcTl(`lwZle0lgrU_hfeF9zT|4>-k4K0QfDZuQ6|6ykm{yj~1w}h;BM(HL((YgFxCc z)*H=_xi1@maNSCKS!aGFUy#nH^b>`E+hbK7S~^Uv^Pxa+<<-7p&Y0oJk z)(P?Aa>G$_HUjSb@RCOVGVoLR(UYjVdYYt;%TX)jsx0=Oow!uo%`SXlT15*@(S)=t zkK>a;{*YDkW1PPXF#-18NIb>9vI&VC!zkn2Q$#ot@_i=@W@SDDQv83qMU;@$<+{(l}B|;L7x%Z3=oNw1Yk#p+;bG}B$#k!mbz+j97)8LpNm1Y23_5b z%L&rXSk_FPQDmSnrd=}_h|FM`KU^LW>3Czl-WS!U&!=g91I}xge5S;D4o}AtE1vdw zJ^C4ePU?@=Mf!d};-1P07s~Uuc6r%nH(aF??4H;CUCPaF7q5C-_G*K-8~c)oFTKgc zuOMH{dCU-Kt*Vx9quJQ5iN>^Fe|tY5akw>R$$?#J?yv5DQy;*+l(ta*wlG>8Wow~W z!l-L%R>43&%2=#M0PQfpY`5&K(z7j~pxoYwjE`lBFqO%()q7qAhqdGvJUoyHbTn9O zf+xW3tOT$mtZ>TGFrmDfS7#v=x7u70QyP%%!+)e8XB*?78Ka)Y79cLNQ~}io5;VxW zCam>7t1%>_?YlT@R?_n)xZOGN6Z3jc_nIqknsID2!GDs~X6dOeXW9N&5GOl-mz&p~ z0qxXL$1y79t=}wlJf&d|q7VZTZu{5aKg%7KbKsClA0MJ}==?%Qr_%+0>y`6TIW);A zQCVO8?=fLoQJ*hSE8Q6z8$+xgH#!|&i)i=b`Z1j}|NR(=L8qPtZA5*J9X|~MQT}(C zezhe&LfzTqEm|}g`?Sg0iDPLVXguN#e!UbA?Wae<(wWhTy&W4F*_Y|K_ahgL=$8)q zG0-pS|CAG}-5M8*G`DWw^Fau_a_u47zexgl;?Bhj%W!gM`dJC@8KPrb#M+BRN^z<-Qq z>jCJG~!aUwmE%4qu)s{DGNiVfPO) zI%4j!XT?}+3EOolYDx948PMHpZA@bDHoAPQsleeaC$8J|EcHU$jcN1nut@uxn$&V- z=~|_=z1}cg8VLKUfG1(eso5Ih-hFUz0m!MYs2@8@Vq#$eUQ8o7q}6S$5w-ew9lkBn zQQmk|Rm$--naoX7FF-ZGhXCt(Gf>)F%=Bb6mh^T9ZlVk!tNtA7N5t3q%XDUJD{uR` z^v_5-&)Wtn)bpSdj_@&yYIPt-!8t*nMxwD5PFSDA@jM47{Q6p)mafKL(6+i@f*kz4 z7~uEuqLr0wZMC3{hhFM)`jniP4Qj=retDC6a+(TVXx0x>{pl*{My#?3lQND^J4;rE?bL5&8WP)-e#7;ZA@>y=J-U@!Zj(XO_3eD{t5- zy_oZD=Be+I= z<-niozPZ$jHpJ{8>B*g`{~`Vm#p^DKew~Zd;d6-J&{K1!` zp3o9n+vDfbMx56k=gZ1`@A{2tD2Sbgq(VS77e_{&KW?^xU9+OBRwOi|FEFl?XpG** zK8<}UV!*nz_^=8QQ!d1qr8;;4lX0b>SVEp|0WDInC%2taJJWW#U41#Hd}IWuLe~0w z5zpt6Ubiak!};9vYlKNlJ0rxv_=YM~SnoZ9{WTE~0MHf&g?p6GhhK^)e)jnWwCiI6 zj#W{>7fSqK5czt@cs9iZiM;Bxyp(G1VKd)IMmKPFgr#=$lFRdp*xF@=a08^Y8DyLH z-8|sduTPG{y&h^bI+i>;)>G!zJI89uuEReP8(qISAK$rtcZC#$gC#t@kKaDUarM_b zjNI$$te?t#t-aY?LT8w++C8^Uzm45|`sAs2a$@GQ_Eg(QadjQ6S@Yckf=W3Aa%Gp* z+#DB0AkFMG@8z%zHW|fdH&J)dP3M7LvD|0XPfSmHtfTj2r;uusr9PJYIF?B za#Rrc9TXJTw5tDt0J#9!ew)2lIxjPkwiH%Unh{7>t!(lV1nrqt+S0S)MN?kRrC?U- zc)2f@(BB_+3ZyE{S#-H{Tuo+jR(K!hT|&LjBTSPZ&bfVX=iw@GuMp$7_2KHQ$DtSq zIokYyEie*D_}VP5+U_x(UbVM)W|~V0!iTDUN1!_iqovj1osDM0fRFi}VFZmKBA~SYNSqj_cA9cLC(A2l9RE=R9%1gIM{Mt&>ifb@ zyi!}+LmITVDNW_yB=LxW%P8sk+HZH*APET^XW_~@b)vZ(07#m?J_6O~GGOmO6UjCiXkV_-<{{I7 zGAT_kfw@7cvQ(JeOsh0jv#Gf%mOL70maJIdyHQ+xp2+sJ4?zvgaH7t^BFGBWo1M5U zM-gPyrtffTH8j${1=A@FQ<4#jZXi?xg&R4WXMkZ$>B5RwqRuApz65ihNsVZhFM8q4 zah~xjy`JS~zITu-Z{-ib8@ekDn6%w0k5A zNuJo%dd$fAp|uwD1xcpy2u%oXg2tf#Y9I@Q?!K1)-Zbmb@{Q~6%Gy;3gG`NN{|thw zb#N%)uzS=h*AdfZ`&t+_(8d?&llG;fKMuY&Atz$3-sk_<0$j?MXJQ@Fox1fHkMJdC zg4C(QLHOsGuc*}>jlS(|Z1lUcn`Kn~uuv8Wo5%UAID*8CFy>zur%S)Ek}!`Nq9rRk ziRaT9V~xxf^Et^%%g=geX`7Sw``(XS2$F<`Rwj;Ju2Dbk0%Ca0*Y4QwBkyQm4PE^P zst(Yry)j~&Fv?)TFgc^-()Dv9y$@B=oFy}SXAIK0yIEf-{-#{p_^Gg+O1Zj7qm8$V zb*d!GV_XF4<9*PtC^R9wKG(MWxCI~l$7NVDt6hIQW#5k%8bw4x8*zh6B5k?mQ_-g& z#XVG{(^3o%xS>fjPS8BQ3=54r3`HZ^1S_rnk4X3PU3UGUuHfEd*I)9)Ol-fGhh1T< zm$PnnbjN(82727j?9ENQ)j)IQajd|sXIcA?OUPd{0t7$N^(@PLt#6V2j$!zj@x|@n zi#)F9Iy>JaZk#mj5j$~if#@TgF1uh%b*cc!a~8kV(=;UQfr61Taz+v zf|URJ(0@$6TVeT%#j}9{VgEm$g7ytx_f@l0-We?GO>dp{4G;r?NI%Fk$y_*AN) z>vwT?q%^?1gp+$%+`GD6MLP%D6qD{XOW}qFmeCJY2cZVZAb3j3e8_x zOXg&TcB;|{W(_k?kSi<$xwz_p?;t6F>&gYe87{w(pMYkci*lGm%t*l?n#sLN_{bfG4; zvwTSHS0~^I2Ss_OYhL4T+`Em1ps+7VVcgHV*I`>?tM}t~={T4pso1#Q-i{TF0SNc@ zQC{h@yHY9jC0T{nXk-TK<%dtwN9J`3arYJ!f6#27CR{aDHM1?Y-%?LlWL=e!f0LU2 zg!4=^2F>kt(^~5;ymzYAAr_z4BA<|EDqt%sOcf^P8p{x#W@gz_OW_!Bzm&>lk zW(-MB>Im>lj>f&Rk!ibUS#?E46<)-tQK1lTSYN|wBP*3iFVN#4>q+JbFrMF*)n?l} z&}?q%{rs_9-JaT*TY@HG{_d=xhDmqxb!N54OJq$&8t{p{wxMFu?SfR#3~ULqI32c4lokBhQDwzEl$VmOhtU z!SD;z@(iW&U$nPhwV7Rkul*N%L_ zCVYTw=dKEw2;7fp7zg!Sl^K$ZT_d$vPJyW3nvs|Pj)Q3sNhv^WX~y&!o*IqO+6C_0 zL0bvurswkJ%%B5a2Grkzk}+>1rS|~{jHuY%55&al3i>v$%(aZ=GvK!Ntz7F}>t$YO z0N_=*(&X-z^47f2aSq{gg2n(RzuG^d`;D64Ra1roF3JDQ*bh@jYL1Z&CiE3D)uK#- zQGXJNSsc-#cwd~93Q#9-5cu0qK<}26SjF9>zWc8hR4!3Cd7Yh}M-h5iK0XTI@;~ih zqE1BIMz0%$hm0Gk9McDrZz;Z1(8rHX%7AU^zmo1Rrev!L?9`g+EOMHn5Nkeny(BY2 z*;l1SW^J_^U!6=Mo(*T(GunCTdtxiY76(#FnxYdk64yCwO3Gf%q@9JYx+o}d7_SkZ z_gR=wHkoNx@plL5YK|t6U1x7egVpf&=sXPFf7T8QIWW~ksbK>CW=-dvLT(4nTj?H| zp&om?3nT;sL+jA%dlGMO*`kOC+>P@tPt|hrsN!j6o(aJiL*@n<`rWIM)ugb;W!Ai= z&M${8nHUwagd>oql-!Gb&Yqq3>2?;_-OV^?a6vQidd&~B!v--0+PCRkt%m*6PT^Ty z#Q13s+E(WxZvqYrY#?}CeA>r}_>6P-bvTS*S>A}KptEhB@#R?FhbO+ndad)`2Kj64 z!DeA1<5!Yh(iXw(c%IIh6E7FPNo$jz58v-$1}V^lmKLP$pZ-Cp0>50&jql>^CgzWIo z7!!nt2WJ3QEBM-Vq`PI`?p42H9_lPSPPt}whc02qA>GW^n+HY`ue7S|1h;k_#<(Fg zA+bNdyV1MtLK!ha)D!29Z^@?e*Y!m=vq|{f?t^*)WG&iLZZc|J>}RNbUZ46)bP-;b zQE5ih$EdtF1`#YBg~4Yr=Q>g^k3{142hF9rV6UaRV3b-mV4taC)R~l(U1*2xYFnST zc9=g~&z@TSH-8b7^#%Swr7n4|yTw1|x)ipcT7CW8t?O-3hr(8fHb2j59As#0hgnBN zePhuri?7fjtY)I5V7yVdsH1K7<!ePjSsr_0IB?E zVOZy}@`m-OYYN(nboq+U5D<+T*fu@9w>6BD^gExFykAP4okA;PV z)^K_BcV}MIDbiL|%7f$=a&N#te~Mu~Nj?|-_C}%+=wEQMx}>)#-vqt>oiEpZ@b&Oy z-@0gsWfrJelyk4T^3AR9{ECt)-vyc)B7a^Z3uQihJ~Z&JGhJ7&wy=~7i?qjkJ-|KA zOeH#9a0^8qS0gCnbXT>%(5}UJ;T{|4gg%s74_qJ5`peKdC26Zvl8d{%Hr0v{HtQ{~ zp@w6+@*P`4P>FkN?I_2;ymfLyov$`!O?@=qT2yTByf<3K`$eA+d7&BI!ByX()rP+J z#KUysT&1Z3kcy7BOBlua*-6>-9SRNjx~rt|xkhgOac~m84?{}u;JK7DruF8Yw)>%7 z?UZBbb#N;!@{Y;Kj4hC(JjV2#!uh6YbuIuMZk+UKfQ;}|0tJWYn?Abq2Hwt%pUyd) zr={@lQm^$hsBM5goo8g*fL{n(qDm}=WrllJF#{tq4p&LM zIhl~R(FVt2TFrc6(MVIy)VH16Adh#GuIm2kIQn>jx2;AR#r0GsH2Ff~JLo8Qf#`5T zMV)i#H^J+V+8hh)96JjRLRegYmnVmBaA%qzk)e zxO=UsniJM;=S3)_>E~Q;hp6lAJ{D}=X9A8?@*W3iR$s`Y>nia#h<+b~EXRo@Iq?ri zkUmRbQ8)P9FW0o{E_+q05^_5@2_;93?EacfY~C(Kg}j;BT3)2@MhQGil+9RM$^OwF z>`@3uW2s|beF&eAqej%6HSs40-(~t?%hD$jWVtHk=^Fj)PW~pXVXrSHr78**lR~*d z2v233tB9L3R-c_U`H8^&Ec%y#{ioB1_X8qca{irWdlH4P3re9uv07OvNBi}f7^L}Xb>Bfpqm4WYN|_z*yB8Y|+#>=b#{nMLD#zZ7YH6vEK+;i%kyoa!jTzo9 z9kdN>4xh&lwP)>|zOHlHdCk=g7nS9vG8H<8#BOAPKF-#5k^j6;j-in~Z0x-2(2B35 z{@+8|Gk%l(Fz~%dY>CVAcB;!BTYQV5gq04oj+AMLD=Ci+OMgs`Z6J}78fFfn@b`pk zLZgt>u@PleC3-X7R)dgK*k?AKgzOV%?U5JQ?3aJDTb-C8CZ(vPZJpX4^Qm||A0i7_ z5Qe5;o<;7zoTq{Nu&<=)xJY!Ni{W^>|2>d{e3Dc7ILHpSMCHNXMf+?kS^KU(W4J7e zhUBvN{1$get(Bdb1u3gJJpR#f@;~K2KD!XU$F=($>qqWi_i3eA zrtgV9K^Ll_y=HaNW%}`NXIHzHkq*OF;&AuXOZZ*;Vs!JN&|Y)7sRd(}az@hAe7kh! zlMeoNuBDm9u?5Z~y9u2m2k3Sr<`e7`Z5}ap^1tD5egE!*Doi)EIJ}--#Irn@#@~*r zLDsjo=AGxXTkc|`UPhTC&f6_a;TWC}Xs;2knCoUxl>Fh!eYzk*%sQP;D#@T45%j2V zmBad33VY|mzRSkNX{y;PLMRMPsK^G=!Kz{0UnGh&5P{ryJgKi=%wK=o%Gj~208DAx ziNVd(Ks)@`Iu-!2MP|Pk5l-DVk1MS! zUENCX*`uqEjCHm4+ozT%@gH}OiHr~rt~T6;R(4RbW;64kwB3YJV-bKkitKNkchO<~ zI}iU~w5>LuTB~!IJdaU>;v;*`n)ff^mcDeO1;)Kk!e?Ya;lApuDThTHS(2sc=(yqt z3Xzz)AnvQ3w891}t~y%1kVQ}^c~|eIw_GcO5w%)Tp(trJjs415@2D#DF-oqJZoa)| zK{D=fKt8Qw$w`$o1n>MzY@g)0_T*fi4>DxC7|g6doRJFmU-T-8Xd)Mu6G^>Kvbc0} zrBp#g%$okZldHWH(N=x_!(C1Nug?xE>KQlma*7<{#R>Kgx*kk!h1Mq4yE?>&HoYm4 zV<^dE(WIuKJBYDUiWY|quoAXY$=C>^llh~OY3B;%42YjUu~e5;Pmy|P_WIMHa0wxN zhB?^$&ztHML0L$<)Vo+8p;y`-n#7um`hSG_Ei8H53jD>knjYhd*3-aUq)fQH6~I*F zJB(dbi!>~g_K?I7y}$f;h!RN(j^`lN)$b4wqX{2f*{2k7g3;CA7n@;$oWzfwZ2W59 zu8>2}s#9lmilU>%M`i4KnPq?*XwfsMlHf*v-|}K6=p^b2;`hp8R<-fVy|o8m_{MVI zk4wGPy#)pe@5jxh=Y@#9D{chR-h0s9c`?BZ%0iM_>X;6~B|1h_s^#C468K0O z*Zs(z{ShP9`O$Q?e(~u`HOWf$|7_q|s^qz;hro$b4P8(^u`~AFQ5ue|l zHI%>RzT5Xy^!h9`Xuw7wOC@GzOYP9qHjg&IOvd$6Au$jq4$62THz<&6W3&9d@tLUo z0}-q=OT)ZE$ac@eLrcZI-@sc#VWuB>*!a zT8{QmlE>3NM{VCA@xWF}pxf5zzr!m6C)BFmD`;mhy4`bA0gkwZ*iQOuq2RWar^atD<_ivdtG1b z^-KdHokQB|!%0U9O3-$4v^WCv>^rkYa?k$m+*tooZhFpp3a}z2z=Jr5+VMK7!SOOI zv#1v1eYFVf7HL>mkUU#Y!tD#VQOJab%2dDSv*&8*%6~KFC9eN$f-$AE|HbfgsJQ)GrgzVEa3&M$X#WO9RNGCz8qvd( zk52+TgC@VNSW|phNhD=KL84?oFq7TYL1I7>y3FIQtzX|Y#YkbbGk7eN*Oq6t(lRl@ z4S}vn$E{Wnng9!qh_YT)t>Sw5-ujJ#D%vZEyD2Xpn9QZqc3b)CH#>5w(Q7ulm}p3l z<=}#-tM?@<$~OCwxUM`mQ5yeBxjy)|i5lZ$Ga%id-5gYDG5xyPlp5p;qO*63<04a4 zFzZR^GsnGwi-a}8z{_gg?bU8Gz-y=fM0uxxfxd$x=k$xH$>U; zb=vkFc{K3ol0E9R`r&x(yo1}Yci;6HQGyU%tK*`DdeiPPoesX#1^loc=(kKWlIf;r z^4AYd12J|YA7^oEf8u!|9U&=_h#X9oRsvY3shThXpx{?#WDEUTHh<|Q;V-`R%W=Zb6D^wu9KM~Ip? zkE!~jbt6;^a)09)miO=@Ku=RJty+J(`Q^J~->e)JF0qhA@_04%3>muAuGfDy4SW3v zZCaCAhJ}W$lo~%w=G#=HG1{~#5lEG+O&mOmrzkB%l%rd#gkMVe9%lw@J8FtaJswB0 zyskvTH->BHu``~YGP01WCJVDPaRfL{hPQrm_+?w1JKq0jZ+G5VN$c=7sHp_pGO7|N ztr)~MY8VLTP4*)MX7*pTfpyKioda8Ezz*RiJe^Y1TbLac zUc?_8r#51BQ1ASbR5GidCn%b~8T7w=_Qn`5PZ}>-gZ3*``uOBbaRX-udj5o=VFc1^ zcbHxIBk=m>HSNslDyWFcX-YYK!A4^>@k-xMsCb#*awN#oeb6C0#o%WcCL;BBLm6kj ze?6Zo4j;=ams|48n>^yTd0RCb@NQ1R9;spTV-SBjzn}Nuz254{L&3n<`}n6z4l=Kq zn5kU+`fMAH#q*@=Jg`ow*&Z>`$4y8`3ac3BibQ|4dE}o=`nKpwU8`eQz>K2XyZ<6w zHyv+klr%>ZuBJ&N@w}m*?cFi{LXbYT2sEh;=#pf}T4A;#SkaW`dDHqPasfB4KO_EB zg>1-r7{SrI8S?Ri5!xBdX~7MfrGO_OFz4i3k2$Gle+R%D9L1sHmQTVu8vr)jXO7em9TkI z7}cgom*d=*bZ9B^*lOU`VQ-Et%`EV=Q8-wo7{rm0;3==o&MQRw`}X}$NU;SQArC}z z$qbLcoP>ndK2`zkuxspR75r;h5-VgR#g=Ub%iTQUHBU<=IB9YuWkLSyon_R^%~+JN z{_ON;L9`P=U5w9M2Je_%>l|gGMS#Dt?YheUn?JQXu=-@ZeNhp8eucI zgR-fjxw^SnF6XcLA_wvfjW`WE-};oC%w62G-egvnINXgT8NaDl9r$nI-y*TjS0h86 zfN?K)N5*$}2U5p3ymD+DbYsXpWd7-=O=yRLraw(kpV;}6@b;*$TsE;64r+(lfGuse zqyXWtrEa4H^GIE*d`E14M*7JpdXKmwmRPE+>DMbTmaEnxX}ewtQ|Wk`CwQ z#5CF146L-?JhLB`fRlaxFtkmk_l9@?F1Hw&ViPsbSeF}3A>KduMyd}aKEQwh`)3!g zqrv>MxH?lpEf_AHi9|oTu*y`3UmE0`ALO&_z(jegWulQLdf?2#?p|h^G4$z@_=bUC z3`hCU;C){;DFz8HKT6!8AgqKA{zI&cNDJ@f6>S}5{S&~o%OB5^(Nu?%k-A%bEWec> zRO{l{LTNX3WgHXPuvfO->FwmJ0J4Gga{>7Jb!5izcd>{l_LR87Ti_et`SRgK6B^Hs z!VLgi1yxfDbZivO%XG%ud^*bL%)+>R_5)8-G!?p8Z9A{0yCtYwxGiNaT#pxQEBw__ zJ@9&J5bNukR0SBOQAkB%9u9(6v?Or>a9za6S`_f8_t@^-*GIcl;`no!^QE_{u&DBd z7pk{nHQ&DtfBhxCrwKl@nR@y&slS#1PK9^z-sJj8T!gC@KY-IXH|2;*$)KS|L0=9b z?rwBXThHu|9_lBK>fe}0wEWpIUP?+LR z=KJD{JJn`G^l5w`UZA8lxTQH3u!`K1GtV*7HLLm#6}mvNpzpoaQGt#}@omvhkdo@x z);H4Hk-Eo(mRVm8V%*L(y(rS&Z3kEliKVs3;Az1=4NDXi4!ij%vXBH0kE2&!sow<$ z9V#FYZK&)0>PJIz3+DEIcnOwv{}ltRdBmaphE?ldc*(vS##>UsqJY5>NBtVxKCixQ@(Uu764!2KN(& z_gq-)Hjl$YLHzt)XEV6-hidD_l`^X>luLv=G=1z>($tTT=V>2?*Gl>5RvC4@U#uK- zLbGF1A2$%g5{k_^sP^l3Rn5p>dR6I0{Nzy%@L_!q8w2^lO}}WOy5&v2yHeMSzz3T~ zIMgJ)DCzQd?TQL?oPFUwzaLGldF{lq7h%xZJ_9|OI9(Y+W$zuSC3CiytOIEJ%G3Wo zlCtoc^}=UNdZV^LwZgQli7@p!P2(|>d<#@3m0+AA75qnu(QWjH*tbr|Ic|8mjAcby zBGDLs4~OUxPjE@R>|rOVIY9%rh9KPQe$oDN-5}Ddx7Fp=Wy)m>rTjzvVe34J(z6_> zdU9l@T1^KX@4yPVbXtQMxz#ol6di;{#&@8AhqU7Hn>5fwJr$M=<4ir|X&w@*WpIga z-?>|nhux-tpXZNpy;Qwky4QL>e4F$q3DOi_Q!`Hm7JbHhc} zGd7wwuyF4#pRic_XPG(qkIubuG<;f6Ho<*+=}JW&Z`#+@B;0|IAzsDmsriJtR>TA< zlPQXVbwDwMI7$ZJ{N+SDZ@ZS$U3mAqL(su4Bs#uPgGGv6B|?lJ*R>(Hr6EmjyMt4e z{|cd6{*0p&!MW|UxjfYkW?+oN{seIqt75>F1 zL3TEwQ(7~U8CPL19hC>(oGp|6fQNYO6e1ay7to1&;es!pYLfK|Z7QkpnylEAs5!GF z)OOS?KKy{Y?&pHVI?pYN|6?BjLI_J{Vy1b>ZsULmuGckN$b1=tHPli-tX)6L9g?C&Qd6Z-LD2Kh`mY`AP%6TAkeg(0@) z#}DWt{%x58bW;4!TN~4g#lxo}BC*~-8$5<7)VqFI8u}A~P_oQu7xO_TJQx`UmD8l2 z8P+Ru#Hp9_HGgeFPt~vYSP3AzEtUyT9!z0f=I)i25S-yVu!J?N9{>aOU8y(% zJU)Z99u3~XpHEZpF|mDTOH{%iuH_!K)15j|kkc7wIwHP7`k&z$G2X`m)5Bsty?D=_ z1x?%%5-5KJLs5BHKq>KU9HkNX1f-Z(`exJzjIciPNp{oP1YUX0h2e;(@#|E!)l6}8 zsMi;~S-ik}d>p8!u^8Y9R?7!ijl?`egr~tjBhQgYH`YiMoQx4*V-~KG+W7yq0LUPS z#W2InL-Xfuu;>Lo;1v_rgMSv^C4xJ5Ak^kRX~hdS*MP@KQeyinc|XcN6U~r%r^^&@ zBMw`1lahvUDx-I$L-qbT?~-+L7DAv7D!LeJMttsa+Vs};aF#&-(n5s1vpYE~3i}KL z+#+Zlb~*U5ZwR@MNq4GUY1K>Xv1)AS??MrqEx+>q_iUVSN0)}}J7;G=6EUjGE+yp1qx*<8O}uypBJk+W)O@1s`mlV8x3 z%)Z(Y473~zBI)eg^Jl-SpHO&1Nx%UITqHUX#?y!5tN&Fsi*&!V4`mes8^v|=?L+iK z%_W{xt&w4M+lp}h?o7X6Udd1SYV{Py#}e(3F)Ach&-j^#@4WCda|;RY8;i@?q}sWb$-BG z_419Xwb!hkmg(;4zWNXcp9KbdCvP;57^hQwL9 zBD*JU4SfCAAbdA`4U=YszQH)S?532DMZp|*tST$dn;o8%_~A089zh=L655v270<7l zeJ1WBwhOs6_zPGi-wl(RDT|EqY%LcK?f$CMo+}1szu1lnw5A?|0vB=)$~`JdgvKm_ z3o}ZJKkEeWj{ig$m*VSsLG2Md$M0y!lgc~7yYhm{k1`WYHjjAa_)ZPF-7Y<610-L! zd(bTcfmqWDYF)TR=O%}Cat=<0u#p^WLMYT_|0yd31M(qp_!qQb_pi`Qo!{UwqL(43 z7_r73#eeTVia`f=QG9ER`i3Mjbb-n?UgD_sJ6fct$b%^a@K&?+v$54o>k<~QCk`4D z=5QeRo@6e5Ik|wH9^6IT+70!kM;%;V?w!Oyht$zLpeaTqE(-M681J8$yd_myl?%%T zif@?%HDwc$g}e0`!)nrEc)2P2-4@&i(f>BZd+@(McNAI_WM3dyQvi$q+hhsJR#AV^ z@`s_Onq~Hfw1xb*EiS(-hrr0hXv$_sfTF%Sg6}y@3P0%WTNDYyMRY0av{ZA+tF*3F zh$A3-q-7PGavDGaTH7tdaN6}?38^>$YD0r5SqKokP7}6Spw)Fu$y%;fYN!{r=RcSJ zE{KgT7GTDsIqaQ zg)17feG@-jC`=GeQzNv&cM?0g5&g0`twsXdCAXGh6pp!O?o`7Qt#DHZ6^8q@W}|e# zf5roVD6q7MYGHuqxgU2mU~DnH-GB2G>d3?-XW_CPZDK_lep9m8wtGqbUJYogGE@)= zRyIr*2w)UYJ*9T^#K$*wpk&msV1RlJ>uJ`gG7E3XHcRn!RzNexSQB?V7SC)HN(Aj6 z9SMM^GX(9H97V+;%Jbh~HZ%NcUR@q~aXDM9K^beGD+oC7e7g(eEO{L}{`H3*lj0!L zVOtOb8J^LT2x_+yj0AED$+gA#PucpufJAf`$lHui&+5^sXD76VFpG=c0sg`68iSj>>>p0{OD5;rt+0(Ir7WyIo4*U)K)D?}i zOO1tVXv#EA$r3NYP{%<%HullzvU#N#|3ui8m9m_&zTB}KyWt@W@d*OF;h}z-(^;E! zH>W(QJULsNvDY@Me1X<#7eWF{;{V+jERQndabfI{@%%zs^a+A2`bX@=OO!_jYwaW- zs+_TiKX$i0nzEv;=O-5W>qWPq)T|Hm7U+o8WhrF$l zPmCT5oJ~d2a%;M^G;PnS9?Rqh_O{UKL<@6nwakT^I4EQ>w=3ZhRY<{AVabQK4%N`} z6KlUbe&zjors_28coYP^K^P)}zQYo?hMm-~;so|7$bIoQbFk-%WDBirxW-PrsB(p) z;d#Kb9-?MlU2jvNL1hMldaR(Y{zPzmDBP8cB5~XC(|!)DJ-PnuLgIVSU^d1Oe6@YM zYWB(*g1jU->?2HkQISP{kQWTF5?4kNawM8|IRgi^&d~*>>ac>o#q8iE`d#ALa=jqF zmEAK_pm0D8%kSM=+s`>vaC`^$T4H`N2CqF*e-BE4Sq-vlz6U%&ctY7*GbjS+1y)uudzU zyoj71o@F9`fIf&fd3P|9)bJe72awF_1DmUlg~yD#V}lNF?Z}-f&VqS8%dqwl!?r&$ zfrtR0B(I*Y!cq~&wt7iQxV}c10w{VE>V`QNt68NKk??Scd|p+VQRplV|9lfAU=;_+J2?4fbftbBc{*Wl<` zv0}-IDU|5vk>IffPmqotpliMY<`ypKiyHYX+)|m-^&68*lv2{=3;*pkxVBODW@7f8 zUx{qJ-?qz$Bq>}iS{C^gbAmuU1fVI*9I6J#mXYIAC?TNXB?xKZO^~1QSYa2n#y^xH zDR>Wh`h346ZH<9(GvndiTYVPg4WxtqkInH^%|NG!L6bo+hP>9JJ zNL7knd-G}<=gG(U*@@hgs8X1n8`VuImhROS$dB7EUGb>46rA!YBK=d95fes}8 zHmS+In44t^dSR!3?R{R0hf)Z+&CS1LLG~5;r-_Ggpmsh6S#rCMPtJVslRwV zf^^{i$DJ=6)N#3WdQoXtZVWY4nMSA1tMPT;wLXu#gBYOysYI4pyo#2;k91w6X*#AR z+D?NwREdcKY_NbQhyme$4;Wx2Vil*c3-s+dx~@llWf(khUb0S9b@=H>gB{ep_Wxd_ zPmMmeTbJFh+@td?Z1SMP7e!;il(UgO6^Gl>Gg%gn5r_svvk?(MQ|eqnTToK8cRU%3 zTXm#D(K90*K5GX~yk%!Q63GNj-tj^*BDDm*GM}{o`Fj5{H9J}m7#^v*JZ3F#8NAeZ zX2~aCT3*U+1m22jjAA}E!^0Sa{)^Jn@(>RN^3~JCvjR*s1$k^w0)U)=uA@glT%F&0 zWu$hN_*O3v89hT&EbE6__<$CgLv`7Bg?|+POUo7x(6oxyRLg{*&gIUi*IGbkf@1&8=7^2vtc(*Z_+du8qi!Z}O3k zK-zyi3#;L$nq!q>@$8V@VLQy<*Z{z&-I7$!bnKX!CPg41W=jkf=R%mr$-D0x!;dyP zHK*~Wle6!~TF58|uO44Ed$lQ1Oczo4@bj~%W#L&fJx%+GLq{53W)k?0Lj2R7upnmz zuQYXHQh#`*j8L-4W`F*F_Q}YFg@AF;j&JZQ3Pho#usE_-f=c=6o;C#NuhKumN|Y2# zTke9-SX|m2gxM+ol^kR z+w)ucrJ6jI1B-4Q73(m0^rfQc|AZCdj3QWn#s@33St2sPiS$mi9}#5wePSoC`-RS zgD!E+mLcu#w%n@cw@0aKlQ^5W^yZ4ZU&IO|HLzenc07QVRS3XSrWe(j0$s`ehkbT? zMH)^nweKWVdWEBV=#qiL@-iOuVWw?1NwbtnzY?^(_gkJICYfz8Y3GZIYS(+M&+J$v zOBPD=FWo=+%7nVUKOHc--~-l!I5cN;l+(%Gi3Ww3EM#bFAL2FdW)L@)RHZJy>5$hE z{lE9e^#_SFX%T~}%=0ribuYz*0;ko`vYE?WroG|=*Qoj{pzqpxg3dH&P?Xx^g-iHw zDXG$@nSK#NKwv{b_&W>~C<&0~lcKXbk&q?F`@0Hs*@d-;_7GC|b=X+#PJ(>vtRZ&sn zFW7oQel=_Rl&|+TZ?UD7u?UO2Rn4>JDy2WVL^s zP0(-(L!8{XYEECTQ*sO5c06Tt)rK#QhHz_dGAs)bfnRed%AYM=mPmEZF{3*?-A0~$ z)b>y5eldPc(AS6<3rtxSvz~dB%0(51uled-rB@{{bN2_o8AERS^oi8ttOaBM3z&_+ z(!{VigM%(h8j+Yk{n25Gq2*+yr=_IU^5VV?;Llg@6wnVKoWTn>jN}GKC2$GP3`R-A zAJt=S%xWe0P_^O(Mx=7^iv=SoCI1|Ra?PNZW6!VwBbkax#ZlzX)aC5C3i)9W!xu&% zGrxx_hi#oY^#?*1!c(av{}jGuBNx&!+D}c&d~R`iX1x#VH{*e@PgR5kvW4G}3y#8k^Lj*o*tYrsO0h2{`l zseheceZTAB05kojb9`sD-N5j%V6MoiuWWA#J8iDg9}8V1)Wbv0zK^$kVrZQssI$>%Gm)tnua~q6?^!v>{6(`33!Y; zLD)AP1-)s4wCm(2PIjGt9hT|z5{~8q+J;;;63^^Kky<_wDNyvH?8KQ#D68ngzyyXD zhN42EL&`NyE`|Y|$1b%yJ^qZB>|NYHaIA0BxvxIF8XpHMzY^laa9p~8{*FKcg0&-N{fwg1r$B!t7@YVa^>nwkmT_EXBk|}B zHmra<>UtWhmD@|7w^o*$Ru%?&g!gAZL#THLn7>LlES_m8#pd>w@w*|CWD&BE{t{<&~=cJ5|R^ROped;>XR*bQk}GxWT^b(tH-$qaly$?GJ_V#!fn8<#>=WD z&0AubI~n8Qo$Wt1+o3BV~$|+cQ7a;3ypZ!d}xy1^6)Q$RPh&NKO7X| zzSR@y&{4~>>me{04C@s|q9NlTXYT!54w)d1jYj-q)NsPsoIxOVQ3%Uuz_hQJEwn0S z2jcz8X}mk*j*9-c$6FGl69IUHO^$p59rLH9=2yG%t3bquJ-h;Df~FRNCH~?^?AWZH zaI;`q;l*j{aAA218@_6IQ=*WSIOwc~|8yFKcs8|}lg93x2|+w?!c*t9zF_s6^XShi z%d@AgrHxcvKXDi~a@pJvlJJmn_05~mfefA#iyFb=CsCPSm2#00+wco0 zFC+3|!$-zuVWl-RkTp7rXp)jteo2Z1 zu(zGucJWJakIxL4ni!T?bd;>v-ft~)qUDtjY;v3H4$@f%z8p&wkCAJwExe~yB%@1F zIQgee4h4^-_mk0vj<(LCg|j`N^f)8Ot$a-!=3{w*G6PQ!?r!fk3d1ox9@H`L<(M5~R z*6Ee$+z)j03@&S>-9-9)G0j%qL_OT^L>n6`u`jb<8i|SQ2VTz~zK{jt`a7aDG}l&i z1)IQ+c#4B6ME~U$;3}gUTc7sjZOz+Rxs?*ID&mnevh^^E2&a~JFn5+P8QU+98gkCp zFt~!Qm_2VgGnd~5r2cYk+$8u@!R2&0;0^wQ=s3uILG)bUultVAel*Z&<7F66=cEFw zt&x;uEFMFm>|E4CeL>vV}v-8pPTF%Gm>*jN!?V0ZQ#+-I#mY|oC0prd7VO7!L z6)H*RvX1sIrc$H+`cFSy7Cgf3N%!gAj88%2-6C)7KLsvMEkvo@?Yyf%I&S~?>nhKw zu5vXOzLF4Y_teS9W zd*!VtRK|rdI9Q5U*XowALkEIW(I~S-u~E>A#N*Wnm;^K*Hs9)-Qz}$Z>0TUG|2u|` zD;SO$a>bW#RL19a6aAa=V9Us_wZ3P^;!i}qkiA011AYe2~jJjsVau_>%v?0Cz@~pi99(kqg{D^FTL<~7-BWKGeQ)! z8?7^NxL^5rchNPUn&BmEvMgvh|GSTv>2oy-w;mFRDDZN5I!BZn`)s}KD~yqa@!PhP zaXsmbK+&@)Qz;^iYmkr_MmQw<$2H{3gW#|IH$gq@t4EH>VV|xk%>l|0Bh+3Ii2B%l z=N>eIysuLkP7VMvvsGVS@$VDCN}8qCQtMxy+8YYZHbzz>b^S4g`k{vWyhtFdXM;2; z5JMtS2nXis(z%lxmk=7VFf(zeM^fPXRnSmM?wz{X%!jZ6zV#j8o_HTe`MfTHv$nR;oa;;I>~!&~7I?`WJa2#4v#kj|Ua=y0|Bo0h z&{JpN@O#{hrtyTJ%J49x5@pasC4n?FIk8=)QNMD-vmssc3FkStlE~-JsdE_~oM%hz zmHWF?0jyFzMkJK-1cCL7iFl9u8X^E81j>&^%U?ZY?=eNE2kpS%srwoz5J+SZ{mm5C zmo}X!lz`FJ*-rCmKR5p#*W`0}sH!rV$&?PlxVWFwG9CVW>&MNAw*F~Jjghz14#Gyu z>D|SDq~PfGV-3Zm)gJtnj{WkL^D{?o=3kCVtKFd@mITdCo408MZ6EhfqycZYTG+*R zgl;dJ8~WH-7A5X`gt{ze;kV$4dyL>PK~re=&|&G)mGGmo2f zY8OEP&9VuthvCSJ2RT_>TmgDfaiWc#x>Y)kNF~cUZyIW0*ua4xlQHa@{*!r$&UfKI z;|?cAu`C4-$#hhYT>8dw=)LvQUX{5>pRjWNS${hd-qtreQ|CW{2mP8xcf=H_L<|o) zPaU;M-VevMAI~(kbNL~u505mea&N!!1-%?@cQrSx8?|%=JhuD@aTH#!gZ(41xX(Up zq-~y$-*i3R-(Ip7BXq7W1vkCO#76r}=o&{`|o7%65n@H+U72K7MXN$NEfno#eoV=nd%@?aa zZO?t$?BKb#ib#0W4lbtWygxLi ze{3G1F-xiLd(hYEloPB*$$4(a3BS%c+M8FbGb?}ph396;(Q$JZ86f8;qpy`NKoO)8 z#Gr3?`&iw`X|q(r?mjleTj;tLK6|w1CjZx~69W|slgs;^Ydq7M-kHYazmeM%{#rdd zkM$>X{5&=ii`(R{PcfhD>VYUbTR~J24XV#hSh1XyTv|*Dz{a^5z&?mz)jKcIrJ6mJ z&I@V%Qw`4C9k7D+3`y zJY@za8?iwgohebetcJ4wKm-_}Q)jk6y@pDN&y$(ukw;BBvg70qtt@I~)-$vz%^n03 znVxpu=~X%@=(t-J2oUJlyfzrA@|#8Mw22z?Z3ptN1(0U$da6!0u(zu(Kd*eKYtZzJ z(!q^E$g{x>nxU<7Pd@r{T3#)B5fAX1T;SNnGO30t1( z)}rP?!Qjf|wEOw@?cCK+o&oZHv-*SQFE?_nwcpNp7&4qrLgzO+_3 zjdvpbV=tTOZXam@sysnbs@@diMVq$k?wcv5oo(!Q%lLZGfgjhX>W*Sv#ce^++qjoC-O&?f2mNY9b}DXkJXX!R<8XP%|h^RQ^$ST~+{qG%Ptb?iN! zkG=ykC%_gt(osUf?<@bmT7Z3ZZkjY5R*4fi9%^AT&)EB4>c-3 zfG%aiTYG-6{|3FV3%hsx)V=lomdpu2sH?NUdN7q;wGG(`KiJgbF{YlCEq9ATO~pfX zj!=vPkkiXTO96(VJN-e<7X@$D7?d89GgAfur@IxFB>u@jnt_?=J0d)9o~cdZX_L!13qmzaciAe9vka&TU5sAP)6%UD_ zM)>=hQOipc?E|pKgkXLwE|QiRao?)+AJc~$8MCTd7olGhHraTGvj4zBD9>E#3xEu; z`~=+$hAhk;PKTKZ8LFR)ho5%fNsZ=kpkHASL0FN0Z3(ezRO-lBqB+cpcO~IhJ`eM$cf4UM8UAz8d?!KEpEsWb*_T$z$GU~X^yDHVXNQcz(!|(t z*+L|-Qudyb^kx8l`WvJn;`6xb^G?!5`(34NWWB?$2sQHR)3@zANVM~_?=?%KiiAPv zKr}OrqAeddzPD#uaMao;MQJC?K+Yf$@sEFi8F?7sCLE58s^_{f%3~AVi|J8A$lnOcj~EQEWpR%M8A+U*s5Auz`z|{I_}Xx= z9#aZ;2>Z_%NyO~j6x3>RB;aV#im0#PYw5J}cVgXBiwi6^cXbGx|}|u;Dnjd*&^tV}1}P)huaLIhP=wB_dLC z%31uNWkBkA2YwYfj&C;PgDuE50pIrCFR{^4cgMfi;20>(+P(-aNw;p zyrm&<>vrnQ=&5yB?sz-&UGI$o2^{BB&6hn1+>>V~R7O%fs=BhLKZZLU4K?DJyv)Rl&e19rgaJw(V=UMphIQEU?2_ zutwWuxEHM2O$r>wJ~vy$(&D875);YQEbXM0007gVJ$UYu;n62UpjV)w)pC?aO8NG% zgMwc;BoW!$;+JSV%^2BN7Rh|F$ZjTJY{>uKxw+dttgN^f`6E*c2C?{ertT8ON_*DKNe|}n|siTrwSENV_IE?3|s(i6q$TdyQ&QW z9Bo>Jq-65QJ~IPs!$d@_tdo=K{$XHU#?xXc69es5mY5g|CwCklzIv{WHM0Y!h{iAQ zCwoP4dDrIrmIS;X!ATZe`qzsOm?^*on5-ht{n!l4_siYZdu&2HR~MluRCro6HQtU1 zD+G=q|A?7BJL7D<5=F^uYG6IcZ!K5dz7CBl7zgN@If>)HC6vBYs7pe__u7LsODrms?mK&d2#KY_6p@EE{auwzd7X`PQGoV?$NJtR!?S-s~oM(4^i z2}|+h@HYZc!?!-MQFreM@5T5b-2ekr4zqCy;UOG!kKXA&M$bfH?IpWkBH-{7j?L}0 z@vKBa{HA5Ze7+rPs=yVDAyf|mJOd#u+B{bS;rUz%+0~zsIL^7*F@lNF!^Yef&l~UI|^J8!ecS^Hfkpq$m@&cekEqJtP@zG zYW?>Dfb`ogkiExK56)l=O4{lutr-ci>=(;52v8R~;9Tl(=^-Ht13e{RF1@-|F7VVL zmy>jzbRh|hPr$$7F$-J`WDAu5v=CP%a1@C`qrXuTtZwRI3!Fd&1lRY1{^`4(!z0h}=E@ zW#O!TQU50@+%%I7V5-tl5`q661Ai?((R)nsRz}&L-p)5PDX8$Q&~@xe8W#y#PZ2@= z6FPOkTbSal4On-ptF7+E#iz@PvkCcg=!{zHQ+K4mHLI71y8Di=;S+HA@w{NbUiyJAUVUw>vO?+%dji z=hn^Op!m)h@g>HkG6a~ZV=&}fMHtzV)ZEVuHq?+wvR%Alzz*E3=VGG}Wyt;8|4*vf zry_icndumb?Os{xGoghX-P~$35|o-0MUbI_kSva_kLS_EdawYlV5JhBGS=_(TPqUd zWIQWfyp8z${1P%KlQ<9x?V`i^y6#U|MQVf<{|GO@UCZ-5v zt$r>Iv)jpP70>pr^YNO=wuzHdGQo>)eFAmaeKPbpx@@a@kqnq6#bdOwv~uVRvV<{O zYo!V$5vHTn%CPcETT~DchZ#l|hQq#^8=r>s<^{w{T>w~+va)PP(lAW@2W-fPvx}3Y zFAeceU)+Zrs$XqOYkQ+(tc0}HZdmUPZr$s^6)wnN$-k;o)K%}%(5K!v1}+e<_X>4y zzkox+Y6--7jxA4D;?#E4iGxm`l4O(2xdAJ32`uz|NuNYaRX6yn7G8R&nxt9t%7K&3 zvN|qCQ6$3bx4*i1WSLk6Z7xzv$xsM=05^PXmCuJuh~^i&3J(mQIsW~gk}H%c|@-}Wej}1jy!FnNXm|xi9NUR(5f(L zn;7@7gy(Tnir4q~y6XyP%C6c{=lbft;UggDNVEBwO#oql1RMHLTTNCZjF<_Ul327|j87Pni0y{Q0h zzXNPm-*NooN5~4<0O)?KAYkR`oQmZgtAlSZ47hRXM_f8T>v@*QQRS4ypAE4n$YbK; zyH>b>Ur&7P@wP)(b0(lMtA5^%KMfZL|qSjEGv2!oqWf3lB?nyhp@_0-)HX>L^39>fpCdLi4w z=$vD4Y)4esQa{N4`^TMkzbZn}z=Vt!2OO{DpxBK3PJ9s-t)rUe+Ur!K-g;yqoFH_d z0!ob%7oQA~Wk~W12ah~IjW~J!Ww(YlCSZ>2K!$=L4W^5+@v?m$whJ!3OzGLX!C^d< zd0N&sRxa}DVsX3zqR3X!#9$`)IK}8IfKmpyQ&T|=X^Sz1w)({YHWcaKTL)Tgl7HQz zYH7K?=RV8^VIAE<$$2LC6y@iU(YXQ183WMwQSYkPs!AMEsxR_cxtf~@gI|P6EUjH+ z@y)e3=!X>Kl*l4`AOGnBB-Bb|Ku%SS!$tLHP6g67MBYK8kXUNhOk10?OV@UmP~SXY zyO?~5PGR~HUo4R1QS#vb2=sGAqM}GAs@qVP*26mkvwlGEz=lFr<_)yHNVcnIGq+8K zZ5V{a)0`1FmQ~ePEDH>w10TlJ_I(Ml?czaY3k8nyNPy#{b&E@A7;SKru76%PKpm(% z0ret*-$12Bji>w@T|1%PY$vUTAw=eBxO?$+iyX_J%l(08g79HqJ~bHI*87qr=lP63 z+hB?RVBu*TOIKfD#&QA_f{9OP2{CU*>t-_e7)p5g!o-Uc9vQ_gNty3k1Yn*(kT5;o zMAqxt<6&GRhhG^XNU5lG)odAyPVt)D0pZ440H~EU4!A3XtPfGy>JvNV3nTWAcj4|! zkG+4!z)JjQ4*=yxOqOe|{68SYP0beLYMuw)VWW=jA$3X-QoS|3~ z2Bhg>{a=52m3nEbv6C+vU{Qqj_@0tVBB`W}XDF{EW}Xsa6cPBM0B?f_++EDD9dK5S zr8bXFov=UZ#wWTI({`~0SsQa+;7%X7F$MHoQB={f3A^6E#a^h#AYMV*4qDFL6OTU7 z2M8ZQ3`i8;Bnxls$U}41`{D0(sbYZ$9B{=uLk4) z-+$M>+oT`YJ2@_U#;a8-&tmFSbh&4g*WWoS8it0lNl$W3I?k)qbH(uRF-S^D{%!p? zG0#o_68nCSSHQ=|$ov-JVXeWc48Z5rsGQyV_0;6x1xrb#m9LyB>*VJ!ci{PQd!|63 zM(<{SGuRlE?%iB?*xRb!rfKlHzSx?DQ#q6IWDL9#1S-TCs%m6oM?1(QGEGg1gVBP! z|7hA7%xQ4&lC%jO*SP;%g9bo{xhYuo`$g$(t`ULhu?1(eH8q!aq*u_4F3;T_Y6Hga zs{-TL@2nAKDhLX{KELz3UGExw{I}=_W zJo@Uxeit843x+KA{rrr&?|BliG0(`#i30bI({E9>tG5H4rep%F_}hR3(bcTux#Y5t zsJ6(tj0J`(!6_p!b#QzC!w>#83_m?R%}lHqBoI%0ot{hVa`2+y`D35cz5nf^qf@Kq zLeg|5o;t=IJ5m(cUhgHww1Dm%bX3^c^mf8a%Ev88_*ZF0S>#PMHk017+w{V$gqr7d zHi+;ZW8U%0O3~G961^@HJ^es-wA3^HBZ(=){n>&p1~RKN}!r6~DIopSS8g3M5JZNI_w7DLZ&d<2qpgQV=Dv^L2Tv z1HId0h1T2KC{i##GTCwRnMP)ILjri%&*)-l;dlpn!?N+f^XB0o)8n)P{lQD+@Y;TR z@`3DSSVghX)3L03;M)JvQfeW-IiX3boUK@{7*f`(yS`?MNF>r$jw$9`8Gt`P01$1VH_1mjO9w&n<2#D=I3wd{IBo z#5a`Hee03%2n!7j9XVN{_ktga+zz;)LzsrnXYe{omO&F6SV+HHbIs5~Z9cwPnW&*h zEEqx-icRL*>!vr>wUI}{)AY%Rv5HpgE?r1)UTW5tS-D>7nXaY|`?x)N-8GaQA$v>Y9C?0Pmg&;6a~&`FF%sQ&xcn9kc*UTJ z^X*n2)gYn-V%RTEgPrybX=?EGbNBeDA4&gb1zyIF~>F5vo~Y*WxN zGsx%l7*o)Lj%BS#Sqe#*>S*88hThtuVtwkzij}!z&JR^V0$DT5!5iKjiGu{^xb?C1(st}peAdaG^| ze;(K86yt0>-A_sv%jBQ#kcw3XC$w3fr_&B>Ec)uCILk+09XtLwP&2;`)Z^NYQZ?P& zoPFr<+%xh5MOF8^gb4YM3%;0&b63r9f_V!AYpV8K?l|07rXW ztV}N2tf?Id?BMtruYF7FJu-Z9K<{UrSZN0U(j7<2S~(hNV%2<{4*wS3Aixs$zA7M| zdwW}eN}dl_nuatLtcR?xfAw&e&ez!68XDFwV*qhk?kbS<&5BB~K<+fr)ByOS+|N$|9gHylJ4n-7!HGt)}N@`cK!D)A)l z%gR}V7d&@4IptYvSaK4RK99XQwP?L9t=Al#u5x7%5?IBNST36`^QLc+<8bBa?6*;MVoh7B+eo=xLm65Y0h14pEu6A*M(N08PXE~V zVrWz+z@WD-wO#(0776QgUlBC9E}M7Z^L&ff@Ns%drMz&v7<_fk>Z*npc~oe6+Iq3g zOgP=#q^##|XO}bi?Fb$~b6%k?mKS^)#ymJ{wmkT=Ncds#bP>sS6@ZP{akrkNjC6dH z=FkbP9NW2M?>gxBb~mrQ)C7g{XWwXVkNhsSX>}(@>Fq@+NJSV+g=Jvp)PhFD%&{{Q z@bkK=n2`+io4sc<%W&RisEWz{_S+Z3BSNXJ`ef6=*3YNK>>uUQ%_mzhwpp*M zl&1*X`fmq&2=mindY)$$W(B!si%C)IZ)@-02qg|t4%VOg^v~1EmOrF1_3iG{QZWQ? zo=H>}OntoE+`$3`0e<(A1O4_ zOS9iqQtBg#!V|*_@^>+(bBK+5Y!wfC5RAW{5C4Xta}xNl6m<1c>umVwR;@LSc(ppH zJ_~-}oRc*~MHvbr?I#yw7oEW;MH%>fcccpnA_k_@{LZmEUj$~+fr4to5MLVTPZ}-+ zwxzsWg=+>^RaQfoe$$xmwO9Lxe9gygID7S0D5qW;7?p1>T|F-!elb_bc<9CF?BtOk zOlx>t9*p9vIM+-!h#$^_!noeQw}b*iMVj+nj`Lc2hkBmYZa!cI1W7$EtSkgh zmg~QpnNDQhk>A$z+biNg5R1*EuWp+-SMCk+o^ZB59dHV4v|MFGDMLp1HOg{i>b-sS zH*6Xad|qgLf9&7I{B@!&cGrfuk+QfMXw5XgOHE4RLio;XY3jBIF8;DowjG;;<39IB zeYNvuCg&M8XvOrIxq)R!seeMjb|SbpRPSUz+&qE#6Xt z*9+xIXo7UgH749i<78!r$wE$&GwIV1I%0)hw1H9#R}hkT^uN`H&6$zUqhR8Yn*1rmVh%JXQ%sp-jJ??VBLv9c@P47arXQR5n^V289k(B` z_NW7yRbis=meLL)hLgG6ghEf_E9}zYrKQuodR3e(m$&4y&aVu>q9rV!li}eMC*}!8 zHeTj81?Vn1(BdSAq#VR0*U(o51pSls8%NhaRwzAWXnDe^NhK51DimjS&2e`eHzjjm z5pHLTjUh(u@F?$hy5rf<%~V(k8k?*4!{huHXX-uz4}9uoV>s8i{V=pTy<8E;*&QW!;3-7wdHosKJHS7kqNUH?z^&QmRi@`tlQ}oWRN*x zFkZW3^lZBrgGje2sObpH&il3A<){hJjupzlb4p%4O6c`|#JgX3Jgi)^;BK|)x1KUT zP$w04Fb*rQqs#s0w-lDccpK0@)Ll_;=_92qYXUFZdUswe+}o|7_tN5FsS{3?K%1B+ z_0%#pzPC24=}}Jj`gSt&jbsPWCmlKayyDqPwp`VyD5Sv|G z3~&QZ3d3c-q3br>uWZkz-3ydHb!ksllwt%VeY^!8yx-fw9WiL$Onntw2d)Y4mV?M~ zJJw=TQl4JGkr(>j=K4l1YmKkB@3V+&Z%LI7xK@;%b&|8D;j)pe4Fv12cl-FA|FvUC zz_On$)trQIzMoIBvVJ`Dv{Tn_T5Ehv9NAygwxlYktfJwFJcdXD!8qvle0viijz`&7 zs5D2OdRYDSW+P+un`@kjY-Y7td_fZNaT6V#Ti^C-0Zr=YH``~&$zjbWuHvsU8drzc z2A1O$AMXbMG#PPt{7{lYl!WK%2oAg7`EWff=h;P{npu3P@F3x)=O!W9?&zmR=|0(V1){$i zPOr|rG)6&PpSj;Im>({JzMp8i3 z;^&xiA-*JydsrYeGspm-mhM`n;ZlThz~vw)+Xa#d34U-=gRtT1pv-HP_2g}Ed?cBd zx9H(+m+7~@qssHh@TZ2BSg*|r!R4kwyOd$B>-!UN*u;{taS3_7-{B6JvbePdCMXod zNI*$e#Q1zgm9atv0eTUIhx3L)au#y&mFnr!W@*%>(`<=xBj4=_q z9?mmn$PCiQ-(_sWiO7PUb{KEVRaR%2r&RThqAqyz2LT@uCOOIYR4E@X^O%@g0@s(} zN*um$79zSI0Wx-AIKeeFnlEWw!WDl2x)1JM`SjA&?li`u!c(KsaE*yP@!+tz1t^FW zH{L2mNfr+g+%sZDi)xDA?SLu(Hn$$?yPcKsj9^4{#h|Xf;W!2Ce@O?>EgO|~E=!wm z1qb;!0jtaNsp7G|&DM2|NnsKZwbDX6E#IHta0;#7dwnCJbBf6E_027_^Z;gUcVXto zVX2Vle=M4sn!k}ykd+vpZZ`L6Aew?!dd_^jIa|d9_VyC~3@vEvoKCpw*uZj>$i|&} z@06lP%_X1_@G7pGIME6#iIb5JQi@7IpUgW7Vnz)fUUnS@-u&b&E9H6|Pn=j@bh2Hb zMNwk07&(S9iPCaaPmTNzyCq4`c3SVEA30>fO2Z9-I=%QH=XTQ6JKB`?+s$%QD;}kc z4C(q*KyTiBC$2&l4;4xB!yT8#*>GUUy7k+l$L!!h|KP{He2($t0b$wG#lCRiV4<{b z)$zl_BoFyC{~i0DHpiX}1ZB8>Ll-Fb;a>RdR z_h|;@`-RAtE1B92+X0y@eqK(FAuJ3__{b1hY}WPBQnG~^;B51e5*1SU1tCAY_p3Sl z*!pzi$~UzTiS?U<{DQh=B!e4EhB$0Y9gX4io5C3JF3`puL_)QLvH5af4li#> z^(E3o;9ajR4pU6!@qLGp&&_2;nNWZobMyV_J=M84^q^8E3NiZ5l|?~QSKBg)@#p?+ z6PT>umhv|DU=x|oB2fGLo`=s;-FZ}$%ZB^MR~KW)tX&=&>3i=d-FlXv-LKk(MZ_on zwr4#X%r8&v_z4bQIOO7%Yjxz?MFkppQx~&8kNJCe;8%$clvpgOQivQC*k<~lU5CTU zZkAaiS>F4^XNRDEMmZP%JXM|+LEN3Cv1O0$R< zfj2GTikqa5Ys8?#_;VZIBFX}vmdCd?okE#bkCQ8ox3XZ{RAv^@c?UWp?fdT691EA- z9%{v5d&j7`0=9yx^ZA#Y;?&gMO&^Dxwwh1IU=lEwrSW@5{YlMo^sSi72GVAheK4RE zL0NwAFHPK>0@xffN$F*8cSy+W=KWa$((UWyL_i~*40}Oz2-P??VM1{@sk*@HSewIn zv;zaUXR;_ZNr072oy;lR|^hxsYXg*@|tLhM81O>ydlkzQ>GT~b$V zaX$f!WzSoR@79ajIH8%2oS$%S6|8P-fW_BTp%U*m$RnGtzZx z#)31VNi0xhP28tR>)`CwW+I5ncb7?dF${ZpVB`Jo>v3h#URa-A(_I$^f!H&a%q_Eg zsgkB`a^Xjf?Pz0)ZUHxEMJA!ozIuZ4^2mqhS&2wlssxm9`S^?~Bi~gk`H@y*T#T6$ z&9eFG9H+^fZSTE~?d!JXepD3e`@LRQX5)GLhv&0(g#xiic;IwIO55%47j<&*3vpKY zmFT{UcBBUB0HZz+V?R)816)t6eP%fgrt>rATUam&&Ypw$Y%xJt!M-er=3KKrdzL3; z9w!b}x@USKe6`HWGsd7}J!Kg`lPLnbPM?sUpMST`dTsoCxbzrrhAIqu6~4MLZ(<@D zq3-9RK&xC&1O8PsE;ldg_D_;N;a4r>>qj zY5?3m7V^{yz{yj8mH1y@XMa9-{`8p>KV3L^>iC8yR{$qY{dD5wiBs2pI(hp1+2eao z9@D4K9N%{J+VvYe=eY!xlp#;8{QQ$sQmN(r3?`?L%&&KzbKg}or3lLc6}0TVQ0TIo zB69L(J|)LJ@*cA~=J)UH{(AlC^of(F&K$Q%Uj3J*lP8b)ojUz5G98zGI(_D^tORAw zatht~)s#zCPV2eX-J?Ok#lMPv_0&~>CSdv7f9~-AT{)_}X{4JP{Q`D_+B1(1i0KLfw1{r?D-qL=~m9%e89H%)3@<57)eeqCJD40qO)r1$~nA)hjGM za?cfv-q>OX4_2n+$DWutOHM((#5Z4fO7SiO^MO(?Lp8Th>3$vmW5_hDB4^QR+_L3N z=Dtevxt+(L-aN5nGm8xDW0GpmW3Va&gAIx9EhE99VS#{Op^Aw#hXnr#_av}0gFSZ} z8TY8Tm6#G-!rU(doBKDATV5wX-4+PRhk}UMF#=z15S`K^SDd}qo*zmYQ0B|ZFOXt} zs(PqYS>XpV8mGOscTRt%fM@6fXajL7Z5lF_=V!Lad^`(hZI#>Ma2Q!i>a(_gf|iC)I3`yh8B%)NEsV)HR`I5r+8}{ zGe~3toN$j`Ob&DBKj;7QcNBmD?gx1rC=elgjkB_gd9qf>inJehAS==>cH^VS3k0!s z$MIMgD`52eT~nGrzVPhttun!W56X-`I&r~JyPe)|)*n;%-Z@V+hpN2-muH$;g76yh zVt7KP8MpC4FTs-XP(!AW$k#!-QnXE48V)VBb|{tN|H2Zklo^c|ETgq2n~9I<$5MnX z45GAVM@y1Gy=D11vSGsC&V2s56;(M6PIDpt=;PpW{!T&QH1;PaQd+BFVNMrjF?pD+(FZD5wutBG3#vjG`XbhUPFcr zDIhCLZYA>~+*o1F9p=H@BtM41b97 zv9eG_n$VA{;VVFCjX>D9!i|Oen3=n-2wE4 zMLhkk`S#D`Z~r1ioI=WbMoT87i9lmavsARBpnIj|3(jQT?PeH;i8ar)fr1Cc? ztOuOaPj;HhL~-W6%}7)Nw9KbotJH01Es?dox`0>is?#QBdX77f*QonO07bzzxP9jt`Km8d(d9Oc0mF6%?(l*DG2hH>$w&ftl04=5R%Qnpsm~_s2>(? zw|qQtNaBa%6D`87wn)z?P-?EXet^~c_XKhczI6Ise96ZuRn)QpvXb^Na($@FRZad! zWyVCTqmf=Xs*(@>cyU+@XUKxqP((-6c*EImSTNDr*X#$S!&}xK{Do@vt6p2 zd9qjjVD57pVFuC3cQY)7aB}A57zPXf7xtiIEgIBYR#bXSZHj~XSXlUh$=iR%1i5jF z&|R6|wVXkn--`;6-g&4}xR(*Uv_nMJRiwVXaH8P|;M&bw`%r%LJ44B2;&140{~^N3 zf>rTw-rGr6J}76~leF4Nmm#*2AB*R3>@@pS9lE&vqVv0C0?@fJ1m`G=Ul9T7Tj^T# zCXIneo`4py5#`~}gN*cx@S)!E{_{rw$tAwDkBV^|&d3_3rf8;j(n&f-=Z_4Cd{C%l zoY)wZF9Iao_+XbFP;t;6%iRi-Q{+da>w3>aMdcnW{GP7Vgg~OoOihF!!f$2VTb}fV zb>yg!s0pbS(w`=S&o4S10g5*6*ZrarHp@R85cG_dUNw8rKMWV*8l@|qeNf$|L*=&o zUYuxC7!~E+;*L)kDOp&><*HHn6TkE)*V2Bo{moG=^r4SjcsvC@DaCQ`?Jin1GM*y{ zKIE-k8A26u(~*Cd31L}P8zLn{f-h7|luFMc9vhU{z;9q*yr8Cz4u-u=qsbmL91>jR-?y_aF^{m1hlI5#cf~{db$h&$*WrRYew@y7}m@ml2EG8R>zh59XPkM5kY9sn{M8wl&$ zU6=j1!L#R=ksD+-b%yEP+_}K)Lnb+AUFRZ0&Bb)U38rc$*PRY{eK&!MpZ8zBufoU6 zcVK;e5?{22zbh*%V_?4JU!q}n8~2A5ZKO5d)*ciBo^b#69}D%rS-5`|oa1AXH3~#9 zx2~<)t^S6x+LGTHUV{vWc}f4mbt>wRo(&BC4PIAY=$X^$N824N9u?NR1Pg)#;n%7qcOIJOia7`YF=zSEy&-Lqg0@#*2eHW@Lw zqP@AS$X^GG@dl(9bAvRu^!UK~lw)g*(o*rLc{B^OqifoRXR1L}0J3`Y&aAaYR?mt? zfa?T%<%|t-6gfdIHzd|py`@{~Ikb1tmANXZZe`_!lvJ)}`_yw6Lhrq9Gey#CR6i?- zaFiCeBVk3WF?v>Uwn8#0!>BTlVdScP3qq=7@j6s+s$kp|RFIm1@Lgs-#=@cI17vE9 z&FaM%r>W7)(;g}9ZyraesG2!c_q7m_f(RPsUddYmj*{a^YJGNF1rMK`lq?3=6*-&Qrq6;-;R?OR9#28m4nX@%v&si02xpd;9Xeq;0_X z^-$!2pX1hCEqcd=>(-vaDVs9H1ze>%sU-y&(KHh#YU|U+>05mgD@J5 ztZ**X-(S5(@!*9EVflR z;&*3713kTA>gSliewULU+P%+@TW9MwHj@+X>T2cEUrD-wuhOdZ82(MUO=1$XA(D8CgmSx~2lXvDv8`GgS# zVclR7yv|FC4v0lx-=J(a7Hy(;?bZ>X%qy`_Eqw+g3|!jVoL%y*WDBG@Huppma$>9` zE%SlOyTR7VrkWfPV@miKH&VWOmOyP4Wxhz5YE)!xKkYdFjHxvkt5RAv%q#3hQ~M3@ z&EQYV|Gz<;_>&QmcmzoPlaT`W$Y-)yxI&ybWpv2z@apj4pUMBq=)@ckiitiw25r)#tZ=oX055z%|=dqk1%ksb{9Wl#i%k6GSd6(Lr zMu?^u7};2`%LNp4%1a~z{Nccru+yLIDvaaZ=qt?cCcm>$8G%=!QrVzdvl>boz?q7j zQ;euf)TOIo>zAr+hEM?ooUh-BGJx5-z9`@41vj}mXpJS%QXZ4m{6wUB2|qWqhAYY_ z_377^68m4YGN^dRO2_))W=^(5csR}B8fa7 zNFqch_=-rG^3x(!|A$z^dTC2z_=}sIgwzN}=FeBQ$Z|-NQZTF}s%St)RNXaWMAX6tj%K3_v5rotp!P}jn zTh>)g;njk4N+}eSJZo8qcp8A94PubzeOlr!&cx}qZnTG#nta)OrB{%tG0MD74_|0E zdz%p;GCswHO9(Jj5xpcjP>}pebs`h%W^zf)BH3Na*E8Wo6t&Q8vgx-!@~BM>pCy*- z+5S%N#d2@O`+kv}Z7JeeTbchJh&Y(X&}xx;WE%bY8H(qn;M1XBgL8Y;Qj2Q>`vYo9 ze?gyqGQ7oa9oe&5XbY{j09W1nD5Blxp2}s7VAw^-+(+f72T;tS5f<3r|KmyjcMAV= zPxkL|(@edkT>vwFe_3Jd20OMQL-MkqF9GuUy#>MIwFm{2NPt9R-Htsk`~l$n>Dt}E zxmZ%Li-TJXWja2Sz|>WBV5!s{8X?svU~!Agu50~~X-KoRc!j>0Ai=UBz7^BasUraY zG9L#=@;>qVet4l;2NO83n=6Lr5stt7@WZES^3|g2JzWzU#3z7n})O!^9aW(8nmYc@7_nq0dn!k>!=ZqqswOioJle4U>m7UOk`+W*oo_jQx&Ba>}+8xyl& zt@SyT@6+7(w>yzSyehEp(N=+1%LNdijD?8P(f*obUN2WWrI3 z2zuCaB`i}~PB2fCpE2sx&|^ev>m5gm)b%`qid*1;hjgr)_Q5}zD}USex4=k-&9k-r zduL|xojOxiHZN~N^qUr|;s?xwD;ubcmEg+nsV}JWMzq9t9$ib<^D4rH8eN*!x=8mC z=fUuY7i|8FqfDB78>#<{R=id*kNQ-y5cGihsh2mqPXVoribBV5O?!M=cryL)?Yg23 z0`Dc<6si`^MNsPNs2J?i6)MAi{99T74y~nH9|v_vNGmK<_521=YwHv$^E#LPB|%E! zmEa>+LwK>Zy}5^+E`+p&CiT`-KPqy)ehkD|mem3ME#{W0gF5bcztq3NWaQg4CJyPtrzIlKjzLY`&(v&=3xrGV2rfL|k=wBCqpCkr zJg_Ty8pd1quu2%@s$1J9yUfKxwwnuRBW0!s+EZ_upb?BLbB_}2a0%J_b_DcJk|`_9 zeyl7$m7PKKKWuLt_PEVbstFdwa>qHFJ_V0_C5Tcjp$!2vx}lui++fg&7kDTn`FLIR zmK}RxmNnv_J$aJ2iG3MKJp+={zax9LxohMyai@1#R%zP9viO_94?kpGu_BZX-cK0=eKbj)hGR0&Hf<7t8Sl1y*wRkRf_!0rgK7D|_Ko6JFMjH+$Fxw6P@ z(G8ozB~#Ro9yuNaXj6PJYxdyaIJ_eP?JB!Fxh= z&|-nxu8ZnLWGcF(gRu7dLR5*gdt0cJ$e>KOY&RG}E<>K`$Tf`Y6>1J)SPBYoJcq8b z_iD4_tl#!{+#5SkrfQsv0meea3G%z(Fk2MjMaxQ zJG*vyCChEw5+tfPeVd66y7zxD>IwZAQfg;iJeZpA@T^ISr1ok~y#ojt!P{Y(Ul(Bn)7+O6EHPLm7ozrYg`FBpEkN4!g9|>Sz?- z-!)iD(kRTBAq^4>`{o>0#xfZ`Wma>x18KREx-Kz8fCr8zV>d2OHW_`G3s;a0Ixo zLe?9u(sY`4=dhg()|+%KoA3UHW|vy=clmUU@KynRD_Ux-5hcVEroWh4#Bgc4R?`@i zW7K*&=k~DtrtCzTo4=CyEJ~ysCFRa!>+zaJcP55I)Ps4ETnAGP#snS89RWOs`%=iVK4ABu-XEmw z2q)bdMo2IrlizJKvGbB9<@FKZPR&EdP0hf$I_?PO{?_wJYt^Poff%Q9$#S;nZwuIO zPlsb86YotZO+DtU?%Y``IWxHCpCAQcm!hc?Ktbk}aRZO~cDoB9rkYk7-fQQpHTpC1 zBSS0x;<&XZqMKn*uW)MMP~D~3L;C5olblfb$BeWMYU!26D-9@u5nPhoU*u$wrOV~I zG&~+q%d67wV&q{2Vf1&w@e9-q`r4a5h;zO!H*2%e4e5sEZF?{2FB4+Ny1&YulAns3 zzfaifZOfc0wWMD%FML{xm(J~=w;gnqLr7-OQL9n>&gI?QxDfBNi8T-X$mDiu))`e& z*%Um4sl?#8HmM>R=k5+&RQQ^?pLU6a6RttKUYAw#ct3%wnHx6OCDzT0;>Hl$Q1*@3 zTIrj@d+tDYxH6Urg?cC$ir9=c(P&ee?sc6N!(&>jD1@l1d-HH^FO7~eGV`{ai%~f- ze_8i=RaL=kobE?7fB5&)v;K!p$j=hJ%jTl)_M=>hxf6bQa#HbKnZPIW zqEkaeQ7wy%vB5E-J6VD}pofVOOL5KV-`%m4DCjNSZ)hv*2$MNOR~vY{W`i-@ulq9a z)PKFVc8W9O&gA?=-K5+~+SXN#F=e9-^%?_Z;f4{oTWnC|Q*_tPab#!Cp4;Z$t4GGa zCJf(9vgkYlOjHT)&U=)U2N*pwl?<@jWE8X~s@Ca>McK`eW*-fW-pDtLFYPqhD|4#* zRoc9>u;KyZbjPI+8PLtc@s5ud?Ib>95WkCER(;vfA<6YFMJ-%t$N`7)HVBfv_0o#e z;m!MYqV8f~Pt#mnah!|xFCn70GRKnVOg97YM*wsL_Kr`mSr$m68>qR|Q}pT7|G(Rt zW2V5yaf%Eftd}Ey-2=gg@RjQ&eme?f0RlO6DjVA5~3j(J7B6%r-GYl z9vVcP5POXH=(jt)EIl)B_n6+Mc{uHjd%Yd&p&Pl{C~Ll zWR2;T1phxG+Exxvow!|Q4msM zNcB<_^-`f%X2&g(DjmNSgUa+CTnq?H0tfBjj62)Ji0p>ppeM8L03X3-0RrDluhj@Y zX@~Op(3Dk%wEOrY%kq?Um0YutdtX=)(9&?wGTjH^C#*@(y@jnp(2#V$)>bQ>g&17<3yW za0iu=&aIf&kD0jo;XexG--&u#f$u#6JgP_Ex-B9MM}vMpubowm zg`Z3mKwKi(WUdgBK|zcNzU+d-y^KY21@xuO!Z!TlH>%qdslm$yDWHI0y8n6*xrL#D zLwR#4Dfpx@uw8GNTL}haUZV-hdhS?{Lcjmw)qc0isqVs;AA9P#EK!b<7~de>+@TKt zIfH&1=<1_4ZEm`z;KSp%w5vnClg~CsP@N-TZtb?rs1c*1!6^6$a0}kK4m|=CkW9rU z3bjqXOm=dJui>n_Qzh6_0@9zJI_Ed-+n}+g*XBJ8ig*^?=By3|ehf>_+q|uFLJcb) zXvyvuG%MKoA(qKfUg*)okB#kO#uEdKIc62Ny2G0zz}04{C&G7FTAAK^=u%ZyKZ&1h z6wF!V+V``{M^ydXiBHD9vt2;;I-}+bkSieV`xObEpuT6*y*?6LJBc#!Rl<)*xY2pr z&G3v&=pbIiZ9$=rgWi2%_Qz7{Wt)VS%lfNlQ)EeNASp8Y$iip{1eS@H7eH{3)e*}+iXB86dDGqx4#*S6WpaI0v1@?Ndt?;;fzIY4K4$p zH1_I?SER=gT!jH0l%5NOJMwdhuNlck4lbpT%lYd|;kli+N2`m}heLtM4ToBO3FOZ< zt(rC~R5G`CyiIyhMyA{>vx{vqjDcpJBPzTbK`Byvi?|yl>*GI218WVG{qFeMW=;=H zle4?+ip+;CtAvK?l47*L?%D`xtIHLPxbt%Ffg-6jYh@_J#h?_nE)2j17n-&T{9&r#GJTZTa6z(euaHVeP^)VS8%;iBKw z=l5}^<__Vg$20m3*}rsAT_p`7y=MH%`R;O>| zoe|L4kgvRqtvz%v;>XODCfKN?1^%T=-SZeUOT-VD2uM?%y`LJPlMqn}-_4g;s6mTx z;v^4Nn8k{@)XjPSwNSEF637oPz5C%^qN4gvK!X4#*gft|%rDK-S8@gtz0x`~&z>!@ zopLDDO&65uEm*lTkQSxf+R>BOlPC8bQ4T)_Y1N)@FkeRXPT#d*92SIG+_$c7KhU^U zE!QN`-C1WmbePeu+#MbZEvQ?3C)fLa3c?1O-q;nGd@42UcZblefA5oKK5f@VDocT7 z{N~VTf`xMXlx|y>XrjC5>SbU?s{3Jv(I?|7Ka><+Pw-!Q-e?_^n8Y*?j$R=kSo^!} zBp%%wA9tB0jxwzBM4uij?Z8Tlu4C$(3)abR1evd${cbzeK+>NkL054UPx4y@ztNqhIYI!D=0*jSUl)jS>Ux*}T6>6kTGYg}N&pBpTCnEXNGLsisc zX0R!1D8J>G_k~4%P8?Q?-u|%^bUWULv^-h0#Y+KEx5Ce?Tyg(=|T8qOVOp zoN!Sry~%l~nr!>-igq-K_Q}dNa4m8-EPw2+)m%A1)DT$}5oJ z`mStipMEa~+5X+{S7g~8;a@YiYv?_~QsTUQMJ~{8g`fht~QybED zh+yK)gy2z7msH&paW&Q1ttO_0ijaoWjHx6y_zHaGAIJNYv;ObXkp6Y^-%XloqJByB zOdGR`IwQ9_L8JAEA|e;@ObCHV5!HDQEGltb!rWmwh*?5QtIEC*gmg;f3=iLf?VX*& zOs~p`fSocp?VU27L+)(J$!X*YX->IpoFz>MojOb3rZ5!;Sk9WSo^L9c3#}wMaX(6N z6E1!&DT!qmY&AiB0sU7VH2xsMm7`IQxYJ4y{>q|@!2 z_`a&PBA#1{2YDlUR?D0f-a#zVc+9=9UwH#XVVLQU^8F-KM$q@Er_DksC zCx>`mI|FZa(OJFauCpT76J$DUSl><0>UY;`;GwGUO;Kvb`iJt`w{t3KI7lUUhM2|J`15Qu|nEd zq&JK)a^|f?8kurn!hJ2z%LLby7n`7s%6d(?8Rp<}-BH-q&o^{KV_c(N3^au0M|FSm zyIHmx2L^-1M=*=#KHX1`7fC2b_?W*Fz6|KzP+TPdJn^&EljQnhwh;Rc8UzQK@82vS zg>`KD(6?yk`WHOdW|CHgb@Po{LD6eIYeJLLlLQeV$h!)7cQz(1{xw)=_dvihB5sJU zFkoF3Ms`D}lEXZ=*3oR|k+_H`=k40qL3!)e>p%2Lz=CF+&j4Z$ZAKv(5Df6_4H7tJ{IB^CVxL_I7$}26azn-*Uy4ehjM`2&J7>z_Uj%~x3;%zs z;0frzl_z~MFtLf+HR>;yfIb~3cct(a?1r^J&te=OTs&0(NE?@l`udhFYa7DYFFb33 zshteA+1I%JYj23)0yOC+b>2E6{oUu`Kr@z`eM_t;WzA4wwrB!+=Rq0?HlbhgT>6d2NBOceXOsT|X*ptyq96KR-7N@U_P3Rf8he2qB1o^FonuajYc_da2fE z=f-?WdY^8}u+f^F0u#YnZbd|{?M0L-kZzO)ZHf9c*ReX?NZ!Lyrk29%%?3`?YvZc+ok>XP$A* z{qpa8?scnf4<>8#hxcQ~A#>ff*_&30vxf24%4+<*r#WZLiNaQw1>aVJS|j%ohKL5p zTH`Y8F%~aO`K}0BlKkPilH{Ut*yrEn$AyqG>}W~Yh7Ee1sDn5F$_!Y0u zmn4WXbPEnNE0U&&8u#OpPEl1CKc^Hv9r2m`z_+$Ww<~xvz{{xlF$s?f9vES`1eXDQ zRps~RbDU`mMZ3^gudQLCy)W<>ROBu1kH)z7$IR96;w>KG_Am>?u~L{sp`p(~mx=i_ zP9=O2+@dVmzT_~;@b^1+edXOPX7>3v`W%L@X_NL2KP-QjGBC{~cJdCqtGwyc~3!$7N5nCrkLqh~hRpW`NWjCrI4Dl3$2+gg7lfBy0p0_QGC7#Cc& zVG=vRLMF$KM3YecSG{^HHMzP%@9tZNoQG-5!JUg%dk*W8UKBe^Qg^4`zzE z>KkO3Mn$E@h(MflPuxVEaEe&@xQdlR!;|b<8X?tr=Sg&#iYjA+f}9zuK#rQdZ-{&3 z-U>?!^ls09sk(F+%C~mP29Eu0DNPED~@XoY=6qxd-^EU$GqzsSEq z5CxSyOVVXHwQS3Em`5D}d=e?TU8FVAA&S1T4R)je>tPncZ(@L@M(*u0Bph*_Su(i? z1YbY*9*bL!xpVh})$+u~zHL}vfa~{K=?j-5+x}sI_MRO6nbLngT(bY$pUFQfy`M~K z7PuGkjTWl?xV;b)if=SF&U`Hsi?k7`x$5g0Wk$#_1#Oyx=oXu92alg!3G^46^9kf@ zA8DJN`Of@fn(!O}qVzZi2~g8s#BfTaKgOhmN2W)`_lRGvdVpxAVM&XPL=9RmJp#Yb z>1`cjZ!$}CeiUJKAlq>q+q*Z-@inT}V};w%$4_hg-9y&=P{6tx;Z)V}!3+;<=B0X} zhvr*h%4@`;H02~v0FEEoK96>9prDdCnVG1RVT7&XU*aMru|w$ExhIq^* z8h^_Y{#1&~(3$PtiexIRwy%+G9avd)9dFr^2Ib^>!G1x6RIP5eM6cVf?#aMBNv%)He-0CP-WsTg5`F<(nP zEz@;avULdxk8@$+99VjI>f}IZX$vXUYO~WpBRP3d2ge ztm_;dtZ0Et5^0=S_8YA>VDBT37e|Q=Gr?&`$|;ndDR-yT0F5T-Jx;YQ2{Mp`P1`L= zjR=ol&Bryny^DEGafc4%L9VoOe(?R{uH&|I0i)^^R- z;jP?)s->A#ikIm8+yoBB-`ijwilq{g4PP9GJd`hxN+)t3>$r~CmA{`JBim249n8Xyp?fa5E}ASF&lR}>f}Z>> z*S}Bylhcei%!5vTc63~D?tM^gwBIG~As3sf9%nV&I^M9f7)+1Z{Z5$cM=q<|{W4TW z8D@s1?(1%Cesbs!#?cBASfGBZD&lOAwTYM1} z9{J)>M9BX59b%|+1b6^A^YFiXu@fo$&qDAFyVNY@Bf#z8o`ZlRz>^0a2e*$wz0c2i z+UW-v>Byc*^YSFhk_{M@aJbw2d*Iu(ev^acOX~5*M#Jy9H2OpfQ|YHyem^u{jTPOg z+xSss|6AGCVRpCElT*Y0mF0ht;ke@DpAyK0k0YjkN-#h1-_iJgum69mlRW|unPrTv z)!+XJTgLL9z}K;C%XT#DLziHn6ZBbN(Ix3Nthhya_P4G|yM^!`1u%pXRwVtpRTGC`pj+w-BBQ!azl$!9i${8?+bGSXftdTOb#YqBSNJhS0Y%;9E8KpY-oi0C@R>B|DNh%@~z-^7BEGgoRWnbgyH3cN7 zZnFDdW{Aoa%*uEpJB)HE!6%`1aid=6Ra9q(mh zfy+ojqzdRL!?oiCWp8>(k{WuW$@ave{A)PH;1Lpzi0M$MDu@VW81X8u}L%7#5c+clb0@plu0(0Gad9bjfa9lru#ba(Q3YMJ>cEbrgld$cF1|v(f`5tyEV&w}FKyh+IaX zKB(W1+tLy%YnnR6>yKhykF@?gPVSgAq74_9O(oNrccI9g@WB!`=?B^Gv{)ctybDg* zx8>d2in(%f{Ejm#S8U$4GZ?&RUCd$8*O#MEfs7uQbXWA^zY{Se%igR_!;}nV4B7*W)9_uBceG(` z49^dNZQm49zZQu*jeHl=FWtW+`r<3b%A`uX>b;*>=CS8D5*}h6qX*4lHnrdHF9naD z!w~zcN|@KCAM&6LxGy((q#LqB<`%EjyJw;j5Y3X_(v~dJuW=Kpcc3JhFox9iYy%Wh zLNBZ#WKu9MOt!$MF*6WVn|}xbZsa(y!n1O|!0d*@_MAAPP+1!hnmoGul#<7o=m)Uz!_OQiX%Z z90L|UXKDy>KMFM(3=l<%;Jq|D#Yd9&SCzc%OEye4bgZ~qTf=?}`(S7ZcT(l4R1{-7 zSc#19PK$s?1zsv&yC2z0L`6*Ag4!6=`WUaH4352(n_lbEYa<@fbFbS|4)x;dpzY!t zA5tTLzH_m4nk8<5>tnR`_Y%4peQJY+D+*#RkV3G(9jvt=y$7wn?z7Vhs|WY6gIsgH zbi;0zND?8wMZ*Y5M>dck_a(UQp=gLjS+IcOo#bBMZfQ27y;A4pzDl)Q=Rt_jje%=R zhK^)={<_FJoP|%bh3)2wWI+>O^rI<>qrq#;TEa_gHmhTCaNa6EnKc!F4%udO=yQ&T zTL!k=dlVhPpV$wxKG$s!;Kg=g3V?V?Fl{%pDpQ~|2c+xiys@P3`~<}3qLIgLUFp6(?8gjqp$zo38Mdz$ zsYk|VjCPQDt@<=vB}OfGU5J5ak zk8w(AlDe9iJfwQ4?uiv)Exi#ORFwTG!*r$_i;&()Fdu#hEHN!alSF8f8D^DXC8o|U zVZM_Ax+P^t0I{CyL6Dp!k|xEVE>9df8=?(m*0>RWoM%uDDMcg$ zlTx#{D6h?U_vIr%$T7+@2KQT!*yHNg6y82NZ1CtTUnpJ`!`d95T)wVi`7K?3$owKJ zKT}>IlKuRZ|!aQOg-9~sDEubDQ2Ie`vX?vyy#Y92EGkK;=j3gn5+h8S96zp zUXE(npk}?k!1rch*iGbg=G~Mjb*|#X{issJ))@hCQ#4_^XMp&jZIN~VIAeLN$d>$S zC}XNz5a=Q2ubJWiS)Bc&sRZWe5TLvAM(Ywdegr%EY!upR7xI`$#3;6^pMEfhrQ9mX zS=e%9Aq7{$inv%lJjE#9333r5gLx$hVV<@n@V?$dtz(Rx9Dl)*KQ-P?(E1m$wqegT z#$v6FBS9-=QXFyw$k2?PAbLd2jQQRtO6!}Xq5?9_i4*9iID? z)v<2y$G%{Tf>`rll`Wu6Qp?V893ll?-?>tY7T~-&7AAhytckY7hM5z3z{8h(OxPvt zt71ip6=>Z0j$=3B%mBLfoL6e}FrSkD!Dj1OS3K3jg90oYUP=pGTFbRrRJ=E?$a4XM zuI!HY3uuDEDUy_9Ob}|8<3|2z_}4zm+@`qM?mq%btL}MyX_S;5XESeam$hDzQYefa z3g7NS^lGnH5U!_CZF6BC;vb|O0Wv9YSO7CA*HTtFr=?cb8}^F%PRrQ^=k7kfHoowN z3QX{#^ZIb;)ro!gA`+|H4X&{AaB4|zX=_Wg^28!eaYI_n`l&f=dreVOCrMm|o@uVs z%x?79^AT^ARYe1xn@UuTTAKwvK7VRm$t_|T`6ZT@4@bI^FLPd}Qm#K)(4!JsAIWA- zTICR!-TLjCL`&c5QB)7~1@qGw{DWPzh;z(U_%xfR?%oHdc*tp5+pL=lGz5M!9w7*n$IhgVup7$!3 zvaGX7KHSis&FPm8^X+d`3Txhvzt+7er61fRl?hr-oGNsywrxi?6X>RK&o}X$m5njZ zFmLiz__bS$#g7*Ck+EQTBhkVG$qEP)P+>$++RU_h|E`XwSY2G&o3VCTuX0lH?4!Akud*r2T zyB~!1;%B(lDRV*rxqC@0F?&w`@%uLRPB!pBV`TwiV?(SXPVz5;+U1Psy8gwJy)7LW zUlo;FSkVU^>(19j&NKuy#97nyqNd)u)l51CC>4wGrLaFZ*p|xREk)nXW1Rd9kAEJZ z6_w?g#4G=(H(&8ZrT<+qHNERky%J1LbAkvJQFE5NS}XMl`(CV}oJm|_GkU_kTZ{}W zx}Ms*)V6R)y*u=@Nq0g5zP!y=>}y`hd%sF2#Gsb8nr(B2}PZ&xqXAKWE}N1 zqwdlCP~7)`a^Rf=DlB$XE>%R`UT6}f>y_#D_9pTE8|tD71nN*HK<=5tcTe_fB+_67 zem_@;9Uo@c^x%?jS7zYK0*o9-#OoxM1{JVl7M!`TfQv0@7~cAOKhu1RNx& zNVJS0`C`&8Zq6-fH)1k`QWALZuG-e+w#Hy+kY)4ECLNI>+7Z18?#Yt-Fxy6`O3M7| zPIa+R8oM-~2bMo~S*pZLa97(KgDSfQg^W@ivDw;w;uG6lGGaP2 zvV3@kc=S>Sko_8QOkWg}aSZ%LA6RA(y<8E#Hd}3D6c98rk84QMl@}p~dS2Dg=PS19ztIj`l;El2WT=u2h=3}+KV(do= zPx15OhNh;n_fv?f*RIfVDY;kA(e4~Ccx)!&i$?&uF730=TO8hQ59l5BqsNBG-raUP zxZu!ycJ^L8FRzj4_qjMiSL_kPEU<3gPosa-wL0knyN+gm50+AkiR04+X%fHnhzL~c_-|YE! z=A7H+o|${*&i8%a$j%Tqzfq-mz3cb!1iD`;>L(TK$9YxVC7EyBkN<7rZ(=N~@;z{7 zc-SsJoQ~1CJE)|kzhR+br5h14jrbyi+f#%_EgA5RC=*X{I_@eU0rLI=Xq^-xcHVU- zq13EoO-h3DcQgE259u4G=wT1kk+T};Rc$9@_)`n`Ew8V|NK=R zIR5JP<|pTJS3>X6s+n`Mu#W%^3flce#)V+53q{?!KwWqBTYtI}`c>iq6bbVD%%%B5 zh2lHtb%oRLO*xz!n+D-%zD;HmM=w!ZvdG(|GHk+Md5M4Ra*VQ_bm7tS9l40*a@LB& zKd1TUZvKxeEgS0+Fn9OEk*%TN@b&Teh`m`E;s;)4+oJ=W_?*NP-!(-9QKu~^6`-V! za4MjAJ7g!-s8cxfj)XR3DdwRYOa%aq=dfmF}x@r$Q$ilfrz&E_zOvy4p@Yrs$7)6VkEnd1wOU>Caz3HLk zgW?@e7w;rQwF$r{B5PoKwSRZOw@dW5cPj6h3s*g&GUqN>m$1D(h$$gQbc) zi^d|&VSBN$$QwcsdP9a;ZOyRg{0ENb>PuIYtb`hVtI4Ef5Td87}F?Yhqw2phL`^1WSeAa!(f`(H_| zL#(8KP&U%`+eGXU%2lFb`zL_pWJ~ok>otkrryK* zx%tf!ZZ_ZBG@^f^@)Vt$423`9(y3+81oGrQ%@w=Ndv8|f*}Q1M$405{-om)RQyIwL zch}pIo}h4Tuq;Q-M$e|g@?2|~mzG}EMBJP8vCz|Yr;Ci~G|Gml|lR-izBqS$!8Ju-35lLqjMc@K=(V9XIS zFNX}$j^+(`nkBc^1bj&~CAaPpVt%DJ`cw42kg$O9hfcVVGsdSot0k zSp20PKf5fs`O`3?D(0-lIrPiaUyyFhoGKdX`m8f&S7?XoIMK0rBdl@Q$FAk(Lf{y+ zW&jpl@g_^(g5haEPgjxB8&RgAHhRcX)Z|ifRba*$US1w>)MY7~H#jsC8itFGLu(u^ zd@GWuP*<1zqPNX`%Z@wWIyqK2CR_0y!o64E3VCodL0jy{+fx_Z*o+f~;nP7!mc)1y z#%IQY`xlwL%l?#Q!-{A`fq&Owt+x~-_tpe>P=7~o&~ZXzgIRspnL;0A^+wP;AlFj) zQb>MO<6y`i5sSOkuqRwPMV&a~VMNaXQ-NB*KJ`2NLTG_MHPSQq&>UzLM}Eb`dihT= z0H2$HLM4hsF2@Rd46TtGnu%o&i?XM)pbhgUDi zt04Z0<|_8G}51Q+uAknbNj`U(bU~(teat^Kbcn-T#elm$|~9bvD`r%owS*vw$L?UK`ZjQy%x`)}Yz z4t0AtmU}qW4S79~@C++6mQe^->HxvR#CjktqYKp7BobnJn78d|;CujeAb>h2KsM0? zdtQq#-1m9|ObW+Ww%^8KomADL^WVc9XJu1DjqVJOvxl0zCLVhtG<#Mq`C$@gMCC9v=7UjGN6(1MwN6i+V-82+Z(1DTx=h(}B^bje3A z?^a=Z?DHq;bUYZ7*GMrBGvN`9Y$7d%{hT-jMC8RwLr3-Ari4$|w%H-OIrYXd2Sdv9 z;=Q=3+!={NNkri3!NLkey!!{{!Uo{fz(s literal 0 HcmV?d00001 diff --git a/Docs/image/uwb安装说明.png b/Docs/image/uwb安装说明.png new file mode 100644 index 0000000000000000000000000000000000000000..699bb7ea06c42511d291996eb21907f1b03e0c83 GIT binary patch literal 14066 zcmeHubx@p7x8~pwEVx4wAVGo!3&9dx0t9z=cb5SIA%OtFgF|q4cXyWwHb8J42yQd` z^83EMb@#t}>sD>mR_)YOO-;Yj-KS35Msqvt*qz zFVN4hK-z3hV|kSAN|;iWQ8i-yoS9PCy00+ZNyf!`QB{r9Ic+vKT$+bT>~GD0pBuv=dt$%I&tP{`$xJ_W zdPy>O^uQ!xIx-}qolK!INU!ptk&v?K9IIb^GcWptH#;Q8Xd9 zTp}O&Iz3xJn(`I;i;9qZr#&KV$NZ2VUJK7Y#G3dsl3f2eEfIGO;ZgZ+>9WVY-S?F7 zOUMX1^ZCWk?S$D~HBbjHPk{`fnC#PsyopAVKVRdsBx1grYUOoGwlm1WZ9-t09ky@r zWn_gvqLX+6hRf3L-%$b+Tn_@0TRyCS!l zLFRqZMUtg|cHfWu3Z`ju4{gK76daz`$)a&th&lhem?$tj`GOXd8j;b=U&@?lNRvX9 zdm=`w(Vcffi1$q2&>)u^nOuL6yPs*3P~Pi>Y@^-N#dzwmc_M}OdOVJRB_j?_|D)&b z5)iSZ3mEvw_fX!pW}iJ%-mU$ea z*JM9HA!IJH`lp*Z2$vY)nRq6`@{Z?|eVo(UNojkh`e7Bf#rW3nCa7thfpu;lmP)}{ zUglhDO8eza$LqztcD2(P`3rpm=HG?{i^NmZ1ciL4K0UED-Ewb1SkR3+gOC@!;Axl( z?IqdYR}mh~9P0O*)7s$Y(v?33uIQ(58hz07t2DaJ#e0oJumatVi-}~$kE`L;tOBfT z-WyNc6`Orbn-{gRCW0@3ZV&xN7|ma|6r3%}JL$P59CEl7H;yf;bQRZybYRt4DCS}F^YH(Mu7cMyTIEy z5~6AatZa{GSvP;3Un9t{?wgWK1YKHHjpzM7Yo1;^v@fa<31_D z7xX0INSzMoOC~TgqMQPpVKfrQ|o@HQa1AWbvC^&g+*AcH3<6z%&OzI zlBjc!RbRF29fJX1NL`g2VWn6D^CZT)@KRAgs}>FQR!sFqyL;-xYEkf(?wY;yum#Pb zJ_KH7s_pF4DvC07-7Re`&+0~nqP$*)o#=$KKViVF-pi|FaKm8|%Eg)wBUK%quOVgt@*K zl7S2YhWA|9H|cwxNiAX(N`}thQ*Md~QmG~Xc&CMDkvPYAk>jdKh^>^)`136#;U*zF zzy0b0gpKy=+drGdHU(2@h88I|`TZsd_`31oPjb9H_>|5@3gzi8qB^vQm|(_?Lf5!j zm*+`IeZM-AL;V~CERUL?3uHb^{;ntI3FQ-Ik@X@!f7r_zE4{#8K_!D51!_ersViy- zMlSW=j(h3dmFP#E&QqC|>$&$@P_*UBdK#q``vfZ}|G_l4{r+7EmvwbrDf#=}_3R$$ zUoJrV`@e~P6+Jx%v;yPHmmEFe^?lsV-}nR(t+C--=^|0s_I2J17`0rB%U-q3!>#!} zWIPy8Znz1*DN;7I5s_aU3axjQaB?WvNzxF(ao<9m>3QCjK0H04|9Vv zO+MS0Zsx{Bxq?wyPl34bkzl3$zKu`F5f2|gRV$%uQg8AR(P|xBE6S7-p|Q1HE=4b zR@igb-LKum*aDM_c_5JkwlOtW z#bO<;;i=?En^R9M7T~ZAHfZ+EA=}CibX%DKFZT?quY`z8$IvvC#;4(y$rR6owGv}3KAds7Y^zo6;T^_ zz8hP58d~hHn6i~Dm?mCnE>(_M%_HmROFHyq)8m3%_;%%xN0?rfq}p%K`F*ncrPfrgv*>_J~ zfjTjRrflog7<5Fqucz%xj&)#AD}dKx_^PP_(K9d_KK-<`J3OCHUhB#Fxrg89t^)c4 z!x0yJT(e-xVgH79ayZH;nJ=kaM29j>9TRjiSU0Jj@-n?-p+?YaDtC^i5R*M$oy2~G z|4dLkK#TytmO1<3^I{uaVa}n;iC03uhth&2OuO;D%N@GoIga1#{&KU=y`{He-VdrL zXVIizV0LDt*f-QMUiNY^)lU-HI$SmmYv&*0`6#eSS59#G-#L2R-NLsz5^@RSx zU`byC8~NAyR@g8{B6uerp?|&+|Ap)R+nEI{%j{^kYi$htkUB<$)?+rBcSFR}q$reR zMVQUUs1L<&7Yb$hJ6X1VTMyNUHt}?aHoUk<=J-gC6PKJtUK29$0V@+1^khE$y1%)o zoW~72v(b+R8kFaGF~Ldx)glg_>}VI=p89-wt!&-3-oVk(b8on|jT;+1#gnj2!FutF z1Mbp_0XoV`7rGSXLRFJ=NcfZU@#FTPqzEzDAP302-{ak2AH(B33Jerboy`={ z)sC=EwcmE?uN$T+MSGh$eQ+})KUDeyPsIFoZu!_ceVA4L@74Db62NXEAo%tZS3-ip zgzqUq%-4ng-}wIp@}HCeX{S^e_@0V9Ng+B8ze0LCrRV$ypB)5PzCZrYh{2`&PYFP2 zHJ5s@!xremKru=Xpg{qS0kh9rvn2X~gbui1>jFLRaBzXP0{(n;BfHU*@2PTDfpKd? zd?28C-yGcNK6k*L8w`lMA`LJTFg8cqwUyx5fXXwFK+TL1_e3yRrSP*D<$EAWBwQgA zjXUj|Favo#v|@KgT{L^q5%ke5#s81e$Nzew`Tzc%Zknz5fnUGouYDP~+9Fem6tGR^ zyOCCMj2<$0Gy`X&m)&V?(ZtBte}4>(oaI0O45I@czp=f_{swqCW&CE8~W z@$DR#HgZH6GrmZnt0&vOpBUiJXJFIGEV$d-V_hw(ul2k{9+XqEKG^+s)UM_D>_v*w zaCo9=O{OepTH*kMgdW{&|alo68;-BnC7rPK&-h z&nZB>t?vRou12fr!R|v?W=0jyikHh$F|<7he7$!EjHYS@4FF@TM7(!Ye7Epd+aW)1 zdnyt{n$7$0J2Z6sUCE&(8w0 zc*aIaKM)a|%j z%Yhq`>za$S4xiqS2uFkKIx#^}J00%TFDs#75j~V=td%du=;p8OhrF}5=tpjAA?_K?$%v7|+rM4z8*445yq z@P=}U$gSVLiH?Gr0UK~eV^<%M#@5YmcdCCp%y_VW=SxUT4KZCy6+N)>9_2+RQ1ldd zzSWsJJ03|)q^>98)m5c zjdG-Fu&byZPx^>N`T+N4Wy4Xk9fAU)kpB+)VKPc?D$jETSTZKcs2xLF4*#sqpUNkZ zD)*Z!m6=a=t^6(;A0F_Gk}VEUwtEQl33v>v6uy0ZY3U1#OL3=K)jvdc7Jy1G2WdCCB$<0p>b}xv?GvAf3OGi+ys& zeZm!b%3{-Q5(e}Qy5#?)2YBMN>(2*1mWHkPAs|pu%D`yK$HGyazF)i| zao9jiUy&K7yWwX>%wNhsX}?v$+Uiss+4U4Z#j@TunD`~BLZ z*J>USN1>|qWEPY-;XN#ZFLeiv-P6xMHg=*d^r-a)9fW3V8lIi04rb{rVm6Q3Nby93{{CW9HmBN<@D~QpwQyTiqZN&@T9>b-#s* z6!Fj$01}}33vbb=`WZ=8%knca86-4LHhwX&_CB7dVDr}sWlzY+tgaqk4c?-2Ql;k* zA6pihkV`LxVF?!oq`IMT-^D$gS36EyE?Z}<*^pJnEoF=gUACLAGcRb*X^XV=j&hpUxhmpD0^`^Ucj&C#Gq znL}3IP-2Q&l>!E8`T3G=TAekEYc^uEytai`l77dwX}oMfHyepKLfV{G)P2%q35Ew} zUVbQRA{zMSQoU%soM_0`g}%|3E12beRN-N+vHSAlpQ3foScgp9d`2Mn3K{?6=U?qG z^cK>@<4(GRY~=Nwq9{edsqLxrbdsTH1=5eWlBF1`#^7EMnsrywB10<{=@ z2MZBc#p$Hi%+%oQnsX~Z?d)}r;QTj8(jDBTGUHUpc+zZkw=b-$`7*!lkHmqEdxEz@ z7>5-ev0~9n9s&!L6$jIoy?X00NLq5s&Aq62}S6{F-8$IY>O z*@Z33Mt`W+LW(vC@he<(=RCAobnf!_g$-IeM!7Av3GN6;$zS50(pE2WR4gvU)a~3T z_Bg;ho~Hn5*|)qdcTshN9L^XxSG(he`k2-oGJV&c19sdgfgEO?4Ib`SC$03lL3ZrS z&Asx_e7jbX?d;g21uCI(NPoI$4y{gDT??-!opVw7ocbo8n&GEW`d5a1F4;#veD}I8 zoV@N^RH=j7QPnG<&otEq?Lo^{_SV&T-9HQX`IZcNOu&dHE{;a7=wKe)tlwEI7y47bdk|zqBHk40ytvx)~D6`l?DHI)4TK_=1 z5+p{Fm1Z}8NHZs8W(_b6#YXPwvU7)A)|XcF=tpLWlzpz|DgD&i*woh9DdB0gCz0oM zzMRUdQm-}A?DW+&KEf1?`95`5&GLNzZd$eIk=_j;PBlYboeY}ltXn}ZF`M>f#80Rf zTMwu~02swZ`^2SS?_$20mn26wthe}cajA)WAq(kL&eL(5rPW_Wqms5YCB@kN7tNL4 z;XNA_#!N>Jh^6*49-3#GG!5yp`qb2^f)@C$g;9h?nfIR~on@hmc}6xpX%@|Y%nn$N z9Ly9PTTCB^#Fa!TAN5wL91?Gx5J%c%c4vd2Sat5Q9+`n?@;~RKmaOq?;5rkVF*c)P zQY(%cByvootne@u@A`RaJwB(VYbKpNzNY*-Y|$Kft&`WSPBs@v zz&phy?upK9SmMcG7>Yl6akc$z@yyuWa!Ol^dLZ)mSvG326ubz_X$I3I>RlJi%%6Ox zuTx=Pw7yfqLe9tWb-z-3=vx z;3JI5{$%JPQ68*kVnAcXbTY_yKBv)8*L>&PJY+GC;y=gBm($WgQeIpxLKPeS63UeK zI;E9ftz=1+k_#|3pOS2;=kggKbyktZ(f9oXY1wIH`01$$uXNHcR8uza=%;MANEvkfDLn%G?+qztk2+MQ1 zjye<^FKEkb5vRw0(r+w4q5W@A;U#Ft6P1_gSiFD9{-@G z^tHH>6SKn;DRB5HlrL{?8R~4FQXOs`?VB;wTob(T9`!NSd@r!D9QkR&(p?n~RD0xj z?16x@4I8N zf&vhP;LH}ZmMe&`yKyf(n&fM=wx*q<;TwMRoWM*mM7XenBA+%261 zKBc>W`I)8r?J+mot_OGAa%7@)dNpnU;oUJ$Xr{pinBdHdoVxj9WA!2E^am!8(0V@W zM;Uus6SOy?!ZL}@Ek7QD0u0uNKiVe?nlxArvDE4~;h;-XEG5?q?sd%R%yD=NC{ z^YZ`~*B2t@YlDvCu;10gKevICMi>K z(Qh`4ufzca@!_M$%P~n{@C(!b(A$};Jjk)A?zP$@$3}bLWjp=%p+dW7ctUETvF57$ncfa}=FsWKvzTC{)>J$_oTWi&Y zT(7(;@c0rr%7iDKjdP)cd&>F9-Eq(U7_K`q8jbC%zl#R6sO@JTJEWcm}#jN<`2iP-#vCjr=`>{``k--zoKC0HcNiTt;W zvn>m2luS66jUPL6D2|jdEkJ%jX!KVqmunwmWA;hzS1gtDs`sAk!29fCpULK^AaUDD zP`)DRFK3t8c%c8o`2z9^07S>Cl_O=^`4JmF+nJ?M(C-zIYsWXSAO_2PG*ahTNJj{- z(iV#KkT&fBzs;PIXzoOs4gK_PK0VguqWr*2l4zS4u9s5l9>9V9Ej?k{cgr}U-k>)F zNKuPR=`8QCh4(xW-HTtl*{21c%f5$P@b98XPS5TE1y(tXU>6J1HOP*6x_cJi9$wTFh) zD~E;Z*MiN_-nQ?QPzwZre1xZ^yX7fBaIacjF23`>*8fbuTD#+V6*3lrnl*AO8nCf+ z)4N}K`{kA-KT^M(2(6?;H-taHVeEc7C9={7i?F19mBYD42x7T>RB0=VRx%^^_lQN_|Daif-g1? zKd?P5R8KCy7_}(4Brf^VwC`K2s(J@f1zvg4c#sw$$zTUxAU7OfIO^N5{ytBiC|Nq# zr$apRUhT<`+=FvBGU&I#BivT1kcAd^{fghyEfRo=W-OI7Elo;Kiy|QbnN^2fcvyso zbh82Y2YB`xIWsqHL~Y>9NcYshOMLbKSU7*9RK;BAfoL7fLvK5af)2{#bHlcdg>_UmN6@dTTt$&RB|cUD=CemuC>EIHQ1-(7O&TwI(rr@J0W8Fz@seJK?=zuvUk2uH^8|bTo%S zN5}Pz$ptxm%o~?Xwi8-Jl_oO4!K~w2-feGbr?){g*p^U1gNv=lliQOv<75Gw{^uhj zudGzxL6%QPYU(!`=Bo2#s~#ez^g8XHrAVsmX2QAM&Js2_AFRJ{9Axt*pJaITwQOM3 zp>~=++-g5`h~~ykf84@Ph4bO={(ZM*3zHj{LuvLqvc4UjxNF!deISSV>z!qQF*G4| zESUECh*?9jrJ336m-%0n<@appgXeub?t=x2b)H|;$eK9ck;>Sc5af8sq5xG-KjYPh z_p6)b=NuPva0~YZ%q`+|cBGe-duo;oUwY;Zz{}6I7OFifn!EcMypPhMm*t&`J$KYn zkrFCo6=J#t4J)CC=gi<=w&NLe5S~069zIVcxxXfH4!0{LJeH42mPFhdoWvk=(~KKb z)b6UkpPo9~wX`rBZD*hxPpU=B`jIhqB5L!G+uL_#74G{HZ|;{q_-{m(q4``ZU8CJ% zl)s&j%Vq%9W~)z=s;!G^KUgqQDC4rF6gFN0mJ~g0y|tFM!S=xK%IAO8oAWB$wYg%72vMgQ|!D{vDBDzrK6cJcedTk zn>+hB=w_OF7MMr)ca)o%tj!sv#6{YZ?`?}?z*TQv%bdLLt-}S&XMg|U zr26u&efea&PYfk~zkjKzk&o51W=cH6#Xl=2`K7JwIt9<3dq@y%hafck z&ie+JtQ=iv!G1QiO2f7=iTPQonESL9egw>v&LpN8EYmFDG_rG6KFt9yc=b{kpY`9d zm*Ch#lz92O7i34YBq+2H_)KBxB!x*!!;Kvz=1WeCrjk1wfK5AuB@2nVr=_M1{Qiqt zw8npA3h*V*=?3b$A1^*haw(v>yJ265)s}D|966$PlA(}@*Wt^QEV9j`1diR5QwqMJbYG6v`1#|NMvf*-mJsPMS zgEowlE7@6JH?d3!MIEouoQf3`*Iwi|E}C>~H2Ni<%=5;tCkbf>DhbEfWy5j4tmzib zTs&mz9(6r0@5V9=HmfM}tskcn!Bl^;T(su!8*VKcC^$d*6DXj7d1mVJmZ5v`U<@C5MQ9OqoPrZaP^YV=$ zG24*k2fpY=7h6bg^vMkI&A}5Lb;Vw{ycL?h?5Pj{uy3XbR>!D#DCTe~kN0yDD6x&> z$G8of(?;8W4N4ag3`BeXMdt~01YOqtV~@S45vQNeaBxruw4SfozL0PGBE7Pk+U|slc$gN;7ldHj|Xa2o@uK!b^blsQ<c6!@;@eL5Dbsi9OL6%t?ZDQDznI(7b#gZM}Wzx3I2bNQh~ z8tcwE`Lgtp%!N{PJr4M9s(qb7InAEXL$>|bS-8x7Xt$;P5x#=1`1Vl?HLL+}Mb~t7 z4==LFj5@c|HM+9&>9)xfVGD0?9q!Wim^L&{;(ht3-yU@!t+co&^q~JJj|kMz{CAOq z)aCKK#3)VNzw)x}D-{NS%$_QZAkSR1erU<8)p8%HH>hiOBOz=p>ueD%?=d7hk~!m; zzJm`nUm;~(QMMWa^-$%;3psH6yn`A_j&@ujOts$f8=1x}3{As1aR%t#_>6WQ8} z03Bf`51-|;{3q4o8i!=J?>>vx!`9#WiTmk~-dM&;N~c@d@%vCjXt9-Udve!P?8hC& z2yOgPe&?h8X6<%|eder>*>bg6?S6AnL4W)!C(fX$b;v?AQ@v(P_1Gya^%9;;=nui+1H4OZgi~?Vt6wku4 zFy|mNDtAtT``4^XdyAJY+)G|jhzyiv!rA)-FdvEEHdN=5(QJyQNJJFmX70ceU83iO zM?{bXx2%x}yqyIp-nBN`MV*a4usAfyjsIk@*~BZ_ypZ4Cpq=^Q&&Bw%r3oY-)#y|o z`~K8vs@#nQ`+-Q6^b)a!!fM(`7SzN84pt2PwxeR_jKex*M``J|zfc0bJ64zeuH^2U zHwgV!Nkujbu4lY&=+QV;OrQ1WeNB5r=())>UrMry}EnX+lq7OydD>P#?et5tri<+zuH-ZYu z*2V>n)>&nBnzD*R<64^?TMZpcOZ!yFfszrfZ(?823h_*O&N&lBARh_%qi9)7?vf#q zyS!^1{FLKsw5Rg6B}~k=FgEUd2-|X_M$h%8)gMJVbMamJIC?m%NLYdu5Q6Wk-sX8m zsKTghr*4eak%1x(41_$IzT>Ag|qK^%l%dQLXMj&*LbuZt@ou1ZQ3=ye6XO9d10~$gQg+I>GgL!X;vSo|H8D3c>igGE?pzpuJ@LE#1L~s)~UBQCi z1mpve0vR9(xi16qqBp}^38Kpc0s=W}3l*dw;Q@nsqn*#u7H=~KVqevhuE|A{Bu7_t zmyjKu3K;yN0FVU9k)Ktlw~YPDGxjuisU-y$U+je8mo2t$6hLq8Bl5_(eH^cp3Hy-m zfXFmI=X7r4@=7q!Jf#f(C;n;&0~7tQubGvj-XN%&EcuL}~u zw`x0l0Qfm8D1f6#+9%wUsmF1knfafUWPd0J7}Y*FdJ6at7?xTAwwmfjiy)Od6~_6M zyubSaohsgHJQ;k|esddUAPV!@UQHEh?|Ms%u58CizKchg$;W7jm~`ZDf4EdhssJ;w zV*n*bM2#y8ooWDye&=8d?ztFm2m2hnQw{pM>v6lhc&^_!l3zP~bR6Mvf%8ajtwQe|gnK3~bb?E1SZAt8VjN$?c(TBgN14E^d@Nb%l1CDFyM zbrt}F6yXwy5so}+k?s6tKa+{ek%LyoVg`dY&*MeWO!D=2JE9bX|?miJ;qotn1KLDC}I zVw6oa9tu+^j9;O-v*P|ws$Iv>W1~=q*<_s} zK}ND8K1xuSG?x{$6!dC&7%@g|T+=~*p$_9oOkhrQ4CYJ)Yd z3B}n8H5)8|TlY2tw<))+g%`xqP?8mjTPV01iMwC{%JcRGvJm?c zKlH&M75fk7p;b7b__$-BN+Z!)8aAbUpvb~!sfq485;2{27scf3S*>j?1*j=8TG{YZ zW%*Ck86w66Eg0GdHLSwr{sn0QP5qw_)ICaqC$V%Mt3@XTj5)1A6z`R-;TfHF2?CSD zxoNe7j7^lfDdouQEz%;+I_!)2D4;?0ImM0odn2*Ii-Pnz5J)$oWS$L$(v&4IH+H#Q ze8B#D*dt^*wMk9O*qV_4Ag1enHXR=RRfFC&8}A33#uy-@7WE(vb9S|^jVHPi<;5P0 z$Q5_GLEHds8Pp&Pki0b={DN~{Jep8gP}3}C$pCcLAkcQ~&Vt+NPQr$u=ER2*8DZ z?5)x+{LNobC7^^bXwt+)uzcShWW)(wz{n0(zohGwjR2?)d(3(h ziYFJ0(st@iK}Z!**a~+kp~|vyZFx7mBR@UWCW!$E$cmcwCPb1ewSrlF$@T-(lloQJ{ch8|tA@77>HDv5mW$qfnw(k){%45@eCdUw$+I;wre zfjulehPJ01fsgeA?vkDBU0V42Syn)pVU|ot0*MLW5w;YNk$|&oAX!O8iE=Tc;Qs|JR2%64 literal 0 HcmV?d00001 diff --git a/Docs/割草机融合定位：.md b/Docs/割草机融合定位：.md new file mode 100644 index 0000000..6cd8fc1 --- /dev/null +++ b/Docs/割草机融合定位：.md @@ -0,0 +1,9 @@ +割草机融合定位： +地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： + +![img](file:///C:\Users\ray\AppData\Local\Temp\ksohtml2328\wps2.png) + +注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 +小车半径：a +标签高度：h +BC标签距离d \ No newline at end of file -- 2.37.2.windows.2 From 568cde0c51eb5a79935daf08e6b45d92b8be425c Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Mon, 18 Mar 2024 17:39:25 +0800 Subject: [PATCH 3/6] 3.18 --- ...{割草机融合定位：.md => uwb使用说明.md} | 5 +++-- Docs/uwb安装说明.md | 10 ++++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) rename Docs/{割草机融合定位：.md => uwb使用说明.md} (76%) create mode 100644 Docs/uwb安装说明.md diff --git a/Docs/割草机融合定位：.md b/Docs/uwb使用说明.md similarity index 76% rename from Docs/割草机融合定位：.md rename to Docs/uwb使用说明.md index 6cd8fc1..60fc27b 100644 --- a/Docs/割草机融合定位：.md +++ b/Docs/uwb使用说明.md @@ -1,7 +1,8 @@ -割草机融合定位： +# uwb安装说明： + 地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： -![img](file:///C:\Users\ray\AppData\Local\Temp\ksohtml2328\wps2.png) +![uwb安装说明](D:\FORK\RobotKernal-UESTC\Docs\image\uwb安装说明.png) 注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 小车半径：a diff --git a/Docs/uwb安装说明.md b/Docs/uwb安装说明.md new file mode 100644 index 0000000..60fc27b --- /dev/null +++ b/Docs/uwb安装说明.md @@ -0,0 +1,10 @@ +# uwb安装说明： + +地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： + +![uwb安装说明](D:\FORK\RobotKernal-UESTC\Docs\image\uwb安装说明.png) + +注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 +小车半径：a +标签高度：h +BC标签距离d \ No newline at end of file -- 2.37.2.windows.2 From 8354e4d9eaf4c62497d29cc725c49735a2736648 Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Mon, 18 Mar 2024 17:47:04 +0800 Subject: [PATCH 4/6] 3.18 --- Docs/UWB解算原理.md | 4 ++-- Docs/uwb使用说明.md | 10 ---------- Docs/uwb安装说明.md | 2 +- 3 files changed, 3 insertions(+), 13 deletions(-) delete mode 100644 Docs/uwb使用说明.md diff --git a/Docs/UWB解算原理.md b/Docs/UWB解算原理.md index ae176dd..4ed46ea 100644 --- a/Docs/UWB解算原理.md +++ b/Docs/UWB解算原理.md @@ -12,7 +12,7 @@ for(int i=0; i<3; i++){ } ``` - +![UWB高度修正](./image/UWB高度修正.png) #### 2.多项式拟合 @@ -30,7 +30,7 @@ d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852; ​ UWB位置求解采用如下图示： - +![UWB位置示意图](./image/UWB位置示意图.png) ​ UWB的定位可以用下面公式描述, 其中$(x,y)$是割草机器人上面的UWB的位置，另外三个坐标点是3个UWB标签的位置，可以有如下的公式。 $$ diff --git a/Docs/uwb使用说明.md b/Docs/uwb使用说明.md deleted file mode 100644 index 60fc27b..0000000 --- a/Docs/uwb使用说明.md +++ /dev/null @@ -1,10 +0,0 @@ -# uwb安装说明： - -地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： - -![uwb安装说明](D:\FORK\RobotKernal-UESTC\Docs\image\uwb安装说明.png) - -注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 -小车半径：a -标签高度：h -BC标签距离d \ No newline at end of file diff --git a/Docs/uwb安装说明.md b/Docs/uwb安装说明.md index 60fc27b..6f3bfcc 100644 --- a/Docs/uwb安装说明.md +++ b/Docs/uwb安装说明.md @@ -2,7 +2,7 @@ 地图坐标系与小车坐标系重合，小车起始点为坐标系原点，使用三个标签分别是A，B，C。A放置在充电桩处。车上放置基站M。通过基站到三个标签的距离获得小车的位置信息。如图所示： -![uwb安装说明](D:\FORK\RobotKernal-UESTC\Docs\image\uwb安装说明.png) +![uwb安装说明](./image/uwb安装说明.png) 注:为了方便，人为设定UWB坐标系与地图坐标系和小车坐标系均重合。 小车半径：a -- 2.37.2.windows.2 From f9631ad63d7461a6aeafa0364ed0c09883d7f4cb Mon Sep 17 00:00:00 2001 From: LXX <916401988@qq.com> Date: Tue, 19 Mar 2024 20:48:24 +0800 Subject: [PATCH 5/6] =?UTF-8?q?=E8=9E=8D=E5=90=88UWB=E5=AE=9A=E4=BD=8D?= =?UTF-8?q?=EF=BC=8C=E5=B0=8F=E8=BD=A6=E9=AA=8C=E8=AF=81=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ros/test/room_exploration_client.launch | 4 +- .../pibot_ros/ros_ws/src/map/160_160.png | Bin 0 -> 994 bytes .../pibot_ros/ros_ws/src/map/250_250.png | Bin 0 -> 1063 bytes .../pibot_ros/ros_ws/src/map/300_300.png | Bin 0 -> 1100 bytes .../pibot_ros/ros_ws/src/map/400_400.png | Bin 0 -> 1121 bytes .../pibot_ros/ros_ws/src/map/500_500.png | Bin 0 -> 1181 bytes .../launch/bringup_with_imu.launch | 11 +- .../src/pibot_bringup/scripts/odom_ekf.py | 4 +- .../src/pibot_navigation/launch/nav.launch | 2 +- .../dwa_local_planner_params_apollo.yaml | 1 + .../ros_ws/src/upbot_location/CMakeLists.txt | 3 +- .../ros_ws/src/upbot_location/include/align.h | 20 ++-- .../src/upbot_location/include/mapping.h | 3 +- .../src/upbot_location/include/senddata.h | 32 +++--- .../launch/upbot_location.launch | 5 + .../ros_ws/src/upbot_location/map/1.png | Bin 0 -> 36831 bytes .../ros_ws/src/upbot_location/src/align.cpp | 106 ++++++++---------- .../ros_ws/src/upbot_location/src/main.cpp | 11 +- .../ros_ws/src/upbot_location/src/mapping.cpp | 4 +- .../src/upbot_location/src/senddata.cpp | 73 +++++++----- 20 files changed, 144 insertions(+), 135 deletions(-) create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/160_160.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/250_250.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/300_300.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/400_400.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/map/500_500.png create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/launch/upbot_location.launch create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_location/map/1.png diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch index 3dd5d50..95a45f2 100644 --- a/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch +++ b/Code/MowingRobot/pibot_ros/ros_ws/src/ipa_coverage_planning/ipa_room_exploration/ros/test/room_exploration_client.launch @@ -13,7 +13,7 @@ - + @@ -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 0000000000000000000000000000000000000000..4c1dfbe0bdcef75e02cea03f35da7a09f3eff832 GIT binary patch literal 994 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#NsWdb&7e;M-*}??gpLeQ3E=7K=d#Wzp$PyhjnS?E literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..911642175a31be56f9647c8732067d139c539889 GIT binary patch literal 1100 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LWDJzX3_Dj44$bmTqYAaGO$2}$|)Pq-`}Kb{+-m|nAIvi6gn{_>*M xIUhgzt(#zMckiU=sA3{QW2Vw5q;d#+Vh%R8o%pRIHx^V(c)I$ztaD0e0ssYt#M=M> literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..dd6b454c08d23efd42d77283573a8c4cb399696a GIT binary patch literal 1121 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LVIJY5_^Dj44$G-PBj5NI*@ zpufxXBlBTZBh67rNeBpi-=Ug2N(`DXpd>Cw^%`l)A3Yp6aRM{n8Bp2b>FVdQ&MBb@ E0Op55CIA2c literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..7dff0905b4df6d59f2d06583010f810018082f4e GIT binary patch literal 1181 zcmeAS@N?(olHy`uVBq!ia0y~yV4MKL96$kvrB^Zq7#LVmJY5_^Dj44$bmV0)5IDS{ z#r_5FQP0XN(>y1RLSjQ;$#GlFNu$Jw3Ik%(#;7{apGN~zC$ebr@q@}8Pgg&ebxsLQ E0LrrjM*si- literal 0 HcmV?d00001 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..3b4bb48 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,8 +66,8 @@ - - + + @@ -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..87b9862 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/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/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/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 0000000000000000000000000000000000000000..67d7af0c3873bc6d1f8ccb457eebbe6d549a0f87 GIT binary patch literal 36831 zcmeHQ4OCNSnnpxHYz>hhsaF1?l1LqzZLM(35PkzhG2wzT)h(&%Y&k9MvMgsSIn5CN zDB%(Z8ROV$N=c5bdV6RL_vgDe@B6&Z`@HY>aZ|6{k^5x$>hPeTpeOV5vWcLe#s7`|4-1AZ*REz= z3JQt~%FoUyu3mKKs(8^qrbmRduAMktfAIH3l8>L||xYWM!vcA<4>4 ze?pR#owMy1`LN^^Bw5*MTPWSmw6DK>H|XWKu!H~lQ{AW8SN~5GGg}R~Aez>!5M_nv z!ks$#HZq)DOWtQxR*3pD>$Ck&zh-^5|LNDP&-Op<{+#0sO#idwj|IQ-t_28`@M5Rn z{kpn>m7uHyWhLk%y=ZA_rgN%m>C+;bb9;VjR91qr5_HyU0of^dzb?37r{Mj%;DVK% ztn9pZdGk;1TwU~0#Lde~KK+A}^XrMbN`n8z(zd4Zl=9cCExVq0eL=mvY)!S9s`^uH zZ|=hXn5qU}o%^QXs6)HOXCKFxA60@a!IGL&QEt1Mx?Y_7rt}4cnTH8tf1+}7tPRS? z6Pyr{{zT>ESj&nR7^^)rgtGMMU!rH})4$N2q&z<)^~teV`t(auXX(?*(kDxw3t2i$ zQXWg6eo@Sml;?+fwV=WN~$S1uT10hp7CE_ z>ZyxKly@z_bYd3geETtF&n|7ECBlkVNo%e1tGcZv_~l8Mcjc@OcKHEQZN z#$-~e5Wz8_7inm%IA2mz(%R7%nP>vebY^@2gH(-&3ROH^Djtx+1DdxvKX7#V98ztryv?g-|UyT$=BHb>NxV{lYadbrA}17yZ| z{>{L?RB)TTH{giwi}q;>Ma!AxmxGP;^5HIWfXbV(QEtr@;VuTNNdc%@RiL8cA0IG#ht4D_KPr12KVe}P|n!6%#gdc&N` zH`G*@5IT>p8_1vbNCHMJFgWaRkpX3ouqH<&UMWc93dYx39GP&n3a$VU5sh%-Tqd#( z$(*|xo1Uv%3s*fw9j2qz)V?BeC^48Dhy0r8IWU$~MGATnWLo&)43}HqhVdufA>h~j z{Ce&X{6o0n6C&!u?b7dOfLn`nmm1YkgBZoS8 zLkYplYFEc57&%sH9iY4H91~AB7Ligp9P9dH$i}6Y)ykxRywH1Yrk{1RxVZmR_tiv{ zS)aa6=2dMK`XXz`H!p|$+mifW{`ODjqM3ty zrgk?&^3O%7^vkT#2?TS5&m7s!92`|j&PH{*NAa&1#yJ5;Ip=gRQ^0JnRA(6Y;4J`= zrf;8hXxsMjHcAgB1_*^D0h%Q77D^wDe)2*R#>a$1QBtm8shHkJ=|f=;9LYhbQw$&o zEAm85u{qsw2w-Ze5fDk~fy8z)d(_m3p4;0Z1F#$>w0cBnnDlJzyxw(iEI{+Fo(lj@ z%X5C{NYrzsD-VQi?rEsrVyWhcf#OOpAfAgP$mHy}m4ea;0ax$nEGpfYmThpH0`qkU zt#x{?&9oSSTC)q_$~^?=g%N9V0g?2kJ_V_JpU<@L%%^$g4S@gVtqrk8Z=oC`(RvqEk63_YWQgT!?NK=`*S|HP^60L%VOeW-RKkN z>YMn?7OucZUoxt>fNoP&&5sfMZS6*WeG{Ammx==ujGmzI+%|1!M1Z zV{Ympa&Lx52+|FX%=6Vb&iqyhPj${SUm?%#&OrnU{;ar3o&ZusfmhSceQZ5)=s}PB6C%Kkm4od74EV%Ac-Y;mF!<5QnFa@V6`c8=h=UN3 zYSS7$r>fTlb8SB~(nUskD|R-{+X|UMc{`q7BBpi6`AjlF4&aO{#3LqJf`GL9D@16h zZSDk%D=C{`gN#a&R}P-RHv&=CpIR1qHlX>nI>&I63>|K*&Zyl_Z=RNG>CG0T0IG9D z^CtvwPWUkuW`V2pfbkUA;-;!JoHuPtEGfMZ)2ESyDa5GR0U>#iFhODghle813}Vj= zm7d#jvjmTA& zlo5-B7u$-cYKJ#Y$x)?=qa-;fpJ_Ord{*5+d~;)pF52P}{WpZx zD>(h8;XyNjso`3&Z4#sdQAnEXd)$|MJ-Zv+cHKz-gu=k-!oV>Z=}q#=G?@rAXf6gx z1qO*zpPpGcz6yrp_GJN}dILTqbNpRbE^~0V$XY%SsQumHT0YP;pJZXF$ENiny2JYVj1ML3@;B}jX|ocU`Mti4#UKvF}& z`qVue6G7n08Imv!cwpjO;)V%`2#Cx?ku?`rv~KbBa{`5Hnb5iggwPXsXj~PPK_v=- zAhbF=AtzMzzZl3$4@+bkuv@B30=+&{A7RTmw1Og8Xf2t^-QN1}o`C!F55|@Oqa(9O zEKnE$3#xE%eTEWL9j*NtglAR%5biiA^0y27r5WH}AlZpuCj~Rs|6Z zBck6f&EvW5PjI1^Zm7-|4MKc&5~wIZ7qJ!K zk>qL$C4rrwgdjq3EOz1ueNAL_+93KdT?aTiJ(V^RKuN=`j{0fRUCe_4xIn;}C zrUJmziEev5crxzc#>}nSovsmOPZEZsZ9U!>E6x-_7MF>}Z$TBU1BSAhP6RyCSE4+; z<}wjOnteOy9Q64xTi-kDLp<~6NAPzDd>l#}pfqifpn#uPHT?H<;~A74e};ulBYzPz zSRm+CXS5|QE~Qt&4MXYD??!~dQBSz?MB@eExDH$vzksYeg0c2|b{teTpsiiY0ZgEU zhA<93f$|nqwdypF183df3Ot!12|MdaF})cTf}-)tz7lI>gkUfO#XR0=0?dLJUxZ*s zlwh*EJ1Yk0za)#STjWfmvS(v+V_3H9rwCYh!BulT|DsTJoN&Df_pn$^!GX|h3iPDF zMlP;JrZV!W;xOTi<86u?K$IE!q%o;CkY|PdWh&gHoEg7%EX`GG>MY!DF|$cv;+t^lvx!xs!?%z$w+H0 z;J&|ujwyuUK9pWHe~ZaNjq|QV$f+ecc151VzYIz}oAn!Y%f}Nr<$rl5@9LNKSAy4Z zs;>Ce|5&G)GYpOxIdi2bd^iUgSl|gsGp{#!_!#{fnhVmn1NkXRKhnzUjkYLD5uu7P zL%#v%D$_(UI}8x;W7xg1kU(qXjJB@$8%a+ z7eXjk6sCy=y=4=8{zx(XoR81wa&?Gr0{8nw3Z}@r!emlcH=54~fO&D__%3#J=z5u} z7;dTrNZ4@%@3@4+`xOYg5;Zl@=CU~IjnKgWhha%=GZSf|nxlje%p8)?1Je*_lFNi8 zo-I^p=1mv~R|Nz)sBME|hngxwHIo|V%FuipF7vBMJO}iqAlA6E3tC z<10)}Iky4%zBF5#WpKR1KbKAInU{o(2gUJH zSHxQGHv~16aLx5x_%Qvcx)(4n zm~+w0oN9YWLUS&jO4e+_=8uC?O-wi9srg(yx!SnH;Ali0YS%dw%4v~;!31#33QQW; zxq=tOG|b|5L&puKSTtEo=F01xp7oDcrjeFSvU5Bsp20U)j< z3c*&oUM6YgGw*@216Fzuhrm{SI=Z1-F!fRnIu07E8)N`Y1Q=~V%e?!MasTgrXM}00 zNcSHXK+KNB%S;e1Xhz*;lDhvA1~%g;MIHmAyPie9gHnLx|_4Woxf=N9Ob4hZ}@n%VXRX3d@6 zy%$rkzP1uridDq4!R4eM4S)De~KPurk#9rwM-I-G?nh|7YT2=#7w| z#TgNY-*Po<2>(#Q?Dy*<02jztV6xADVxRxCaKDI^ee9@TIcM5$rt#-99@(#8^6U5A zuur~cpL~Ds)8xLx_h(Q0%`|iVGKQ&9*(cw#Prje^1Cd$T`IyVj8{;QF3JT)9m7kqa zJZls^j1yT2%1Ti7$@dQ<(iXsXtxY|n-k(*`)Tr!}@7X8cvroSF>jUg(%}n1T3Hra6 z@99PNC@PhEIZuwb_data_.uwb_t_) { - std::cout << "uwb received" << std::endl; - //这一步是为了把上述提到的UWB设备与imu设备不在同一坐标轴上设计的坐标轴对齐操作 - Rotate.mat[0][0] = cos(roll); - Rotate.mat[0][1] = -sin(roll); - Rotate.mat[1][0] = sin(roll); - Rotate.mat[1][1] = cos(roll); + // 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); + // 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; - //后期如UWB摆放在imu正上方即删除 + // uwbPos = uwbPos - Rotate * uwbStartPos; + //后期如UWB摆放在imu正上方即删除 //卡尔曼更新过程 - predPos = F * syncPos + detPos; + predPos = F * syncPos + deltaPos; Z = H * uwbPos; P = F * P * (~F) + Q; Kg = P * (~H) / (H * P * (~H) + R); @@ -97,27 +95,27 @@ namespace uwb_slam{ P = (I - Kg * H) * P; uwbDataRxTime = uwb_->uwb_data_.uwb_t_; } else { - syncPos = syncPos + detPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新 + syncPos = syncPos + deltaPos;//如果UWB没有更新信息，则使用imu对齐位置进行更新 } 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 << " 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"; } } @@ -126,23 +124,7 @@ namespace uwb_slam{ // 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; @@ -157,7 +139,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; @@ -167,6 +150,9 @@ 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..e295b0a 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,20 +1,27 @@ + /**--------------------------------------------------------------------- + * Function : Senddata::publishOdometry + * Description : 发布UWB IMU 里程计融合的定位信息 一个发布给导航 一个发布给tftree 坐标变换使用 + * Date : 2024/3/19 linyuehang + *---------------------------------------------------------------------**/ + #include "senddata.h" namespace uwb_slam { - void Senddata::Run(std::shared_ptruwb){ + void Senddata::Run(){ // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 ros::Rate loop_rate(10); // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 // 主题被设置为"uwb_odom"，队列大小为50 - position_pub_=nh_.advertise("uwb_odom",50); + position_pub_=nh_.advertise("odom",10); // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到 // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10 - odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); + while(ros::ok()){ // 按照10Hz频率发布uwb信息 - publishOdometry(uwb); + publishOdometry(); + publishtf(); ros::spinOnce(); // 用于控制循环速率 loop_rate.sleep(); @@ -22,54 +29,62 @@ namespace uwb_slam } - 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); + } + -- 2.37.2.windows.2 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 6/6] =?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; -} -- 2.37.2.windows.2