├── .gitignore ├── Final_project ├── Readme.md ├── controllers │ ├── openDoor │ │ ├── Makefile │ │ └── openDoor.c │ └── wasd │ │ ├── Makefile │ │ └── wasd.cpp └── worlds │ ├── world1.wbt │ ├── world2.wbt │ ├── world3.wbt │ └── world4.wbt ├── LICENSE ├── PRM_planning ├── Readme.md ├── controllers │ ├── Move │ │ ├── Makefile │ │ ├── Move.cpp │ │ ├── example.png │ │ ├── example.txt │ │ ├── main.cpp │ │ ├── maze.png │ │ ├── maze.txt │ │ ├── nice.png │ │ ├── nice.txt │ │ ├── qwq.png │ │ ├── test.cpp │ │ ├── tu.cpp │ │ └── tu.h │ └── display │ │ ├── Makefile │ │ └── display.cpp ├── maze.png ├── protos │ └── car.wbo └── worlds │ └── empty.wbt ├── README.md └── RRT_planning ├── Readme.md ├── controllers ├── Move │ ├── Makefile │ ├── Move.cpp │ ├── example.txt │ ├── example_far.txt │ ├── example_nei.png │ ├── example_nei_far.png │ ├── example_nei_no_rewrite.png │ ├── example_norewrite.txt │ ├── example_path.png │ ├── example_path_far.png │ ├── example_path_norewrite.png │ ├── example_point.png │ ├── example_point_far.png │ ├── example_point_norewrite.png │ ├── maze.png │ ├── test.cpp │ ├── tu.cpp │ └── tu.h └── display │ ├── Makefile │ └── display.cpp ├── maze.png ├── protos └── car.wbo └── worlds └── empty.wbt /.gitignore: -------------------------------------------------------------------------------- 1 | ## Core latex/pdflatex auxiliary files: 2 | *.aux 3 | *.lof 4 | *.log 5 | *.lot 6 | *.fls 7 | *.out 8 | *.toc 9 | *.fmt 10 | *.fot 11 | *.cb 12 | *.cb2 13 | .*.lb 14 | delete-temp-files.bat 15 | elegantpaper-cn.pdf 16 | elegantpaper-en.pdf 17 | *clean.bat 18 | 19 | ## Intermediate documents: 20 | *.dvi 21 | *.xdv 22 | *-converted-to.* 23 | # these rules might exclude image files for figures etc. 24 | # *.ps 25 | # *.eps 26 | # *.pdf 27 | 28 | ## Generated if empty string is given at "Please type another file name for output:" 29 | .pdf 30 | 31 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 32 | *.bbl 33 | *.bcf 34 | *.blg 35 | *-blx.aux 36 | *-blx.bib 37 | *.run.xml 38 | 39 | ## Build tool auxiliary files: 40 | *.fdb_latexmk 41 | *.synctex 42 | *.synctex(busy) 43 | *.synctex.gz 44 | *.synctex.gz(busy) 45 | *.pdfsync 46 | 47 | ## Auxiliary and intermediate files from other packages: 48 | # algorithms 49 | *.alg 50 | *.loa 51 | 52 | # achemso 53 | acs-*.bib 54 | 55 | # amsthm 56 | *.thm 57 | 58 | # beamer 59 | *.nav 60 | *.pre 61 | *.snm 62 | *.vrb 63 | 64 | # changes 65 | *.soc 66 | 67 | # cprotect 68 | *.cpt 69 | 70 | # elsarticle (documentclass of Elsevier journals) 71 | *.spl 72 | 73 | # endnotes 74 | *.ent 75 | 76 | # fixme 77 | *.lox 78 | 79 | # feynmf/feynmp 80 | *.mf 81 | *.mp 82 | *.t[1-9] 83 | *.t[1-9][0-9] 84 | *.tfm 85 | 86 | #(r)(e)ledmac/(r)(e)ledpar 87 | *.end 88 | *.?end 89 | *.[1-9] 90 | *.[1-9][0-9] 91 | *.[1-9][0-9][0-9] 92 | *.[1-9]R 93 | *.[1-9][0-9]R 94 | *.[1-9][0-9][0-9]R 95 | *.eledsec[1-9] 96 | *.eledsec[1-9]R 97 | *.eledsec[1-9][0-9] 98 | *.eledsec[1-9][0-9]R 99 | *.eledsec[1-9][0-9][0-9] 100 | *.eledsec[1-9][0-9][0-9]R 101 | 102 | # glossaries 103 | *.acn 104 | *.acr 105 | *.glg 106 | *.glo 107 | *.gls 108 | *.glsdefs 109 | 110 | # gnuplottex 111 | *-gnuplottex-* 112 | 113 | # gregoriotex 114 | *.gaux 115 | *.gtex 116 | 117 | # htlatex 118 | *.4ct 119 | *.4tc 120 | *.idv 121 | *.lg 122 | *.trc 123 | *.xref 124 | 125 | # hyperref 126 | *.brf 127 | 128 | # knitr 129 | *-concordance.tex 130 | # TODO Comment the next line if you want to keep your tikz graphics files 131 | *.tikz 132 | *-tikzDictionary 133 | 134 | # listings 135 | *.lol 136 | 137 | # makeidx 138 | *.idx 139 | *.ilg 140 | *.ind 141 | *.ist 142 | 143 | # minitoc 144 | *.maf 145 | *.mlf 146 | *.mlt 147 | *.mtc[0-9]* 148 | *.slf[0-9]* 149 | *.slt[0-9]* 150 | *.stc[0-9]* 151 | 152 | # minted 153 | _minted* 154 | *.pyg 155 | 156 | # morewrites 157 | *.mw 158 | 159 | # nomencl 160 | *.nlg 161 | *.nlo 162 | *.nls 163 | 164 | # pax 165 | *.pax 166 | 167 | # pdfpcnotes 168 | *.pdfpc 169 | 170 | # sagetex 171 | *.sagetex.sage 172 | *.sagetex.py 173 | *.sagetex.scmd 174 | 175 | # scrwfile 176 | *.wrt 177 | 178 | # sympy 179 | *.sout 180 | *.sympy 181 | sympy-plots-for-*.tex/ 182 | 183 | # pdfcomment 184 | *.upa 185 | *.upb 186 | 187 | # pythontex 188 | *.pytxcode 189 | pythontex-files-*/ 190 | 191 | # thmtools 192 | *.loe 193 | 194 | # TikZ & PGF 195 | *.dpth 196 | *.md5 197 | *.auxlock 198 | 199 | # todonotes 200 | *.tdo 201 | 202 | # easy-todo 203 | *.lod 204 | 205 | # xmpincl 206 | *.xmpi 207 | 208 | # xindy 209 | *.xdy 210 | 211 | # xypic precompiled matrices 212 | *.xyc 213 | 214 | # endfloat 215 | *.ttt 216 | *.fff 217 | 218 | # Latexian 219 | TSWLatexianTemp* 220 | 221 | ## Editors: 222 | # WinEdt 223 | *.bak 224 | *.sav 225 | 226 | # Texpad 227 | .texpadtmp 228 | 229 | # Kile 230 | *.backup 231 | 232 | # KBibTeX 233 | *~[0-9]* 234 | 235 | # auto folder when using emacs and auctex 236 | ./auto/* 237 | *.el 238 | 239 | # expex forward references with \gathertags 240 | *-tags.tex 241 | 242 | # standalone packages 243 | *.sta 244 | 245 | # generated if using elsarticle.cls 246 | *.spl 247 | 248 | *.mp4 249 | **/.DS_Store 250 | 251 | # Byte-compiled / optimized / DLL files 252 | __pycache__/ 253 | *.py[cod] 254 | *$py.class 255 | *.d 256 | *.o 257 | build/ 258 | 259 | bilibili -------------------------------------------------------------------------------- /Final_project/Readme.md: -------------------------------------------------------------------------------- 1 | # Webots Final Project 2 | 3 | 演示视频：[bilibili](https://www.bilibili.com/video/BV1pm4y1Z7ee) 4 | 5 | 实验报告：[Webots——Final大作业](https://blog.lanly.vip/article/11) 6 | 7 | 实验平台：Webots 2021a 8 | 9 | 注：2022a版对坐标系进行了改变，详情点击[此处](https://github.com/cyberbotics/webots/wiki/How-to-adapt-your-world-or-PROTO-to-Webots-R2022a)，故请使用此版本之前的平台，比如2021a。当然你也可以使用该网站提到的的[脚本](https://github.com/cyberbotics/webots/blob/master/scripts/converter/convert_nue_to_enu_rub_to_flu.py)对世界文件进行坐标变换 10 | 11 | 小车通过感知周围环境，绕开障碍物抵达终点。 12 | 13 | 其中小车位置和终点位置通过`GPS`定位获得。 14 | 15 | 感知参考`SLAM`写法。 16 | 17 | 规划采用`D*`算法。 18 | 19 | 运动使用了`PID`控制器。 20 | 21 | ```bash 22 | . 23 | ├── Readme.md // 须知 24 | ├── controllers // 控制器 25 | │   ├── openDoor // 门开关控制逻辑（world3里） 26 | │   │   ├── Makefile 27 | │   │   └── openDoor.c 28 | │   └── wasd // 小车控制逻辑 29 | │   ├── Makefile // 开了C++17 30 | │   └── wasd.cpp 31 | └── worlds 32 | ├── world1.wbt // 迷宫一 33 | ├── world2.wbt // 迷宫二 34 | ├── world3.wbt // 迷宫三 35 | └── world4.wbt // 迷宫四 36 | ``` 37 | 38 | 双击`world`文件夹下的`wbt`文件打开世界。 39 | 40 | ### 控制器代码 41 | 42 | 务必开启 `c++17`标准。 43 | 44 | 由于不同世界的尺寸不一样，需要更改`wasd.cpp`的`22-40`行的注释，以适应不同世界的尺寸，使得在感知出的图的障碍物定位与实际一致。 45 | -------------------------------------------------------------------------------- /Final_project/controllers/openDoor/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | ### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ 43 | ### 44 | ### ---- Compilation options ---- 45 | ### if special compilation flags are necessary: 46 | ### CFLAGS = -Wno-unused-result 47 | ### 48 | ### ---- Linked libraries ---- 49 | ### if your program needs additional libraries: 50 | ### INCLUDE = -I"/my_library_path/include" 51 | ### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library 52 | ### 53 | ### ---- Linking options ---- 54 | ### if special linking flags are needed: 55 | ### LFLAGS = -s 56 | ### 57 | ### ---- Webots included libraries ---- 58 | ### if you want to use the Webots C API in your C++ controller program: 59 | ### USE_C_API = true 60 | ### 61 | ### ---- Debug mode ---- 62 | ### if you want to display the gcc command line for compilation and link, as 63 | ### well as the rm command details used for cleaning: 64 | ### VERBOSE = 1 65 | ### 66 | ###----------------------------------------------------------------------------- 67 | 68 | ### Do not modify: this includes Webots global Makefile.include 69 | null := 70 | space := $(null) $(null) 71 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 72 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 73 | -------------------------------------------------------------------------------- /Final_project/controllers/openDoor/openDoor.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char *argv[]) 8 | { 9 | wb_robot_init(); 10 | 11 | const WbNodeRef root_node = wb_supervisor_node_get_root(); 12 | const WbFieldRef root_children_field = wb_supervisor_node_get_field(root_node, "children"); 13 | double timeStep = wb_robot_get_basic_time_step(); 14 | 15 | WbNodeRef door1, door2; 16 | WbFieldRef field1, field2; 17 | door1 = wb_supervisor_field_get_mf_node(root_children_field, 6); 18 | field1 = wb_supervisor_node_get_field(door1, "position"); 19 | door2 = wb_supervisor_field_get_mf_node(root_children_field, 7); 20 | field2 = wb_supervisor_node_get_field(door2, "position"); 21 | 22 | int cnt = 0; 23 | while (wb_robot_step(timeStep) != -1) 24 | { 25 | if (cnt < 150) 26 | { 27 | wb_supervisor_field_set_sf_float(field1, -1.57); // door1开 28 | wb_supervisor_field_set_sf_float(field2, 0); // door2关 29 | } 30 | else 31 | { 32 | wb_supervisor_field_set_sf_float(field1, 0); // door1关 33 | wb_supervisor_field_set_sf_float(field2, 1.57); // door2开 34 | } 35 | cnt++; 36 | if (cnt > 300) 37 | cnt = cnt % 300; 38 | } 39 | 40 | wb_robot_cleanup(); 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /Final_project/controllers/wasd/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | ### CXX_SOURCES = Move.cpp ground.cpp 43 | ### CXX_SOURCES = main.cpp 44 | ### 45 | ### ---- Compilation options ---- 46 | ### if special compilation flags are necessary: 47 | CFLAGS = -std=c++17 48 | ### 49 | ### ---- Linked libraries ---- 50 | ### if your program needs additional libraries: 51 | ### INCLUDE = -I/opt/homebrew/opt/opencv/include/opencv4 52 | ### LIBRARIES = -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 53 | ### 54 | ### ---- Linking options ---- 55 | ### if special linking flags are needed: 56 | ### LFLAGS = -s 57 | ### 58 | ### ---- Webots included libraries ---- 59 | ### if you want to use the Webots C API in your C++ controller program: 60 | ### USE_C_API = true 61 | ### 62 | ### ---- Debug mode ---- 63 | ### if you want to display the gcc command line for compilation and link, as 64 | ### well as the rm command details used for cleaning: 65 | ### VERBOSE = 1 66 | ### 67 | ###----------------------------------------------------------------------------- 68 | 69 | ### Do not modify: this includes Webots global Makefile.include 70 | null := 71 | space := $(null) $(null) 72 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 73 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 74 | -------------------------------------------------------------------------------- /Final_project/controllers/wasd/wasd.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace webots; 21 | 22 | const double pi = acos(-1); 23 | const double eps = 1e-6; 24 | const int up = 20; 25 | const int update_up = 16; 26 | // 原点在左上角，x y意义同坐标系下的意义 27 | /* world 1 */ 28 | // const double WORLD_WIDTH = 5; 29 | // const double WORLD_HEIGHT = 5; 30 | // const int obs_r = 1; 31 | // const int start_x = 90, start_y = 5, end_x = 5, end_y = 90; 32 | /* --------------------------------------- */ 33 | /* world 2 */ 34 | // const double WORLD_WIDTH = 4.5; 35 | //const double WORLD_HEIGHT = 6; 36 | // const int obs_r = 1; 37 | // const int start_x = 90, start_y = 5, end_x = 5, end_y = 90; 38 | /* --------------------------------------- */ 39 | /* world 3 */ 40 | const double WORLD_WIDTH = 10; 41 | const double WORLD_HEIGHT = 10; 42 | const int obs_r = 1; 43 | const int start_x = 90, start_y = 5, end_x = 5, end_y = 90; 44 | /* --------------------------------------- */ 45 | 46 | #define RED 0xBB2222 47 | #define GREEN 0x22BB11 48 | #define BLUE 0x2222BB 49 | #define BLACK 0x000000 50 | 51 | #define NEW 0 52 | #define OPEN 1 53 | #define CLOSED 2 54 | 55 | int occupy[1000][1000] = {}; 56 | int stop[1000][1000] = {}; 57 | 58 | class Coordinates 59 | { 60 | public: 61 | double x, y; 62 | Coordinates(double xx, double yy) : x(xx), y(yy) {} 63 | Coordinates() : x(0), y(0) {} 64 | bool operator==(const Coordinates &c) { return x == c.x && y == c.y; } 65 | bool operator!=(const Coordinates &c) { return !(this->operator==(c)); } 66 | bool operator==(const Coordinates &c) const 67 | { 68 | return (x == c.x && y == c.y); 69 | } 70 | }; 71 | 72 | /** 73 | * @brief PID控制器，用于PID控制算法 74 | * 75 | */ 76 | class PID 77 | { 78 | public: 79 | Coordinates SetPoint; // 设定目标 Desired Value 80 | Coordinates SumError; // 误差累计 81 | 82 | double Proportion; // 比例常数 Proportional Const 83 | double Integral; // 积分常数 Integral Const 84 | double Derivative; // 微分常数 Derivative Const 85 | 86 | Coordinates LastError; // Error[-1] 87 | Coordinates PrevError; // Error[-2] 88 | 89 | PID() 90 | { 91 | Proportion = Integral = Derivative = SetPoint.x = SetPoint.y = 92 | SumError.x = SumError.y = LastError.x = LastError.y = PrevError.x = 93 | PrevError.y = 0; 94 | } 95 | 96 | /** 97 | * @brief 设置期望点 98 | * 99 | * @param setpoint 期望点 100 | */ 101 | void PID_SetPoint(Coordinates *setpoint) 102 | { 103 | SetPoint.x = setpoint->x; 104 | SetPoint.y = setpoint->y; 105 | } 106 | void init() 107 | { 108 | SetPoint.x = SetPoint.y = SumError.x = SumError.y = LastError.x = 109 | LastError.y = PrevError.x = PrevError.y = 0; 110 | 111 | Proportion = 0.97; 112 | Integral = 0.05; 113 | Derivative = 50; 114 | } 115 | 116 | /** 117 | * @brief x方向PID算法 118 | * 119 | * @param CurPoint 当前位置 120 | * @return double PID反馈量 121 | */ 122 | double PID_x_PosLocCalc(Coordinates *CurPoint) 123 | { 124 | double iError_x, dError_x; 125 | 126 | iError_x = SetPoint.x - CurPoint->x; // 偏差 127 | SumError.x += iError_x; // //累计偏差，用于积分 128 | if (SumError.x > 10.0) //积分限幅10 129 | SumError.x = 10.0; 130 | else if (SumError.x < -10.0) 131 | SumError.x = -10.0; 132 | dError_x = iError_x - LastError.x; // 当前微分 133 | LastError.x = iError_x; 134 | 135 | return (double)(Proportion * iError_x // 比例项 136 | + Integral * SumError.x // 积分项 137 | + Derivative * dError_x); 138 | } 139 | 140 | /** 141 | * @brief y方向PID算法 142 | * 143 | * @param CurPoint 当前位置 144 | * @return double PID反馈量 145 | */ 146 | double PID_y_PosLocCalc(Coordinates *CurPoint) 147 | { 148 | double iError_y, dError_y; 149 | 150 | iError_y = SetPoint.y - CurPoint->y; // 偏差 151 | SumError.y += iError_y; // //累计偏差，用于积分 152 | if (SumError.y > 10.0) //积分限幅10 153 | SumError.y = 10.0; 154 | else if (SumError.y < -10.0) 155 | SumError.y = -10.0; 156 | dError_y = iError_y - LastError.y; // 当前微分 157 | LastError.y = iError_y; 158 | 159 | return (double)(Proportion * iError_y // 比例项 160 | + Integral * SumError.y // 积分项 161 | + Derivative * dError_y); 162 | } 163 | }; 164 | 165 | vector path; // #TODO：存放规划后路径 166 | 167 | // 返回一个浮点数的符号 168 | int sign(double x) 169 | { 170 | if (x > eps) 171 | return 1; 172 | if (x < -eps) 173 | return -1; 174 | return 0; 175 | } 176 | 177 | // 激光雷达感知过程 178 | class process 179 | { 180 | int **cnt; // 用于统计 181 | 182 | int n, m; // 栅栏地图大小 183 | 184 | double st; //常数 -pi / 2; 185 | 186 | double real_n, real_m; // 真实地图大小 187 | 188 | double unit; //单元数 189 | 190 | double max_range; //雷达最大范围 191 | 192 | public: 193 | // 真实地图坐标转化为栅栏地图坐标 194 | pair real_to_pic(double x, double y) 195 | { 196 | auto xx = (x + real_m / 2) / real_m; 197 | xx = max(min(xx, 0.99), 0.0); 198 | auto yy = -(y - real_n / 2) / real_n; 199 | yy = max(min(yy, 0.99), 0.0); 200 | int nxx = m * xx; 201 | if (nxx == m) 202 | -- nxx; 203 | int nyy = n * yy; 204 | if (nyy == n) 205 | -- nyy; 206 | return make_pair(nxx, nyy); 207 | } 208 | 209 | // 初始化内存 210 | process(int resolve, 211 | double max_range = 1, 212 | int n = 512, 213 | int m = 512, 214 | double real_n = 10, 215 | double real_m = 10) 216 | : max_range(max_range), n(n), m(m), real_n(real_n), real_m(real_m) 217 | { 218 | st = -pi / 2; 219 | unit = 2 * pi / resolve; 220 | cnt = new int *[n]; 221 | for (int i = 0; i < n; ++i) 222 | { 223 | cnt[i] = new int[m]; 224 | memset(cnt[i], 0, sizeof(int) * m); 225 | } 226 | } 227 | 228 | // 释放内存 229 | ~process() 230 | { 231 | for (int i = 0; i < m; ++i) 232 | { 233 | delete[] cnt[i]; 234 | } 235 | delete[] cnt; 236 | } 237 | 238 | // 增加计数, 计数大于一定阈值才判定为障碍 239 | void add(int total, const float *dis, double x, double y) 240 | { 241 | for (int i = 0; i < total; ++i) 242 | { 243 | if (sign(dis[i] - max_range) == 0) 244 | continue; 245 | auto [ny, nx] = real_to_pic(x + dis[i] * cos(st - i * unit), 246 | y + dis[i] * sin(st - i * unit)); 247 | if (nx >= 0 && nx < n && ny >= 0 && ny < m) 248 | cnt[nx][ny]++; 249 | } 250 | } 251 | 252 | // 用红色绘制障碍 253 | void draw(Display *display) 254 | { 255 | display->setColor(RED); 256 | for (int i = 0; i < n; ++i) 257 | { 258 | for (int j = 0; j < m; ++j) 259 | { 260 | if (occupy[i][j]) 261 | display->drawPixel(i, j); 262 | } 263 | } 264 | } 265 | 266 | // 通过计数更新障碍 267 | void update(Display *display) 268 | { 269 | display->setColor(RED); 270 | for (int i = 0; i < n; ++i) 271 | { 272 | for (int j = 0; j < m; ++j) 273 | { 274 | if (cnt[i][j] >= up) 275 | { 276 | occupy[i][j] = 1; 277 | display->drawPixel(j, i); 278 | } 279 | } 280 | } 281 | 282 | for (int i = 0; i < n; ++i) 283 | memset(cnt[i], 0, sizeof(int) * m); 284 | } 285 | 286 | // 用蓝色绘制曾经到达过的点 287 | void draw_pos(Display *display, const double *pos) 288 | { 289 | display->setColor(BLUE); 290 | auto [y, x] = real_to_pic(pos[0], pos[1]); 291 | display->drawPixel(y, x); 292 | } 293 | }; 294 | 295 | class Dstar 296 | { 297 | public: 298 | int start_x, start_y, end_x, end_y, cur_x, cur_y;// 开始位置, 结束位置, 当前位置 299 | 300 | int m, n;// 地图大小 301 | 302 | int stay, flag, turn;// 用来解决卡墙问题的变量 303 | 304 | int **state, **nxt_x, **nxt_y;// 节点当前状态 305 | 306 | double **hscore, **kscore;// h值和k值 307 | 308 | vector> openlist;// OPEN list 309 | 310 | // 八个方向的邻居 311 | int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1}, 312 | dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1}; 313 | 314 | vector> path;// 记录路径 315 | 316 | // 内存初始化 317 | Dstar(int start_x, int start_y, int end_x, int end_y, int m, int n) 318 | : start_x(start_x), 319 | start_y(start_y), 320 | end_x(end_x), 321 | end_y(end_y), 322 | cur_x(start_x), 323 | cur_y(start_y), 324 | m(m), 325 | n(n) 326 | { 327 | path.clear(); 328 | state = new int *[n]; 329 | nxt_x = new int *[n]; 330 | nxt_y = new int *[n]; 331 | hscore = new double *[n]; 332 | kscore = new double *[n]; 333 | for (int i = 0; i < n; ++i) 334 | { 335 | state[i] = new int[m]; 336 | nxt_x[i] = new int[m]; 337 | nxt_y[i] = new int[m]; 338 | hscore[i] = new double[m]; 339 | kscore[i] = new double[m]; 340 | // 设置为New 341 | memset(state[i], 0, sizeof(int) * m); 342 | memset(nxt_x[i], 0, sizeof(int) * m); 343 | memset(nxt_y[i], 0, sizeof(int) * m); 344 | } 345 | for (int i = 0; i < n; i++) 346 | { 347 | for (int j = 0; j < m; j++) 348 | { 349 | hscore[i][j] = calDis(j, i, end_y, end_x); 350 | kscore[i][j] = hscore[i][j]; 351 | } 352 | } 353 | nxt_x[end_y][end_x] = end_x; 354 | nxt_y[end_y][end_x] = end_y; 355 | openlist.push_back(make_pair(end_y, end_x)); 356 | stay = 0; 357 | flag = 0; 358 | turn = 0; 359 | } 360 | 361 | // 回收内存 362 | ~Dstar() 363 | { 364 | for (int i = 0; i < m; ++i) 365 | { 366 | delete[] hscore[i]; 367 | delete[] kscore[i]; 368 | } 369 | delete[] hscore; 370 | delete[] kscore; 371 | } 372 | 373 | // 得到OPEN list中最小k值 374 | double get_mink() 375 | { 376 | if (!openlist.size()) 377 | return -1; 378 | double res = 1e6; 379 | for (int i = 0; i < (int)openlist.size(); i++) 380 | { 381 | res = min(res, kscore[openlist[i].first][openlist[i].second]); 382 | } 383 | return res; 384 | } 385 | 386 | // 得到OPEN list中最小k值对应的节点 387 | pair get_min_state() 388 | { 389 | if (!openlist.size()) 390 | return make_pair(-1, -1); 391 | double mink = get_mink(); 392 | pair res = make_pair(-1, -1); 393 | for (int i = 0; i < (int)openlist.size(); i++) 394 | { 395 | if (mink == kscore[openlist[i].first][openlist[i].second]) 396 | { 397 | res = openlist[i]; 398 | openlist.erase(openlist.begin() + i); 399 | return res; 400 | } 401 | } 402 | return res; 403 | } 404 | 405 | // 向 OPEN list 插入更新的节点 406 | void insert(int x, int y, double h) 407 | { 408 | double k; 409 | if (state[y][x] == NEW) 410 | { 411 | k = h; 412 | } 413 | if (state[y][x] == OPEN) 414 | { 415 | k = min(kscore[y][x], h); 416 | } 417 | if (state[y][x] == CLOSED) 418 | { 419 | k = min(hscore[y][x], h); 420 | } 421 | kscore[y][x] = k; 422 | hscore[y][x] = h; 423 | state[y][x] = OPEN; 424 | openlist.push_back(make_pair(y, x)); 425 | } 426 | 427 | // 节点处理函数 428 | int process_state() 429 | { 430 | // 取所有节点中 k 值最小的 431 | pair tmp = get_min_state(); 432 | int y = tmp.first, x = tmp.second; 433 | // OPEN list 为空，结束 434 | if (y == -1 && x == -1) 435 | return -1; 436 | 437 | //获得该点k值，并将该点弹出Openlist队列 438 | double h = hscore[y][x], k = kscore[y][x]; 439 | state[y][x] = CLOSED; 440 | 441 | // 该点 X 处于 RAISE 状态 442 | if (k < h) 443 | { 444 | //遍历邻域节点 Y 445 | for (int i = 0; i < 8; i++) 446 | { 447 | if (x + dx[i] < 0 || x + dx[i] >= m) 448 | continue; 449 | if (y + dy[i] < 0 || y + dy[i] >= n) 450 | continue; 451 | int extend_x = x + dx[i], extend_y = y + dy[i]; 452 | double cost = calDis(x, y, extend_x, extend_y); 453 | 454 | // 遇到障碍重新设置其 h 值和 k 值 455 | if (stop[extend_y][extend_x] || obstacle(extend_x, extend_y)) 456 | { 457 | hscore[extend_y][extend_x] = kscore[extend_y][extend_x] = 458 | 1e6; 459 | stop[extend_y][extend_x] = 1; 460 | } 461 | 462 | //Y 不处于RAISE 状态，且用其更新x后，X 的 h 值更小, 那么就更新 X 的父节点及其 h 值 463 | if (hscore[extend_y][extend_x] <= k && 464 | hscore[y][x] > hscore[extend_y][extend_x] + cost) 465 | { 466 | nxt_x[y][x] = extend_x, nxt_y[y][x] = extend_y; 467 | hscore[y][x] = hscore[extend_y][extend_x] + cost; 468 | } 469 | } 470 | } 471 | 472 | // 该点 X 处于 LOWER 状态 473 | if (k == h) 474 | { 475 | // 遍历邻域节点Y 476 | for (int i = 0; i < 8; i++) 477 | { 478 | if (x + dx[i] < 0 || x + dx[i] >= m) 479 | continue; 480 | if (y + dy[i] < 0 || y + dy[i] >= n) 481 | continue; 482 | int extend_x = x + dx[i], extend_y = y + dy[i]; 483 | int back_flag = (nxt_x[extend_y][extend_x] == x && 484 | nxt_y[extend_y][extend_x] == y); 485 | double cost = calDis(x, y, extend_x, extend_y); 486 | 487 | // 分以下三种情况 488 | // Y是首次添加到OPEN list并处理 489 | // Y是X的子节点，并且其h值不需要改变 490 | // Y不是X的子节点，并且其h值可以变得更小 491 | if (state[extend_y][extend_x] == NEW || 492 | (back_flag && 493 | hscore[extend_y][extend_x] != hscore[y][x] + cost) || 494 | (!back_flag && 495 | hscore[extend_y][extend_x] > hscore[y][x] + cost)) 496 | { 497 | // 将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中 498 | nxt_x[extend_y][extend_x] = x, 499 | nxt_y[extend_y][extend_x] = y; 500 | insert(extend_x, extend_y, hscore[y][x] + cost); 501 | } 502 | } 503 | } 504 | else 505 | { 506 | // 遍历邻域节点Y 507 | for (int i = 0; i < 8; i++) 508 | { 509 | if (x + dx[i] < 0 || x + dx[i] >= m) 510 | continue; 511 | if (y + dy[i] < 0 || y + dy[i] >= n) 512 | continue; 513 | int extend_x = x + dx[i], extend_y = y + dy[i]; 514 | int back_flag = (nxt_x[extend_y][extend_x] == x && 515 | nxt_y[extend_y][extend_x] == y); 516 | double cost = calDis(x, y, extend_x, extend_y); 517 | 518 | // 分以下两种情况 519 | // Y是首次添加到OPEN list并处理 520 | // Y是X的子节点，并且其h值不需要改变 521 | if (state[extend_y][extend_x] == NEW || 522 | (back_flag && 523 | hscore[extend_y][extend_x] != hscore[y][x] + cost)) 524 | { 525 | // 将X作为Y的父节点，修改其h值，并将Y点添加到Openlist中 526 | nxt_x[extend_y][extend_x] = x, 527 | nxt_y[extend_y][extend_x] = y; 528 | insert(extend_x, extend_y, hscore[y][x] + cost); 529 | } 530 | else 531 | { 532 | //Y不是X的子节点，并且其h值可以变得更小 533 | if (!back_flag && 534 | hscore[extend_y][extend_x] > hscore[y][x] + cost) 535 | { 536 | //修改X的h值，并将其点添加到Openlist中 537 | insert(x, y, hscore[y][x]); 538 | } 539 | //Y不是X的子节点，并且其h值可以变得更小, 并且Y不在Openlist中，且h值处于上升状态 540 | else if (!back_flag && 541 | hscore[y][x] > 542 | hscore[extend_y][extend_x] + cost && 543 | state[extend_y][extend_x] == CLOSED && 544 | hscore[extend_y][extend_x] > k) 545 | { 546 | //修改Y的h值，并将其点添加到Openlist中 547 | insert(extend_x, extend_y, hscore[extend_y][extend_x]); 548 | } 549 | } 550 | } 551 | } 552 | //返回该点k值 553 | return get_mink(); 554 | } 555 | 556 | // 绘制路径 557 | void draw(Display *display) 558 | { 559 | display->setColor(GREEN); 560 | for (int i = 0; i < (int)path.size(); i++) 561 | { 562 | if (occupy[path[i].second][path[i].first]) 563 | continue; 564 | display->drawPixel(path[i].first, path[i].second); 565 | } 566 | } 567 | 568 | // 清除上次绘制的路径, 实现路径的动态变化 569 | void drawclear(Display *display) 570 | { 571 | display->setColor(BLACK); 572 | for (int i = 0; i < (int)path.size(); i++) 573 | { 574 | if (occupy[path[i].second][path[i].first]) 575 | continue; 576 | display->drawPixel(path[i].first, path[i].second); 577 | } 578 | } 579 | 580 | // 生成路径 581 | void generate_path(Display *display) 582 | { 583 | // 遍历得到路径 584 | path.clear(); 585 | int x = cur_x, y = cur_y; 586 | int cnt = 0; 587 | while (1) 588 | { 589 | if (cnt >= 10000) 590 | break; 591 | cnt += 1; 592 | path.push_back(make_pair(x, y)); 593 | if (x == end_x && y == end_y) 594 | break; 595 | if (x == -1 || y == -1) 596 | break; 597 | int tmp_x = nxt_x[y][x], tmp_y = nxt_y[y][x]; 598 | x = tmp_x, y = tmp_y; 599 | if (x == end_x && y == end_y) 600 | break; 601 | } 602 | // 遇到卡墙情况, 进行时间计数 603 | if (obstacle(cur_x, cur_y)) 604 | { 605 | stay++; 606 | } 607 | // 超过一定时间进行调整位置 608 | if (stay >= 25) 609 | { 610 | flag = 15; 611 | stay = 0; 612 | turn = !turn; 613 | } 614 | // 按照路径反方向调整位置 615 | if (flag) 616 | { 617 | flag--; 618 | stay = 0; 619 | vector> tmp_path; 620 | for (int i = 0; i < 10; i++) 621 | { 622 | if (turn) 623 | { 624 | tmp_path.push_back(make_pair(2 * cur_x - path[i * 2].first, cur_y)); 625 | } 626 | else 627 | { 628 | tmp_path.push_back(make_pair(cur_x, 2 * cur_y - path[i * 2].second)); 629 | } 630 | } 631 | path = tmp_path; 632 | } 633 | // 绘制路径 634 | draw(display); 635 | } 636 | 637 | // 初始化路径 638 | void work(Display *display) 639 | { 640 | drawclear(display); 641 | kscore[end_y][end_x] = 0; 642 | state[cur_y][cur_x] = NEW; 643 | while (1) 644 | { 645 | if (state[cur_y][cur_x] == CLOSED) 646 | { 647 | break; 648 | } 649 | process_state(); 650 | } 651 | generate_path(display); 652 | } 653 | 654 | // 获取路径 655 | vector> get_path() { return path; } 656 | 657 | // 遇到障碍物时进行路径更新 658 | void update(int cur_x, int cur_y, Display *display) 659 | { 660 | drawclear(display); 661 | this->cur_x = cur_x, this->cur_y = cur_y; 662 | kscore[end_y][end_x] = 0; 663 | int x = cur_x, y = cur_y; 664 | int cnt = 0; 665 | while (1) 666 | { 667 | if (cnt >= 1000) 668 | break; 669 | cnt += 1; 670 | if (x == end_x && y == end_y) 671 | break; 672 | if (x == -1 || y == -1) 673 | break; 674 | // 遇到障碍物重新计算cost 675 | if (stop[nxt_y[y][x]][nxt_x[y][x]] || 676 | obstacle(nxt_x[y][x], nxt_y[y][x])) 677 | { 678 | stop[nxt_y[y][x]][nxt_x[y][x]] = 1; 679 | modify(x, y); 680 | continue; 681 | } 682 | int tmp_x = nxt_x[y][x], tmp_y = nxt_y[y][x]; 683 | x = tmp_x, y = tmp_y; 684 | if (x == end_x && y == end_y) 685 | break; 686 | } 687 | generate_path(display); 688 | } 689 | 690 | // 重新计算节点cost 691 | void modify_cost(int x, int y) 692 | { 693 | if (state[y][x] == CLOSED) 694 | { 695 | double cost = calDis(x, y, nxt_x[y][x], nxt_y[y][x]); 696 | double tmp = hscore[y][x] + cost; 697 | insert(x, y, tmp); 698 | } 699 | } 700 | 701 | // 重新计算节点cost, 并递归更新受影响的其他节点 702 | void modify(int x, int y) 703 | { 704 | modify_cost(x, y); 705 | int cnt = 0; 706 | while (1) 707 | { 708 | if (cnt >= 10) 709 | break; 710 | cnt += 1; 711 | double mink = process_state(); 712 | if (mink >= hscore[y][x]) 713 | break; 714 | } 715 | } 716 | 717 | // 判断一个点是否在障碍上 718 | int obstacle(int x, int y) 719 | { 720 | if (occupy[y][x]) 721 | return 1; 722 | if (abs(end_x - x) <= 2 && abs(end_y - y) <= 2) 723 | return 0; 724 | for (int i = -obs_r; i <= obs_r; i++) 725 | { 726 | for (int j = -obs_r; j <= obs_r; j++) 727 | { 728 | if (x + i < 0 || x + i >= m) 729 | continue; 730 | if (y + j < 0 || y + j >= n) 731 | continue; 732 | if (occupy[y + j][x + i]) 733 | return 1; 734 | } 735 | } 736 | return 0; 737 | } 738 | 739 | // 计算两点之间的距离 740 | double calDis(int x1, int y1, int x2, int y2) 741 | { 742 | // 遇到了障碍 743 | if (stop[y1][x1] || stop[y2][x2] || obstacle(x1, y1) || 744 | obstacle(x2, y2)) 745 | return 1e6; 746 | return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); 747 | } 748 | }; 749 | 750 | /** 751 | * @brief 将目标路径存储到Path中，传给运动控制部分 752 | * 753 | * @param dstar 规划控制器 754 | * @param path 路径存储结构 755 | */ 756 | void loadPath(Dstar *dstar, vector *path) 757 | { 758 | (*path).clear(); 759 | for (int i = 0; i < dstar->path.size(); ++i) 760 | { 761 | Coordinates *ls = 762 | new Coordinates(dstar->path[i].first, dstar->path[i].second); 763 | (*path).push_back(ls); 764 | } 765 | } 766 | 767 | char action_x(double pid, int cur) { 768 | double d = pid - cur; 769 | if (abs(d) <= 1) return 'N'; 770 | char ret; 771 | ret = d > 0 ? 'S' : 'W'; 772 | return ret; 773 | } 774 | 775 | char action_y(double pid, int cur) { 776 | double d = pid - cur; 777 | if (abs(d) <= 1) return 'N'; 778 | char ret; 779 | ret = d > 0 ? 'A' : 'D'; 780 | return ret; 781 | } 782 | 783 | int main() 784 | { 785 | Robot *robot = new Robot(); 786 | 787 | int timeStep = (int)robot->getBasicTimeStep(); 788 | cout << timeStep << endl; 789 | 790 | Keyboard keyboard; 791 | keyboard.enable(1); 792 | 793 | Lidar *lidar = robot->getLidar("lidar"); 794 | lidar->enable(1); 795 | lidar->enablePointCloud(); 796 | const int lidar_point_count = lidar->getHorizontalResolution(); 797 | 798 | GPS *gps = robot->getGPS("gps"); 799 | gps->enable(1); 800 | 801 | InertialUnit *inertial = robot->getInertialUnit("yaw"); 802 | inertial->enable(1); 803 | 804 | Display *display = robot->getDisplay("display"); 805 | 806 | display->setOpacity(1); 807 | 808 | int width = display->getWidth(); 809 | int height = display->getHeight(); 810 | 811 | process slam(lidar_point_count, lidar->getMaxRange(), height, width, 812 | WORLD_HEIGHT, WORLD_WIDTH); 813 | 814 | // Dstar 算法 815 | Dstar dstar(start_x, start_y, end_x, end_y, height, width); 816 | // 生成初始地图 817 | dstar.work(display); 818 | 819 | loadPath(&dstar, &path); 820 | 821 | Motor *motors[4]; 822 | 823 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 824 | double speed1[4]; 825 | double speed2[4]; 826 | double speed3[4]; 827 | 828 | for (int i = 0; i < 4; i++) 829 | { // 初始化电机和距离传感器 830 | motors[i] = robot->getMotor(wheelsNames[i]); 831 | motors[i]->setPosition(std::numeric_limits::infinity()); 832 | motors[i]->setVelocity(0.0); 833 | speed1[i] = 0; 834 | speed2[i] = 0; 835 | speed3[i] = 0; 836 | } 837 | 838 | const double velocity = 50; 839 | const double velocity2 = 10; 840 | 841 | double speed_forward[4] = {velocity, velocity, velocity, velocity}; 842 | double speed_backward[4] = {-velocity, -velocity, -velocity, -velocity}; 843 | double speed_leftward[4] = {velocity, -velocity, velocity, -velocity}; 844 | double speed_rightward[4] = {-velocity, velocity, -velocity, velocity}; 845 | 846 | double speed_leftCircle[4] = {velocity2, -velocity2, -velocity2, velocity2}; 847 | double speed_rightCircle[4] = {-velocity2, velocity2, velocity2, 848 | -velocity2}; 849 | 850 | const double front = 1.57; 851 | 852 | int count = 0; 853 | 854 | int idx = 0; 855 | 856 | PID pos_pid; // PID manager 857 | pos_pid.init(); // 对PID控制器进行初始化 858 | 859 | double xx = 1.95; 860 | double yy = 1.92; 861 | 862 | 863 | for (int i = 0; i < path.size(); i++) 864 | { 865 | cout << path[i]->x << " " << path[i]->y << endl; 866 | } 867 | 868 | while (robot->step(timeStep) != -1) 869 | { 870 | const double *yaw = inertial->getRollPitchYaw(); 871 | 872 | if (fabs(yaw[0] - front) >= 0.05) 873 | { 874 | int dir = (yaw[0] < front && yaw[0] > -front ? -1 : 1); 875 | for (int i = 0; i < 4; ++i) 876 | { 877 | speed3[i] = speed_rightCircle[i] * dir; 878 | motors[i]->setVelocity(speed3[i]); 879 | } 880 | continue; 881 | } 882 | else 883 | { 884 | for (int i = 0; i < 4; ++i) 885 | speed3[i] = 0; 886 | } 887 | count++; 888 | const double *pos = gps->getValues(); 889 | const float *dis = lidar->getRangeImage(); 890 | 891 | // 计算障碍的计数 892 | slam.add(lidar_point_count, dis, pos[0], pos[1]); 893 | 894 | // for (int i = 0; i < lidar_point_count; ++i) 895 | // { 896 | // printf("%.4f ", dis[i]); 897 | // } 898 | // puts(""); 899 | 900 | // 进行地图的更新和路径的重新规划 901 | if (count == update_up) 902 | { 903 | count = 0; 904 | slam.update(display);// 更新地图 905 | pair tmp = slam.real_to_pic(pos[0], pos[1]);// 当前位置 906 | dstar.update(tmp.first, tmp.second, display);// 重新规划路径 907 | 908 | // 转储Path 909 | loadPath(&dstar, &path); 910 | 911 | idx = 0; // 下标重置 912 | pos_pid.init(); // 初始化PID控制器 913 | } 914 | 915 | slam.draw_pos(display, pos); 916 | printf("position & yaw: %.2lf %.2lf %.2lf %.2lf\n", pos[0], pos[1], 917 | pos[2], yaw[0]); 918 | 919 | // 设置PID目标值 920 | pos_pid.PID_SetPoint(path[idx]); 921 | 922 | double cx = pos[0]; 923 | double cy = pos[1]; 924 | 925 | // 记录当前位置 926 | auto [curx, cury] = slam.real_to_pic(pos[0], pos[1]); 927 | 928 | Coordinates cur(curx, cury); 929 | 930 | // 根据当前位置计算PID反馈量，并加到当前位置中 931 | double pid_x = pos_pid.PID_x_PosLocCalc(&cur) + cur.x; 932 | double pid_y = pos_pid.PID_y_PosLocCalc(&cur) + cur.y; 933 | 934 | int keyValue1; 935 | int keyValue2; 936 | 937 | // 根据PID反馈量进行决策 938 | cout << "cur: " << curx << " " << cury << endl; 939 | keyValue1 = action_x(pid_x, curx); 940 | keyValue2 = action_y(pid_y, cury); 941 | 942 | cout << "action: " << char(keyValue1) << ", " << char(keyValue2) 943 | << endl; 944 | 945 | cout << "gps_car Value X: " << curx << " Y: " << cury 946 | << " Z: " << pos[2] << endl; 947 | 948 | cout << "pid Value X: " << pid_x << " Y: " << pid_y << endl; 949 | 950 | cout << "target Value X: " << path[idx]->x << " Y: " << path[idx]->y 951 | << endl; 952 | 953 | for (int i = 0; i < 4; i++) 954 | { 955 | speed1[i] = speed2[i] = 0; 956 | } 957 | 958 | if (keyValue1 == 'W') 959 | { 960 | for (int i = 0; i < 4; i++) 961 | { 962 | speed1[i] = speed_forward[i]; 963 | } 964 | } 965 | else if (keyValue1 == 'S') 966 | { 967 | for (int i = 0; i < 4; i++) 968 | { 969 | speed1[i] = speed_backward[i]; 970 | } 971 | } 972 | else if (keyValue1 == 'A') 973 | { 974 | for (int i = 0; i < 4; i++) 975 | { 976 | speed1[i] = speed_leftward[i]; 977 | } 978 | } 979 | else if (keyValue1 == 'D') 980 | { 981 | for (int i = 0; i < 4; i++) 982 | { 983 | speed1[i] = speed_rightward[i]; 984 | } 985 | } 986 | else if (keyValue1 == 'Q') 987 | { 988 | for (int i = 0; i < 4; i++) 989 | { 990 | speed1[i] = speed_leftCircle[i]; 991 | } 992 | } 993 | else if (keyValue1 == 'E') 994 | { 995 | for (int i = 0; i < 4; i++) 996 | { 997 | speed1[i] = speed_rightCircle[i]; 998 | } 999 | } 1000 | else 1001 | { 1002 | for (int i = 0; i < 4; i++) 1003 | { 1004 | speed1[i] = 0; 1005 | } 1006 | } 1007 | if (keyValue2 == 'W') 1008 | { 1009 | for (int i = 0; i < 4; i++) 1010 | { 1011 | speed2[i] = speed_forward[i]; 1012 | } 1013 | } 1014 | else if (keyValue2 == 'S') 1015 | { 1016 | for (int i = 0; i < 4; i++) 1017 | { 1018 | speed2[i] = speed_backward[i]; 1019 | } 1020 | } 1021 | else if (keyValue2 == 'A') 1022 | { 1023 | for (int i = 0; i < 4; i++) 1024 | { 1025 | speed2[i] = speed_leftward[i]; 1026 | } 1027 | } 1028 | else if (keyValue2 == 'D') 1029 | { 1030 | for (int i = 0; i < 4; i++) 1031 | { 1032 | speed2[i] = speed_rightward[i]; 1033 | } 1034 | } 1035 | else 1036 | { 1037 | for (int i = 0; i < 4; i++) 1038 | { 1039 | speed2[i] = 0; 1040 | } 1041 | } 1042 | cout << "speed : "; 1043 | for (int i = 0; i < 4; i++) 1044 | { 1045 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 1046 | cout << speed1[i] + speed2[i] + speed3[i] << ", "; 1047 | } 1048 | cout << endl; 1049 | if (abs(end_y - cury) <= 1 && abs(end_x - curx) <= 1) 1050 | { 1051 | cout << endl 1052 | << "============ Arrive Goal ! ============" << endl; 1053 | for (int i = 0; i < 4; i++) 1054 | { 1055 | motors[i]->setVelocity(0); 1056 | } 1057 | break; 1058 | } 1059 | if ((abs(pid_y - cury) <= 3 && abs(pid_x - curx) <= 3) && 1060 | idx < path.size()) 1061 | { 1062 | idx++; 1063 | } 1064 | } 1065 | delete robot; 1066 | return 0; 1067 | } 1068 | -------------------------------------------------------------------------------- /Final_project/worlds/world1.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2021a utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material1 "ExteriorWheelMat" 7 | coulombFriction [ 8 | 1.8, 0, 0.2 9 | ] 10 | frictionRotation 0.965 0 11 | bounce 0 12 | forceDependentSlip [ 13 | 10, 0 14 | ] 15 | } 16 | ContactProperties { 17 | material1 "InteriorWheelMat" 18 | coulombFriction [ 19 | 1.8, 0, 0.2 20 | ] 21 | frictionRotation -0.965 0 22 | bounce 0 23 | forceDependentSlip [ 24 | 10, 0 25 | ] 26 | } 27 | ] 28 | } 29 | Viewpoint { 30 | orientation 0.9856405345181397 0.006403397693210816 0.16873569039411412 0.07736081503109599 31 | position -1.383971025228012 -1.743616369388367 21.832517640553636 32 | followType "None" 33 | } 34 | TexturedBackground { 35 | } 36 | TexturedBackgroundLight { 37 | } 38 | RectangleArena { 39 | rotation 1 0 0 1.57 40 | floorSize 5 5 41 | wallHeight 0.5 42 | } 43 | Wall { 44 | translation 0.75 1 0 45 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 46 | name "wall1" 47 | size 3.5 0.5 0.1 48 | } 49 | Wall { 50 | translation -0.75 -1 0 51 | rotation 1 0 0 1.5708 52 | name "wall2" 53 | size 3.5 0.5 0.1 54 | } 55 | DEF car Robot { 56 | translation 1.9503566561605703 1.9201896923280404 0.05707612160994237 57 | rotation -2.1949434369512534e-08 0.0004000847233330271 0.9999999199661036 3.141582349006734 58 | children [ 59 | Display { 60 | width 100 61 | height 100 62 | } 63 | InertialUnit { 64 | name "yaw" 65 | model "yaw" 66 | } 67 | DEF gps GPS { 68 | } 69 | Lidar { 70 | translation 0 0 0.075 71 | rotation 1 0 0 1.5708 72 | horizontalResolution 64 73 | numberOfLayers 1 74 | spherical FALSE 75 | type "rotating" 76 | resolution 0.001 77 | rotatingHead Solid { 78 | rotation 0 -1 0 2.9802322387695313e-08 79 | children [ 80 | Transform { 81 | translation -0.055 0 0 82 | children [ 83 | Shape { 84 | appearance PBRAppearance { 85 | } 86 | geometry Sphere { 87 | radius 0.03 88 | } 89 | } 90 | ] 91 | } 92 | Transform { 93 | translation 0.055 0 0 94 | children [ 95 | Shape { 96 | appearance PBRAppearance { 97 | } 98 | geometry Sphere { 99 | radius 0.03 100 | } 101 | } 102 | ] 103 | } 104 | ] 105 | } 106 | } 107 | HingeJoint { 108 | jointParameters HingeJointParameters { 109 | position 1.0805429119930097e-06 110 | axis 0 1 0 111 | anchor 0.1 -0.15 0 112 | } 113 | device [ 114 | DEF motor1 RotationalMotor { 115 | name "motor1" 116 | maxVelocity 100 117 | } 118 | ] 119 | endPoint Solid { 120 | translation 0.1 -0.13 0 121 | rotation 0 1 0 1.0807194927018264e-06 122 | children [ 123 | Shape { 124 | appearance Appearance { 125 | material Material { 126 | } 127 | } 128 | geometry DEF lunzi Cylinder { 129 | height 0.05 130 | radius 0.06 131 | } 132 | } 133 | ] 134 | contactMaterial "InteriorWheelMat" 135 | boundingObject USE lunzi 136 | physics Physics { 137 | } 138 | } 139 | } 140 | HingeJoint { 141 | jointParameters HingeJointParameters { 142 | position 1.2612946286005862e-05 143 | axis 0 1 0 144 | anchor 0.1 0.15 0 145 | } 146 | device [ 147 | DEF motor2 RotationalMotor { 148 | name "motor2" 149 | maxVelocity 100 150 | } 151 | ] 152 | endPoint Solid { 153 | translation 0.1 0.13 0 154 | rotation 0 1 0 1.261293295174105e-05 155 | children [ 156 | Shape { 157 | appearance Appearance { 158 | material Material { 159 | } 160 | texture ImageTexture { 161 | } 162 | } 163 | geometry DEF lunzi Cylinder { 164 | height 0.05 165 | radius 0.06 166 | } 167 | } 168 | ] 169 | name "solid(1)" 170 | contactMaterial "ExteriorWheelMat" 171 | boundingObject USE lunzi 172 | physics Physics { 173 | } 174 | } 175 | } 176 | HingeJoint { 177 | jointParameters HingeJointParameters { 178 | position 6.005694924050056e-08 179 | axis 0 1 0 180 | anchor -0.1 0.15 0 181 | } 182 | device [ 183 | DEF motor3 RotationalMotor { 184 | name "motor3" 185 | maxVelocity 100 186 | } 187 | ] 188 | endPoint Solid { 189 | translation -0.1 0.13 -1.514618646954335e-23 190 | rotation 0 1 0 5.960464477539063e-08 191 | children [ 192 | Shape { 193 | appearance Appearance { 194 | material Material { 195 | } 196 | texture ImageTexture { 197 | } 198 | } 199 | geometry DEF lunzi Cylinder { 200 | height 0.05 201 | radius 0.06 202 | } 203 | } 204 | ] 205 | name "solid(2)" 206 | contactMaterial "InteriorWheelMat" 207 | boundingObject USE lunzi 208 | physics Physics { 209 | } 210 | } 211 | } 212 | HingeJoint { 213 | jointParameters HingeJointParameters { 214 | position 4.932489063156657e-06 215 | axis 0 1 0 216 | anchor -0.1 -0.15 0 217 | } 218 | device [ 219 | DEF motor4 RotationalMotor { 220 | name "motor4" 221 | maxVelocity 100 222 | } 223 | ] 224 | endPoint Solid { 225 | translation -0.1 -0.13 1.5284732060091868e-21 226 | rotation 0 1 0 4.932531951326607e-06 227 | children [ 228 | Shape { 229 | appearance Appearance { 230 | material Material { 231 | } 232 | } 233 | geometry DEF lunzi Cylinder { 234 | height 0.05 235 | radius 0.06 236 | } 237 | } 238 | ] 239 | name "solid(3)" 240 | contactMaterial "ExteriorWheelMat" 241 | boundingObject USE lunzi 242 | physics Physics { 243 | } 244 | } 245 | } 246 | DEF Body Shape { 247 | appearance PBRAppearance { 248 | baseColor 0 0 0 249 | } 250 | geometry Box { 251 | size 0.3 0.2 0.08 252 | } 253 | } 254 | ] 255 | name "car" 256 | contactMaterial "InteriorWheelMat" 257 | boundingObject USE Body 258 | physics Physics { 259 | } 260 | controller "wasd" 261 | } 262 | -------------------------------------------------------------------------------- /Final_project/worlds/world2.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2021a utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material1 "InteriorWheelMat" 7 | coulombFriction [ 8 | 1.8, 0, 0.2 9 | ] 10 | frictionRotation -0.965 0 11 | bounce 0 12 | forceDependentSlip [ 13 | 10, 0 14 | ] 15 | } 16 | ContactProperties { 17 | material1 "ExteriorWheelMat" 18 | coulombFriction [ 19 | 1.8, 0, 0.2 20 | ] 21 | frictionRotation 0.965 0 22 | bounce 0 23 | forceDependentSlip [ 24 | 10, 0 25 | ] 26 | } 27 | ] 28 | } 29 | Viewpoint { 30 | orientation 1 1.204484742155182e-15 3.130575906041404e-14 0.06999999999999268 31 | position -2.445318478554593 -1.7574508464259089 24.256929703649604 32 | followType "None" 33 | } 34 | TexturedBackground { 35 | } 36 | TexturedBackgroundLight { 37 | } 38 | RectangleArena { 39 | rotation 1 0 0 1.57 40 | floorSize 4.5 6 41 | wallHeight 0.5 42 | } 43 | Wall { 44 | translation 1.75 1.5 0 45 | rotation 1 0 0 1.5708 46 | size 1 0.5 0.1 47 | } 48 | Wall { 49 | translation 0 1.5 0 50 | rotation 1 0 0 1.5708 51 | name "wall(1)" 52 | size 1.5 0.5 0.1 53 | } 54 | Wall { 55 | translation 0.75 2 0 56 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 57 | name "wall(2)" 58 | size 1 0.5 0.1 59 | } 60 | Wall { 61 | translation -1.75 1.5 0 62 | rotation 1 0 0 1.5708 63 | name "wall(3)" 64 | size 1 0.5 0.1 65 | } 66 | Wall { 67 | translation -1.25 2 0 68 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 69 | name "wall(4)" 70 | size 1 0.5 0.1 71 | } 72 | Wall { 73 | translation 0 2.5 0 74 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 75 | name "wall(5)" 76 | size 1 0.5 0.1 77 | } 78 | Wall { 79 | translation 1.25 0 0 80 | rotation 1 0 0 1.5708 81 | name "wall(6)" 82 | size 2 0.5 0.1 83 | } 84 | Wall { 85 | translation -0.75 0.75 0 86 | rotation 1 0 0 1.5708 87 | name "wall(7)" 88 | size 2 0.5 0.1 89 | } 90 | Wall { 91 | translation 0.25 -0.25 0 92 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 93 | name "wall(8)" 94 | size 2 0.5 0.1 95 | } 96 | Wall { 97 | translation -1.75 -0.25 0 98 | rotation 1 0 0 1.5708 99 | name "wall(9)" 100 | size 1 0.5 0.1 101 | } 102 | Wall { 103 | translation 1.25 -0.75 0 104 | rotation 1 0 0 1.5708 105 | name "wall(10)" 106 | size 1 0.5 0.1 107 | } 108 | Wall { 109 | translation -0.5 -1.25 0 110 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 111 | name "wall(11)" 112 | size 2 0.5 0.1 113 | } 114 | Wall { 115 | translation 1.775 -1.5 0 116 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 117 | name "wall(12)" 118 | size 1.5 0.5 0.1 119 | } 120 | Wall { 121 | translation 1 -1.75 0 122 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 123 | name "wall(13)" 124 | size 1 0.5 0.1 125 | } 126 | Wall { 127 | translation 0.225 -2.25 0 128 | rotation 1 0 0 1.5708 129 | name "wall(14)" 130 | size 1.5 0.5 0.1 131 | } 132 | Wall { 133 | translation -1 -1 0 134 | rotation 1 0 0 1.5708 135 | name "wall(15)" 136 | size 1 0.5 0.1 137 | } 138 | Wall { 139 | translation -1.5 -1.5 0 140 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 141 | name "wall(16)" 142 | size 1 0.5 0.1 143 | } 144 | DEF car Robot { 145 | translation 1.95036 2.49019 0.0566201 146 | rotation -2.1949434369512534e-08 0.0004000847233330271 0.9999999199661036 3.141582349006734 147 | scale 0.7 0.7 0.7 148 | children [ 149 | Display { 150 | width 100 151 | height 100 152 | } 153 | InertialUnit { 154 | name "yaw" 155 | model "yaw" 156 | } 157 | DEF gps GPS { 158 | } 159 | Lidar { 160 | translation 0 0 0.075 161 | rotation 1 0 0 1.5708 162 | horizontalResolution 256 163 | numberOfLayers 1 164 | spherical FALSE 165 | type "rotating" 166 | resolution 0.001 167 | rotatingHead Solid { 168 | rotation 0 -1 0 2.9802322387695313e-08 169 | children [ 170 | Transform { 171 | translation -0.055 0 0 172 | children [ 173 | Shape { 174 | appearance PBRAppearance { 175 | } 176 | geometry Sphere { 177 | radius 0.03 178 | } 179 | } 180 | ] 181 | } 182 | Transform { 183 | translation 0.055 0 0 184 | children [ 185 | Shape { 186 | appearance PBRAppearance { 187 | } 188 | geometry Sphere { 189 | radius 0.03 190 | } 191 | } 192 | ] 193 | } 194 | ] 195 | } 196 | } 197 | HingeJoint { 198 | jointParameters HingeJointParameters { 199 | position 1.0805429119930097e-06 200 | axis 0 1 0 201 | anchor 0.1 -0.15 0 202 | } 203 | device [ 204 | DEF motor1 RotationalMotor { 205 | name "motor1" 206 | maxVelocity 100 207 | } 208 | ] 209 | endPoint Solid { 210 | translation 0.1 -0.13 0 211 | rotation 0 1 0 1.0807194927018264e-06 212 | children [ 213 | Shape { 214 | appearance Appearance { 215 | material Material { 216 | } 217 | } 218 | geometry DEF lunzi Cylinder { 219 | height 0.05 220 | radius 0.06 221 | } 222 | } 223 | ] 224 | contactMaterial "InteriorWheelMat" 225 | boundingObject USE lunzi 226 | physics Physics { 227 | } 228 | } 229 | } 230 | HingeJoint { 231 | jointParameters HingeJointParameters { 232 | position 1.2612946286005862e-05 233 | axis 0 1 0 234 | anchor 0.1 0.15 0 235 | } 236 | device [ 237 | DEF motor2 RotationalMotor { 238 | name "motor2" 239 | maxVelocity 100 240 | } 241 | ] 242 | endPoint Solid { 243 | translation 0.1 0.13 0 244 | rotation 0 1 0 1.261293295174105e-05 245 | children [ 246 | Shape { 247 | appearance Appearance { 248 | material Material { 249 | } 250 | texture ImageTexture { 251 | } 252 | } 253 | geometry DEF lunzi Cylinder { 254 | height 0.05 255 | radius 0.06 256 | } 257 | } 258 | ] 259 | name "solid(1)" 260 | contactMaterial "ExteriorWheelMat" 261 | boundingObject USE lunzi 262 | physics Physics { 263 | } 264 | } 265 | } 266 | HingeJoint { 267 | jointParameters HingeJointParameters { 268 | position 6.005694924050056e-08 269 | axis 0 1 0 270 | anchor -0.1 0.15 0 271 | } 272 | device [ 273 | DEF motor3 RotationalMotor { 274 | name "motor3" 275 | maxVelocity 100 276 | } 277 | ] 278 | endPoint Solid { 279 | translation -0.1 0.13 -1.6253561046853066e-27 280 | rotation 0 1 0 5.960464477539063e-08 281 | children [ 282 | Shape { 283 | appearance Appearance { 284 | material Material { 285 | } 286 | texture ImageTexture { 287 | } 288 | } 289 | geometry DEF lunzi Cylinder { 290 | height 0.05 291 | radius 0.06 292 | } 293 | } 294 | ] 295 | name "solid(2)" 296 | contactMaterial "InteriorWheelMat" 297 | boundingObject USE lunzi 298 | physics Physics { 299 | } 300 | } 301 | } 302 | HingeJoint { 303 | jointParameters HingeJointParameters { 304 | position 4.932489063156657e-06 305 | axis 0 1 0 306 | anchor -0.1 -0.15 0 307 | } 308 | device [ 309 | DEF motor4 RotationalMotor { 310 | name "motor4" 311 | maxVelocity 100 312 | } 313 | ] 314 | endPoint Solid { 315 | translation -0.1 -0.13 5.430842521902593e-30 316 | rotation 0 1 0 4.932531951326607e-06 317 | children [ 318 | Shape { 319 | appearance Appearance { 320 | material Material { 321 | } 322 | } 323 | geometry DEF lunzi Cylinder { 324 | height 0.05 325 | radius 0.06 326 | } 327 | } 328 | ] 329 | name "solid(3)" 330 | contactMaterial "ExteriorWheelMat" 331 | boundingObject USE lunzi 332 | physics Physics { 333 | } 334 | } 335 | } 336 | DEF Body Shape { 337 | appearance PBRAppearance { 338 | baseColor 0 0 0 339 | } 340 | geometry Box { 341 | size 0.3 0.2 0.08 342 | } 343 | } 344 | ] 345 | name "car" 346 | contactMaterial "InteriorWheelMat" 347 | boundingObject USE Body 348 | physics Physics { 349 | } 350 | controller "wasd" 351 | } 352 | -------------------------------------------------------------------------------- /Final_project/worlds/world3.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2021a utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material1 "ExteriorWheelMat" 7 | coulombFriction [ 8 | 1.8, 0, 0.2 9 | ] 10 | frictionRotation 0.965 0 11 | bounce 0 12 | forceDependentSlip [ 13 | 10, 0 14 | ] 15 | } 16 | ContactProperties { 17 | material1 "InteriorWheelMat" 18 | coulombFriction [ 19 | 1.8, 0, 0.2 20 | ] 21 | frictionRotation -0.965 0 22 | bounce 0 23 | forceDependentSlip [ 24 | 10, 0 25 | ] 26 | } 27 | ] 28 | } 29 | Viewpoint { 30 | orientation 0 1 0 0 31 | position -0.4066433063003011 -2.311940200781253e-07 33.40132387215836 32 | } 33 | TexturedBackground { 34 | } 35 | TexturedBackgroundLight { 36 | } 37 | RectangleArena { 38 | rotation 1 0 0 1.5708 39 | floorSize 10 10 40 | } 41 | DEF Supervisor Robot { 42 | controller "openDoor" 43 | supervisor TRUE 44 | } 45 | DEF DOOR1 Door { 46 | translation -2.5 -4.25 0 47 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 48 | name "door1" 49 | size 1.5 2.4 0.2 50 | } 51 | DEF DOOR2 Door { 52 | translation -1.125 2.75 0 53 | rotation 1 0 0 1.5708 54 | name "door2" 55 | size 1.25 2.4 0.2 56 | } 57 | Wall { 58 | translation -4 -2.75 1.23289e-06 59 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 60 | size 2.5 2 0.1 61 | } 62 | Wall { 63 | translation -3.25 -1.5 0 64 | rotation 1 0 0 1.5708 65 | name "wall(1)" 66 | size 1.5 2 0.1 67 | } 68 | Wall { 69 | translation -2.5 -2.5 0 70 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 71 | name "wall(2)" 72 | size 2 2 0.1 73 | } 74 | Wall { 75 | translation -1 -3 0 76 | rotation 1 0 0 1.5708 77 | name "wall(4)" 78 | size 3 2 0.1 79 | } 80 | Wall { 81 | translation 0 -1.5 0 82 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 83 | name "wall(7)" 84 | size 3 2 0.1 85 | } 86 | Wall { 87 | translation 2 3.75 0 88 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 89 | name "wall(8)" 90 | size 2 2 0.1 91 | } 92 | Wall { 93 | translation 4 2.75 0 94 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 95 | name "wall(9)" 96 | size 2 2 0.1 97 | } 98 | Wall { 99 | translation 0.5 2.75 0 100 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 101 | name "wall(11)" 102 | size 2 2 0.1 103 | } 104 | Wall { 105 | translation 3 3.25 0 106 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 107 | name "wall(12)" 108 | size 1 2 0.1 109 | } 110 | Wall { 111 | translation 2.5 1.25 0 112 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 113 | name "wall(13)" 114 | size 2 2 0.1 115 | } 116 | Wall { 117 | translation 1.5 -0.5 0 118 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 119 | name "wall(14)" 120 | size 6.5 2 0.1 121 | } 122 | Wall { 123 | translation 2.5 -3.75 0 124 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 125 | name "wall(15)" 126 | size 2 2 0.1 127 | } 128 | Wall { 129 | translation 3.5 -2.75 0 130 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 131 | name "wall(16)" 132 | size 2 2 0.1 133 | } 134 | Wall { 135 | translation 4 -0.25 0 136 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 137 | name "wall(21)" 138 | size 2 2 0.1 139 | } 140 | Wall { 141 | translation -2.25 0.5 0 142 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 143 | name "wall(22)" 144 | size 3.5 2 0.1 145 | } 146 | Wall { 147 | translation -1.75 3.5 0 148 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 149 | name "wall(23)" 150 | size 3 2 0.1 151 | } 152 | Wall { 153 | translation -2.25 2 0 154 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 155 | name "wall(25)" 156 | size 1 2 0.1 157 | } 158 | Wall { 159 | translation -4 2.75 0 160 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 161 | name "wall(26)" 162 | size 2 2 0.1 163 | } 164 | Wall { 165 | translation -4 3.75 0 166 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 167 | name "wall(27)" 168 | size 2 2 0.1 169 | } 170 | Wall { 171 | translation -4 0 0 172 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 173 | name "wall(29)" 174 | size 1 2 0.1 175 | } 176 | DEF car Robot { 177 | translation 4.12036 4.47019 0.055036 178 | rotation -2.1949434369512534e-08 0.0004000847233330271 0.9999999199661036 3.141582349006734 179 | children [ 180 | Display { 181 | width 100 182 | height 100 183 | } 184 | InertialUnit { 185 | name "yaw" 186 | model "yaw" 187 | } 188 | DEF gps GPS { 189 | } 190 | Lidar { 191 | translation 0 0 0.075 192 | rotation 1 0 0 1.5708 193 | horizontalResolution 256 194 | numberOfLayers 1 195 | spherical FALSE 196 | type "rotating" 197 | resolution 0.001 198 | rotatingHead Solid { 199 | rotation 0 -1 0 2.9802322387695313e-08 200 | children [ 201 | Transform { 202 | translation -0.055 0 0 203 | children [ 204 | Shape { 205 | appearance PBRAppearance { 206 | } 207 | geometry Sphere { 208 | radius 0.03 209 | } 210 | } 211 | ] 212 | } 213 | Transform { 214 | translation 0.055 0 0 215 | children [ 216 | Shape { 217 | appearance PBRAppearance { 218 | } 219 | geometry Sphere { 220 | radius 0.03 221 | } 222 | } 223 | ] 224 | } 225 | ] 226 | } 227 | } 228 | HingeJoint { 229 | jointParameters HingeJointParameters { 230 | position 1.0805429119930097e-06 231 | axis 0 1 0 232 | anchor 0.1 -0.15 0 233 | } 234 | device [ 235 | DEF motor1 RotationalMotor { 236 | name "motor1" 237 | maxVelocity 100 238 | } 239 | ] 240 | endPoint Solid { 241 | translation 0.1 -0.13 0 242 | rotation 0 1 0 1.0807194927018264e-06 243 | children [ 244 | Shape { 245 | appearance Appearance { 246 | material Material { 247 | } 248 | } 249 | geometry DEF lunzi Cylinder { 250 | height 0.05 251 | radius 0.06 252 | } 253 | } 254 | ] 255 | contactMaterial "InteriorWheelMat" 256 | boundingObject USE lunzi 257 | physics Physics { 258 | } 259 | } 260 | } 261 | HingeJoint { 262 | jointParameters HingeJointParameters { 263 | position 1.2612946286005862e-05 264 | axis 0 1 0 265 | anchor 0.1 0.15 0 266 | } 267 | device [ 268 | DEF motor2 RotationalMotor { 269 | name "motor2" 270 | maxVelocity 100 271 | } 272 | ] 273 | endPoint Solid { 274 | translation 0.1 0.13 0 275 | rotation 0 1 0 1.261293295174105e-05 276 | children [ 277 | Shape { 278 | appearance Appearance { 279 | material Material { 280 | } 281 | texture ImageTexture { 282 | } 283 | } 284 | geometry DEF lunzi Cylinder { 285 | height 0.05 286 | radius 0.06 287 | } 288 | } 289 | ] 290 | name "solid(1)" 291 | contactMaterial "ExteriorWheelMat" 292 | boundingObject USE lunzi 293 | physics Physics { 294 | } 295 | } 296 | } 297 | HingeJoint { 298 | jointParameters HingeJointParameters { 299 | position 6.005694924050056e-08 300 | axis 0 1 0 301 | anchor -0.1 0.15 0 302 | } 303 | device [ 304 | DEF motor3 RotationalMotor { 305 | name "motor3" 306 | maxVelocity 100 307 | } 308 | ] 309 | endPoint Solid { 310 | translation -0.1 0.13 -8.519644008279305e-67 311 | rotation 0 1 0 5.960464477539063e-08 312 | children [ 313 | Shape { 314 | appearance Appearance { 315 | material Material { 316 | } 317 | texture ImageTexture { 318 | } 319 | } 320 | geometry DEF lunzi Cylinder { 321 | height 0.05 322 | radius 0.06 323 | } 324 | } 325 | ] 326 | name "solid(2)" 327 | contactMaterial "InteriorWheelMat" 328 | boundingObject USE lunzi 329 | physics Physics { 330 | } 331 | } 332 | } 333 | HingeJoint { 334 | jointParameters HingeJointParameters { 335 | position 4.932489063156657e-06 336 | axis 0 1 0 337 | anchor -0.1 -0.15 0 338 | } 339 | device [ 340 | DEF motor4 RotationalMotor { 341 | name "motor4" 342 | maxVelocity 100 343 | } 344 | ] 345 | endPoint Solid { 346 | translation -0.1 -0.13 4.691074945814483e-87 347 | rotation 0 1 0 4.932531951326607e-06 348 | children [ 349 | Shape { 350 | appearance Appearance { 351 | material Material { 352 | } 353 | } 354 | geometry DEF lunzi Cylinder { 355 | height 0.05 356 | radius 0.06 357 | } 358 | } 359 | ] 360 | name "solid(3)" 361 | contactMaterial "ExteriorWheelMat" 362 | boundingObject USE lunzi 363 | physics Physics { 364 | } 365 | } 366 | } 367 | DEF Body Shape { 368 | appearance PBRAppearance { 369 | baseColor 0 0 0 370 | } 371 | geometry Box { 372 | size 0.3 0.2 0.08 373 | } 374 | } 375 | ] 376 | name "car" 377 | contactMaterial "InteriorWheelMat" 378 | boundingObject USE Body 379 | physics Physics { 380 | } 381 | controller "wasd" 382 | } 383 | -------------------------------------------------------------------------------- /Final_project/worlds/world4.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2021a utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material1 "InteriorWheelMat" 7 | coulombFriction [ 8 | 1.8, 0, 0.2 9 | ] 10 | frictionRotation -0.965 0 11 | bounce 0 12 | forceDependentSlip [ 13 | 10, 0 14 | ] 15 | } 16 | ContactProperties { 17 | material1 "ExteriorWheelMat" 18 | coulombFriction [ 19 | 1.8, 0, 0.2 20 | ] 21 | frictionRotation 0.965 0 22 | bounce 0 23 | forceDependentSlip [ 24 | 10, 0 25 | ] 26 | } 27 | ] 28 | } 29 | Viewpoint { 30 | orientation 0 1 0 0 31 | position 0.40121033631550307 -0.1727665534027727 40.740026299049354 32 | } 33 | TexturedBackground { 34 | } 35 | TexturedBackgroundLight { 36 | } 37 | RectangleArena { 38 | rotation 1 0 0 1.5708 39 | floorSize 10 10 40 | floorTileSize 1 1 41 | } 42 | Wall { 43 | translation -1 -4 0 44 | rotation 1 0 0 1.5708 45 | size 4 2 0.1 46 | } 47 | Wall { 48 | translation -4 -1.5 0 49 | rotation 1 0 0 1.5708 50 | name "wall(1)" 51 | size 2 2 0.1 52 | } 53 | Wall { 54 | translation -3 -4.5 0 55 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 56 | name "wall(2)" 57 | size 1 2 0.1 58 | } 59 | Wall { 60 | translation 3.25 0.5 0 61 | rotation 1 0 0 1.5708 62 | name "wall(4)" 63 | size 1.5 2 0.1 64 | } 65 | Wall { 66 | translation 3 3.5 0 67 | rotation 1 0 0 1.5708 68 | name "wall(8)" 69 | size 4 2 0.1 70 | } 71 | Wall { 72 | translation 3.25 2 0 73 | rotation 1 0 0 1.5708 74 | name "wall(11)" 75 | size 1.5 2 0.1 76 | } 77 | Wall { 78 | translation 4 1.25 0 79 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 80 | name "wall(12)" 81 | size 1.5 2 0.1 82 | } 83 | Wall { 84 | translation -3 -2 0 85 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 86 | name "wall(6)" 87 | size 1 2 0.1 88 | } 89 | Wall { 90 | translation -0.499992 2.03 -1.11441e-05 91 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 92 | name "wall(13)" 93 | size 1 2 0.1 94 | } 95 | Wall { 96 | translation 1.07 0.99 -1.33174e-06 97 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 98 | name "wall(14)" 99 | size 5 2 0.1 100 | } 101 | Wall { 102 | translation -0.25 -2.5 0 103 | rotation 1 0 0 1.5708 104 | name "wall(15)" 105 | size 5.5 2 0.1 106 | } 107 | Wall { 108 | translation 2.5 -1.5 0 109 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 110 | name "wall(16)" 111 | size 2 2 0.1 112 | } 113 | Wall { 114 | translation 3.75 -3 0 115 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 116 | name "wall(3)" 117 | size 4 2 0.1 118 | } 119 | Wall { 120 | translation -1 -1.5 0 121 | rotation 1 0 0 1.5708 122 | name "wall(5)" 123 | size 2 2 0.1 124 | } 125 | Wall { 126 | translation -3.5 3 0 127 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 128 | name "wall(21)" 129 | size 1 2 0.1 130 | } 131 | Wall { 132 | translation -2 -0.5 0 133 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 134 | name "wall(22)" 135 | size 3 2 0.1 136 | } 137 | Wall { 138 | translation -2 2 0 139 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 140 | name "wall(23)" 141 | size 3 2 0.1 142 | } 143 | Wall { 144 | translation -0.5 1 0 145 | rotation 1.6952594753969508e-09 0.707104781184338 0.7071087811831002 -3.1415853071795863 146 | name "wall(25)" 147 | size 1 2 0.1 148 | } 149 | Wall { 150 | translation -0.5 3.5 0 151 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 152 | name "wall(26)" 153 | size 3 2 0.1 154 | } 155 | Wall { 156 | translation -3.5 3.5 0 157 | rotation 1 0 0 1.5708 158 | name "wall(27)" 159 | size 3 2 0.1 160 | } 161 | Wall { 162 | translation -3.5 0.5 0 163 | rotation 0.5773509358554485 0.5773489358556708 0.5773509358554485 2.0944 164 | name "wall(29)" 165 | size 2 2 0.1 166 | } 167 | DEF car Robot { 168 | translation 4.12036 4.47019 0.055036 169 | rotation -2.1949434369512534e-08 0.0004000847233330271 0.9999999199661036 3.141582349006734 170 | children [ 171 | Display { 172 | width 100 173 | height 100 174 | } 175 | InertialUnit { 176 | name "yaw" 177 | model "yaw" 178 | } 179 | DEF gps GPS { 180 | } 181 | Lidar { 182 | translation 0 0 0.075 183 | rotation 1 0 0 1.5708 184 | horizontalResolution 256 185 | numberOfLayers 1 186 | spherical FALSE 187 | type "rotating" 188 | resolution 0.001 189 | rotatingHead Solid { 190 | rotation 0 -1 0 2.9802322387695313e-08 191 | children [ 192 | Transform { 193 | translation -0.055 0 0 194 | children [ 195 | Shape { 196 | appearance PBRAppearance { 197 | } 198 | geometry Sphere { 199 | radius 0.03 200 | } 201 | } 202 | ] 203 | } 204 | Transform { 205 | translation 0.055 0 0 206 | children [ 207 | Shape { 208 | appearance PBRAppearance { 209 | } 210 | geometry Sphere { 211 | radius 0.03 212 | } 213 | } 214 | ] 215 | } 216 | ] 217 | } 218 | } 219 | HingeJoint { 220 | jointParameters HingeJointParameters { 221 | position 1.0805429119930097e-06 222 | axis 0 1 0 223 | anchor 0.1 -0.15 0 224 | } 225 | device [ 226 | DEF motor1 RotationalMotor { 227 | name "motor1" 228 | maxVelocity 100 229 | } 230 | ] 231 | endPoint Solid { 232 | translation 0.1 -0.13 0 233 | rotation 0 1 0 1.0807194927018264e-06 234 | children [ 235 | Shape { 236 | appearance Appearance { 237 | material Material { 238 | } 239 | } 240 | geometry DEF lunzi Cylinder { 241 | height 0.05 242 | radius 0.06 243 | } 244 | } 245 | ] 246 | contactMaterial "InteriorWheelMat" 247 | boundingObject USE lunzi 248 | physics Physics { 249 | } 250 | } 251 | } 252 | HingeJoint { 253 | jointParameters HingeJointParameters { 254 | position 1.2612946286005862e-05 255 | axis 0 1 0 256 | anchor 0.1 0.15 0 257 | } 258 | device [ 259 | DEF motor2 RotationalMotor { 260 | name "motor2" 261 | maxVelocity 100 262 | } 263 | ] 264 | endPoint Solid { 265 | translation 0.1 0.13 0 266 | rotation 0 1 0 1.261293295174105e-05 267 | children [ 268 | Shape { 269 | appearance Appearance { 270 | material Material { 271 | } 272 | texture ImageTexture { 273 | } 274 | } 275 | geometry DEF lunzi Cylinder { 276 | height 0.05 277 | radius 0.06 278 | } 279 | } 280 | ] 281 | name "solid(1)" 282 | contactMaterial "ExteriorWheelMat" 283 | boundingObject USE lunzi 284 | physics Physics { 285 | } 286 | } 287 | } 288 | HingeJoint { 289 | jointParameters HingeJointParameters { 290 | position 6.005694924050056e-08 291 | axis 0 1 0 292 | anchor -0.1 0.15 0 293 | } 294 | device [ 295 | DEF motor3 RotationalMotor { 296 | name "motor3" 297 | maxVelocity 100 298 | } 299 | ] 300 | endPoint Solid { 301 | translation -0.1 0.13 -8.519644008279305e-67 302 | rotation 0 1 0 5.960464477539063e-08 303 | children [ 304 | Shape { 305 | appearance Appearance { 306 | material Material { 307 | } 308 | texture ImageTexture { 309 | } 310 | } 311 | geometry DEF lunzi Cylinder { 312 | height 0.05 313 | radius 0.06 314 | } 315 | } 316 | ] 317 | name "solid(2)" 318 | contactMaterial "InteriorWheelMat" 319 | boundingObject USE lunzi 320 | physics Physics { 321 | } 322 | } 323 | } 324 | HingeJoint { 325 | jointParameters HingeJointParameters { 326 | position 4.932489063156657e-06 327 | axis 0 1 0 328 | anchor -0.1 -0.15 0 329 | } 330 | device [ 331 | DEF motor4 RotationalMotor { 332 | name "motor4" 333 | maxVelocity 100 334 | } 335 | ] 336 | endPoint Solid { 337 | translation -0.1 -0.13 4.691074945814483e-87 338 | rotation 0 1 0 4.932531951326607e-06 339 | children [ 340 | Shape { 341 | appearance Appearance { 342 | material Material { 343 | } 344 | } 345 | geometry DEF lunzi Cylinder { 346 | height 0.05 347 | radius 0.06 348 | } 349 | } 350 | ] 351 | name "solid(3)" 352 | contactMaterial "ExteriorWheelMat" 353 | boundingObject USE lunzi 354 | physics Physics { 355 | } 356 | } 357 | } 358 | DEF Body Shape { 359 | appearance PBRAppearance { 360 | baseColor 0 0 0 361 | } 362 | geometry Box { 363 | size 0.3 0.2 0.08 364 | } 365 | } 366 | ] 367 | name "car" 368 | contactMaterial "InteriorWheelMat" 369 | boundingObject USE Body 370 | physics Physics { 371 | } 372 | controller "wasd" 373 | } 374 | -------------------------------------------------------------------------------- /PRM_planning/Readme.md: -------------------------------------------------------------------------------- 1 | # Webots PRM路径规划 2 | 3 | 演示视频：[bilibili](https://www.bilibili.com/video/BV1pL4y1s7MV) 4 | 5 | 实验报告：[Webots——PRM算法](https://blog.lanly.vip/article/8) 6 | 7 | 实验平台：Webots 2021b 8 | 9 | 注：2022a版对坐标系进行了改变，详情点击[此处](https://github.com/cyberbotics/webots/wiki/How-to-adapt-your-world-or-PROTO-to-Webots-R2022a)，故请使用此版本之前的平台，比如2021b。当然你也可以使用该网站提到的的[脚本](https://github.com/cyberbotics/webots/blob/master/scripts/converter/convert_nue_to_enu_rub_to_flu.py)对世界文件进行坐标变换 10 | ```bash 11 | . 12 | ├── controllers 13 | │   ├── Move // 运动规划控制器 14 | │   │   ├── Makefile // 开了C++17 15 | │   │   ├── Move.cpp // 控制器代码 16 | │   │   ├── example.png // 规划路径一的图 17 | │   │   ├── example.txt // 保存了规划路径一的数据 18 | │   │   ├── main.cpp // 二合一代码，可能在其他电脑能编译 19 | │   │   ├── maze.png // 原图 20 | │   │   ├── nice.png // 规划路径二的图 21 | │   │   ├── nice.txt // 保存了规划路径二的数据 22 | │   │   ├── qwq.png // 规划路径三的图 23 | │   │   ├── qwq.txt // 保存了规划路径三的数据 24 | │   │   ├── test.cpp // 源代码，能键盘控制小车上下左右移动 25 | │   │   ├── tu.cpp // PRM算法主程序 26 | │   │   └── tu.h // 实现PRM算法的类 27 | │   └── display 28 | │   ├── Makefile // 开了C++17 29 | │   └── display.cpp // 可视化控制器代码 30 | ├── libraries 31 | ├── maze.png 32 | ├── plugins 33 | ├── protos 34 | └── worlds // 世界文件 35 | ``` 36 | 37 | 双击```world/empty.wbt```打开世界。 38 | 39 | ### 编译 40 | 编译 ```tu.cpp``` 41 | ```bash 42 | g++ tu.cpp `pkg-config --cflags --libs opencv` -std=c++17 -g -o tu # 部分电脑可能是opencv4 43 | ./tu 44 | ``` 45 | 46 | 如非 ```bash```环境或未安装```pkg-config```或未对opencv配置，请自行更改为将``` `pkg-config --cflags --libs opencv` ```更改为链接本机```OpenCV```库的参数指令。 47 | 48 | 如在Mac下``` `pkg-config --cflags --libs opencv` ```的运行结果为 49 | ```bash 50 | -I/opt/homebrew/opt/opencv/include/opencv4 -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 51 | ``` 52 | 53 | 通过修改```tu.h```文件下的 ```REC, neiDis, disX, disY```变量可以得到不同的路径图。部分情况可能求解时间超过1s，还请耐心等待。 54 | 55 | **限于系统原因，OpenCV安装时所编译的动态链接库为Arm架构，而Webots暂未对MacOS的M1类型适配，其动态链接库均为x86_64架构。这将导致Webots在生成项目时，在链接动态链接库阶段出现找不到OpenCV相关成员变量的x86_64的动态库，而在手动用命令行编译时，亦出现找不到Webots相关成员变量的Arm的动态库。因此这里将这两个代码分开，即路径规划和巡线代码为不同文件，其中路径规划代码引用了OpenCV库，在命令行编译下能够正常编译成功运行，巡线代码在Webots下亦能正常编译成功运行。而理论上它们在Linux或Windows应能融洽在同一个代码中。** 56 | 57 | ### 控制器代码 58 | 59 | 务必开启 ```c++17```标准。 60 | 61 | 通过修改109行的```Path maze("./example.txt");```中的文件名来实现不同的路径寻路。 62 | 63 | 同时也要相应的修改 ```display.cpp```文件的118行的```Path maze("../Move/example.txt");```于相应的文件。 64 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | # CXX_SOURCES = Move.cpp ground.cpp 43 | # CXX_SOURCES = main.cpp 44 | ### 45 | ### ---- Compilation options ---- 46 | ### if special compilation flags are necessary: 47 | CFLAGS = -std=c++17 48 | ### 49 | ### ---- Linked libraries ---- 50 | ### if your program needs additional libraries: 51 | # INCLUDE = -I/opt/homebrew/opt/opencv/include/opencv4 52 | # LIBRARIES = -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 53 | ### 54 | ### ---- Linking options ---- 55 | ### if special linking flags are needed: 56 | ### LFLAGS = -s 57 | ### 58 | ### ---- Webots included libraries ---- 59 | ### if you want to use the Webots C API in your C++ controller program: 60 | ### USE_C_API = true 61 | ### 62 | ### ---- Debug mode ---- 63 | ### if you want to display the gcc command line for compilation and link, as 64 | ### well as the rm command details used for cleaning: 65 | VERBOSE = 1 66 | ### 67 | ###----------------------------------------------------------------------------- 68 | 69 | ### Do not modify: this includes Webots global Makefile.include 70 | null := 71 | space := $(null) $(null) 72 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 73 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 74 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/Move.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | const int lookDis = 20; 17 | const double k = 1; 18 | const double velocity = 40; 19 | const double velocity2 = 30; 20 | 21 | inline pair real2pic(double X, double Y){ // 模拟现实左边转换成像素坐标 22 | return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)}; 23 | } 24 | 25 | inline pair pic2real(int x, int y){ // 像素坐标转换成模拟世界坐标 26 | return {3 - 6 * x * 1.0 / 800, -2.25 + 4.5 * y * 1.0 / 600}; 27 | } 28 | 29 | class Path{ 30 | vector path; // 读取路径 31 | int l; 32 | int cols, rows; 33 | public: 34 | 35 | Path(){} 36 | Path(string s){ 37 | FILE * fp = fopen(s.c_str(), "r"); 38 | int tot = 0; 39 | fscanf(fp, "%d%d%d", &rows, &cols, &tot); 40 | while(tot--){ 41 | int x; 42 | fscanf(fp, "%d", &x); 43 | path.push_back(x); 44 | } 45 | } 46 | 47 | inline int two2one(int x, int y){ // 将二维坐标转换成一维 48 | return x * cols + y; 49 | } 50 | 51 | inline pair one2two(int id){ // 将一维坐标转换成二维 52 | return {id / cols, id % cols}; 53 | } 54 | 55 | inline double calcDis(int x, int y){ // 计算距离 56 | return sqrt(x * x + y * y); 57 | } 58 | 59 | inline double dis(int x1, int y1, int x2, int y2){ // 计算两个二维点坐标距离 60 | return calcDis(x2 - x1, y2 - y1); 61 | } 62 | 63 | inline double dis(int u, int v){ // 计算两个一维点坐标距离 64 | auto [x1, y1] = one2two(u); 65 | auto [x2, y2] = one2two(v); 66 | return calcDis(x2 - x1, y2 - y1); 67 | } 68 | 69 | void clear(){ 70 | curPoint = -1; 71 | } 72 | 73 | void setLookhead(int x){ // 设置lookahead distance 74 | l = x; 75 | } 76 | 77 | int curPoint = -1; 78 | 79 | int findNearestPoint(int x, int y){ // 寻找距离当前位置最近的点 80 | int cc = curPoint; 81 | int cur = two2one(x, y); 82 | double ansdis = 1e9 + 7; 83 | curPoint = 0; 84 | for(size_t i = cc; i < path.size(); ++ i){ 85 | double distance = dis(cur, path[i]); 86 | if (distance < ansdis){ 87 | ansdis = distance; 88 | cc = i; 89 | } 90 | } 91 | // return one2two(path[curPoint]); 92 | return cc; 93 | } 94 | 95 | pair findLookheadPoint(int x, int y){ // 寻找距离当前位置大于lookahead distance的最近的点 96 | int id = findNearestPoint(x, y); 97 | if (id > curPoint) 98 | curPoint = id; 99 | int cur = two2one(x, y); 100 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 101 | ++ curPoint; 102 | } 103 | return one2two(path[curPoint]); 104 | } 105 | }; 106 | 107 | int main() 108 | { 109 | Path maze("./example.txt"); 110 | maze.setLookhead(lookDis); 111 | 112 | // Robot *robot = new Robot(); 113 | Supervisor *robot = new Supervisor(); 114 | 115 | int timeStep = (int)robot->getBasicTimeStep(); 116 | cout << timeStep << endl; 117 | 118 | Keyboard keyboard; 119 | keyboard.enable(1); 120 | 121 | GPS* gps = robot->getGPS("gps"); 122 | gps->enable(1); 123 | 124 | Motor *motors[4]; 125 | DistanceSensor *dismotors[4]; 126 | 127 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 128 | char distanceNames[4][8] = {"front", "back", "left", "right"}; 129 | double speed1[4]; 130 | double speed2[4]; 131 | double speed3[4]; 132 | 133 | 134 | for (int i = 0; i < 4; i++) 135 | { // 初始化电机和距离传感器 136 | motors[i] = robot->getMotor(wheelsNames[i]); 137 | dismotors[i] = robot->getDistanceSensor(distanceNames[i]); 138 | dismotors[i]->enable(1); 139 | motors[i]->setPosition(std::numeric_limits::infinity()); 140 | motors[i]->setVelocity(0.0); 141 | speed1[i] = 0; 142 | speed2[i] = 0; 143 | speed3[i] = 0; 144 | } 145 | 146 | double speed_forward[4] = { velocity ,velocity ,velocity ,velocity }; 147 | double speed_backward[4] = { -velocity ,-velocity ,-velocity ,-velocity }; 148 | double speed_leftward[4] = { velocity ,-velocity ,velocity ,-velocity }; 149 | double speed_rightward[4] = { -velocity ,velocity ,-velocity ,velocity }; 150 | 151 | double speed_leftCircle[4] = { velocity2 ,-velocity2 ,-velocity2 ,velocity2 }; 152 | double speed_rightCircle[4] = { -velocity2 ,velocity2 ,velocity2 ,-velocity2 }; 153 | 154 | for(int i = 0; i < 4; ++ i) 155 | speed1[i] = speed_forward[i]; 156 | 157 | //规定小车的朝向 158 | 159 | double orix = -1; 160 | double oriy = 0; 161 | double oriz = 0; 162 | 163 | while (robot->step(timeStep) != -1) 164 | { 165 | // const double* rotation = field->getSFRotation(); 166 | // cout << field->getSFBool() << endl; 167 | 168 | double X = gps->getValues()[0]; 169 | double Y = gps->getValues()[1]; 170 | double Z = gps->getValues()[2]; 171 | double v = gps->getSpeed(); 172 | // 获取小车坐标以及速度 173 | 174 | double dismo[4]; 175 | for(int i = 0; i < 4; ++ i){ // 获取距离传感器的值 176 | dismo[i] = dismotors[i]->getValue(); 177 | } 178 | 179 | printf("Speed: %lf\n", v); 180 | // 根据速度设置Lookahead distance 181 | maze.setLookhead(min(30.0, v * k + lookDis)); 182 | 183 | // 计算当前坐标在像素坐标系的坐标 184 | auto [x, y] = real2pic(Y, X); 185 | 186 | // 找到目标点 187 | auto [nx, ny] = maze.findLookheadPoint(x, y); 188 | 189 | // 换算成模拟世界坐标 190 | auto [nY, nX] = pic2real(nx, ny); 191 | 192 | // 计算距离 193 | double l = maze.dis(Y, X, nX, nY); 194 | 195 | // 计算以圆上行走的半径 196 | double R = l * l / 2 / abs(X - nX); 197 | // 判断是否直线，直线则加速 198 | double vv = (R > 2 ? 3 : 1); 199 | 200 | 201 | // printf("%lf %lf %lf %lf\n", X, Y, nX, nY); 202 | // 判断当前应该向前向后以及向左向右 203 | int f = nX < X ? 1 : -1; 204 | int r = nY > Y ? 1 : -1; 205 | printf("%s %s\n", f == 1 ? "Forward" : "Back", r == 1 ? "Right" : "Left"); 206 | for(int i = 0; i < 4; ++ i){ // 小车速度 207 | // speed1[i] = f * speed_forward[i] * fabs(nX - X); 208 | // speed2[i] = r * speed_rightward[i] * fabs(nY - Y); 209 | speed1[i] = f * speed_forward[i] * fabs(nX - X) * vv * min(1.0, (f == 1 ? dismo[0] : dismo[1])); 210 | speed2[i] = r * speed_rightward[i] * fabs(nY - Y) * vv * min(1.0, (r == 1 ? dismo[3] : dismo[2])); 211 | } 212 | 213 | // 判断当前小车朝向 214 | const double curx = robot->getSelf()->getOrientation()[0]; 215 | const double cury = robot->getSelf()->getOrientation()[1]; 216 | const double curz = robot->getSelf()->getOrientation()[2]; 217 | printf("%lf %lf %lf\n", curx, cury, curz); 218 | printf("%lf %lf %lf\n", orix, oriy, oriz); 219 | // 如果偏离朝向，则根据偏离方向，设定自旋速度，并降低原先的行进速度 220 | if (abs(curx - orix) > 0.1){ 221 | if (cury < 0){ 222 | printf("LEFT rec\n"); 223 | for(int i = 0; i < 4; ++ i){ 224 | speed1[i] *= 0.01; 225 | speed2[i] *= 0.01; 226 | speed3[i] = speed_leftCircle[i] * abs(curx - orix); 227 | } 228 | }else{ 229 | printf("RIGHT rec\n"); 230 | for(int i = 0; i < 4; ++ i){ 231 | speed1[i] *= 0.01; 232 | speed2[i] *= 0.01; 233 | speed3[i] = speed_rightCircle[i] * abs(curx - orix); 234 | } 235 | } 236 | }else{ 237 | for(int i = 0; i < 4; ++ i) 238 | speed3[i] = 0; 239 | } 240 | 241 | // int keyValue = 0; 242 | // keyValue = keyboard.getKey(); 243 | // if (keyValue == 'W') 244 | // { 245 | // for (int i = 0; i < 4; ++i) 246 | // { 247 | // speed1[i] = speed_forward[i]; 248 | // } 249 | // } 250 | // else if (keyValue == 'S') 251 | // { 252 | // for (int i = 0; i < 4; ++i) 253 | // { 254 | // speed1[i] = speed_backward[i]; 255 | // } 256 | // } 257 | // else if (keyValue == 'A') 258 | // { 259 | // for (int i = 0; i < 4; ++i) 260 | // { 261 | // speed1[i] = speed_leftward[i]; 262 | // } 263 | // } 264 | // else if (keyValue == 'D') 265 | // { 266 | // for (int i = 0; i < 4; ++i) 267 | // { 268 | // speed1[i] = speed_rightward[i]; 269 | // } 270 | // } 271 | // else if (keyValue == 'Q'){ 272 | // for (int i = 0; i < 4; ++ i) 273 | // speed1[i] = speed_leftCircle[i]; 274 | // }else if (keyValue == 'E'){ 275 | // for (int i = 0; i < 4; ++ i) 276 | // speed1[i] = speed_rightCircle[i]; 277 | // }else 278 | // { 279 | // for (int i = 0; i < 4; ++i) 280 | // { 281 | // speed1[i] = 0; 282 | // } 283 | // } 284 | // 285 | for (int i = 0; i < 4; ++i) // 设置速度 286 | { 287 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 288 | } 289 | } 290 | 291 | delete robot; 292 | return 0; 293 | } 294 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/PRM_planning/controllers/Move/example.png -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/example.txt: -------------------------------------------------------------------------------- 1 | 800 600 60 2 | 20343 3 | 19729 4 | 14901 5 | 22074 6 | 29250 7 | 29234 8 | 24409 9 | 27983 10 | 25568 11 | 41764 12 | 59152 13 | 66959 14 | 80147 15 | 96332 16 | 96307 17 | 92075 18 | 94455 19 | 89635 20 | 96202 21 | 108205 22 | 119593 23 | 138191 24 | 157991 25 | 147763 26 | 145353 27 | 144724 28 | 151297 29 | 159675 30 | 156644 31 | 177041 32 | 188440 33 | 204642 34 | 225043 35 | 226875 36 | 231081 37 | 229315 38 | 234739 39 | 235352 40 | 239575 41 | 238397 42 | 247396 43 | 262995 44 | 278599 45 | 293604 46 | 298371 47 | 297751 48 | 300135 49 | 294703 50 | 300674 51 | 312049 52 | 328858 53 | 339047 54 | 357655 55 | 370833 56 | 385829 57 | 403827 58 | 416427 59 | 435622 60 | 454233 61 | 461431 62 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "tu.h" 13 | 14 | using namespace std; 15 | using namespace webots; 16 | 17 | const int lookDis = 20; 18 | const double k = 1; 19 | const double velocity = 40; 20 | const double velocity2 = 30; 21 | 22 | inline pair real2pic(double X, double Y){ 23 | return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)}; 24 | } 25 | 26 | inline pair pic2real(int x, int y){ 27 | return {3 - 6 * x * 1.0 / 800, -2.25 + 4.5 * y * 1.0 / 600}; 28 | } 29 | 30 | int main() 31 | { 32 | tuP maze("./maze.txt"); 33 | maze.setLookhead(lookDis); 34 | 35 | // Robot *robot = new Robot(); 36 | Supervisor *robot = new Supervisor(); 37 | int timeStep = (int)robot->getBasicTimeStep(); 38 | cout << timeStep << endl; 39 | 40 | Keyboard keyboard; 41 | keyboard.enable(1); 42 | 43 | GPS* gps = robot->getGPS("gps"); 44 | gps->enable(1); 45 | 46 | Motor *motors[4]; 47 | DistanceSensor *dismotors[4]; 48 | 49 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 50 | char distanceNames[4][8] = {"front", "back", "left", "right"}; 51 | double speed1[4]; 52 | double speed2[4]; 53 | double speed3[4]; 54 | 55 | 56 | for (int i = 0; i < 4; i++) 57 | { 58 | motors[i] = robot->getMotor(wheelsNames[i]); 59 | dismotors[i] = robot->getDistanceSensor(distanceNames[i]); 60 | dismotors[i]->enable(1); 61 | motors[i]->setPosition(std::numeric_limits::infinity()); 62 | motors[i]->setVelocity(0.0); 63 | speed1[i] = 0; 64 | speed2[i] = 0; 65 | speed3[i] = 0; 66 | } 67 | 68 | double speed_forward[4] = { velocity ,velocity ,velocity ,velocity }; 69 | double speed_backward[4] = { -velocity ,-velocity ,-velocity ,-velocity }; 70 | double speed_leftward[4] = { velocity ,-velocity ,velocity ,-velocity }; 71 | double speed_rightward[4] = { -velocity ,velocity ,-velocity ,velocity }; 72 | 73 | double speed_leftCircle[4] = { velocity2 ,-velocity2 ,-velocity2 ,velocity2 }; 74 | double speed_rightCircle[4] = { -velocity2 ,velocity2 ,velocity2 ,-velocity2 }; 75 | 76 | for(int i = 0; i < 4; ++ i) 77 | speed1[i] = speed_forward[i]; 78 | 79 | double orix = -1; 80 | double oriy = 0; 81 | double oriz = 0; 82 | 83 | while (robot->step(timeStep) != -1) 84 | { 85 | // const double* rotation = field->getSFRotation(); 86 | // cout << field->getSFBool() << endl; 87 | 88 | double X = gps->getValues()[0]; 89 | double Y = gps->getValues()[1]; 90 | double Z = gps->getValues()[2]; 91 | double v = gps->getSpeed(); 92 | 93 | double dismo[4]; 94 | for(int i = 0; i < 4; ++ i){ 95 | dismo[i] = dismotors[i]->getValue(); 96 | } 97 | 98 | cout << dismo[0] << ' ' << dismo[1] << ' ' << dismo[2] << ' ' << dismo[3] << endl; 99 | 100 | printf("Speed: %lf\n", v); 101 | maze.setLookhead(min(30.0, v * k + lookDis)); 102 | 103 | auto [x, y] = real2pic(Y, X); 104 | 105 | auto [nx, ny] = maze.findLookheadPoint(x, y); 106 | 107 | auto [nY, nX] = pic2real(nx, ny); 108 | 109 | double l = maze.dis(Y, X, nX, nY); 110 | 111 | double R = l * l / 2 / abs(X - nX); 112 | double vv = (R > 2 ? 3 : 1); 113 | 114 | 115 | // printf("%lf %lf %lf %lf\n", X, Y, nX, nY); 116 | int f = nX < X ? 1 : -1; 117 | int r = nY > Y ? 1 : -1; 118 | printf("%s %s\n", f == 1 ? "Forward" : "Back", r == 1 ? "Right" : "Left"); 119 | for(int i = 0; i < 4; ++ i){ 120 | // speed1[i] = f * speed_forward[i] * fabs(nX - X); 121 | // speed2[i] = r * speed_rightward[i] * fabs(nY - Y); 122 | speed1[i] = f * speed_forward[i] * fabs(nX - X) * vv * min(1.0, (f == 1 ? dismo[0] : dismo[1])); 123 | speed2[i] = r * speed_rightward[i] * fabs(nY - Y) * vv * min(1.0, (r == 1 ? dismo[3] : dismo[2])); 124 | } 125 | 126 | const double curx = robot->getSelf()->getOrientation()[0]; 127 | const double cury = robot->getSelf()->getOrientation()[1]; 128 | const double curz = robot->getSelf()->getOrientation()[2]; 129 | printf("%lf %lf %lf\n", curx, cury, curz); 130 | printf("%lf %lf %lf\n", orix, oriy, oriz); 131 | if (abs(curx - orix) > 0.1){ 132 | if (cury < 0){ 133 | printf("LEFT rec\n"); 134 | for(int i = 0; i < 4; ++ i){ 135 | speed1[i] *= 0.01; 136 | speed2[i] *= 0.01; 137 | speed3[i] = speed_leftCircle[i] * abs(curx - orix); 138 | } 139 | }else{ 140 | printf("RIGHT rec\n"); 141 | for(int i = 0; i < 4; ++ i){ 142 | speed1[i] *= 0.01; 143 | speed2[i] *= 0.01; 144 | speed3[i] = speed_rightCircle[i] * abs(curx - orix); 145 | } 146 | } 147 | }else{ 148 | for(int i = 0; i < 4; ++ i) 149 | speed3[i] = 0; 150 | } 151 | 152 | // int keyValue = 0; 153 | // keyValue = keyboard.getKey(); 154 | // if (keyValue == 'W') 155 | // { 156 | // for (int i = 0; i < 4; ++i) 157 | // { 158 | // speed1[i] = speed_forward[i]; 159 | // } 160 | // } 161 | // else if (keyValue == 'S') 162 | // { 163 | // for (int i = 0; i < 4; ++i) 164 | // { 165 | // speed1[i] = speed_backward[i]; 166 | // } 167 | // } 168 | // else if (keyValue == 'A') 169 | // { 170 | // for (int i = 0; i < 4; ++i) 171 | // { 172 | // speed1[i] = speed_leftward[i]; 173 | // } 174 | // } 175 | // else if (keyValue == 'D') 176 | // { 177 | // for (int i = 0; i < 4; ++i) 178 | // { 179 | // speed1[i] = speed_rightward[i]; 180 | // } 181 | // } 182 | // else if (keyValue == 'Q'){ 183 | // for (int i = 0; i < 4; ++ i) 184 | // speed1[i] = speed_leftCircle[i]; 185 | // }else if (keyValue == 'E'){ 186 | // for (int i = 0; i < 4; ++ i) 187 | // speed1[i] = speed_rightCircle[i]; 188 | // }else 189 | // { 190 | // for (int i = 0; i < 4; ++i) 191 | // { 192 | // speed1[i] = 0; 193 | // } 194 | // } 195 | // 196 | for (int i = 0; i < 4; ++i) 197 | { 198 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 199 | } 200 | } 201 | 202 | delete robot; 203 | return 0; 204 | } 205 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/PRM_planning/controllers/Move/maze.png -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/maze.txt: -------------------------------------------------------------------------------- 1 | 800 600 57 2 | 20343 3 | 37127 4 | 46709 5 | 51500 6 | 66494 7 | 75493 8 | 88685 9 | 106680 10 | 113281 11 | 130682 12 | 143267 13 | 146240 14 | 142608 15 | 143183 16 | 147358 17 | 148532 18 | 153910 19 | 156891 20 | 158665 21 | 157437 22 | 153204 23 | 154370 24 | 145949 25 | 153131 26 | 152509 27 | 150676 28 | 157847 29 | 170427 30 | 190228 31 | 203433 32 | 214246 33 | 214868 34 | 219082 35 | 229311 36 | 231133 37 | 238358 38 | 236579 39 | 240193 40 | 256399 41 | 274995 42 | 288200 43 | 299582 44 | 295962 45 | 294131 46 | 292912 47 | 292896 48 | 293466 49 | 310856 50 | 325841 51 | 343247 52 | 360639 53 | 380434 54 | 400828 55 | 418821 56 | 435624 57 | 450630 58 | 461431 59 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/nice.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/PRM_planning/controllers/Move/nice.png -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/nice.txt: -------------------------------------------------------------------------------- 1 | 800 600 130 2 | 20343 3 | 23333 4 | 23324 5 | 22710 6 | 22106 7 | 19695 8 | 15482 9 | 16671 10 | 14859 11 | 19046 12 | 19032 13 | 16622 14 | 18413 15 | 19006 16 | 25596 17 | 27988 18 | 29177 19 | 35773 20 | 39370 21 | 45970 22 | 52565 23 | 59758 24 | 65755 25 | 68150 26 | 75343 27 | 77734 28 | 82534 29 | 89138 30 | 86725 31 | 86711 32 | 89700 33 | 88492 34 | 86080 35 | 89672 36 | 88462 37 | 86655 38 | 84848 39 | 86038 40 | 90232 41 | 89620 42 | 95613 43 | 98612 44 | 105211 45 | 110611 46 | 114210 47 | 120811 48 | 128608 49 | 132795 50 | 135185 51 | 141179 52 | 145969 53 | 144758 54 | 142952 55 | 144139 56 | 146526 57 | 153119 58 | 156115 59 | 153104 60 | 152491 61 | 150678 62 | 156068 63 | 162057 64 | 162043 65 | 166834 66 | 172232 67 | 179435 68 | 184235 69 | 189034 70 | 195641 71 | 201044 72 | 207044 73 | 214846 74 | 217856 75 | 216068 76 | 218480 77 | 216691 78 | 217903 79 | 219116 80 | 219729 81 | 224539 82 | 224545 83 | 228758 84 | 228769 85 | 231774 86 | 236585 87 | 240791 88 | 247389 89 | 254588 90 | 262988 91 | 270794 92 | 276797 93 | 281601 94 | 286998 95 | 289386 96 | 289378 97 | 291765 98 | 294153 99 | 294141 100 | 296533 101 | 301932 102 | 306120 103 | 310916 104 | 317507 105 | 321095 106 | 319288 107 | 321674 108 | 321064 109 | 326463 110 | 334262 111 | 342061 112 | 349261 113 | 351659 114 | 358259 115 | 366054 116 | 372657 117 | 379862 118 | 387060 119 | 394858 120 | 399057 121 | 403851 122 | 408055 123 | 415851 124 | 422450 125 | 430847 126 | 433245 127 | 439241 128 | 445243 129 | 449438 130 | 457232 131 | 461431 132 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/qwq.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/PRM_planning/controllers/Move/qwq.png -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | const int lookDis = 20; 17 | const double k = 1; 18 | const double velocity = 10; 19 | const double velocity2 = 10; 20 | 21 | int main() 22 | { 23 | 24 | // Robot *robot = new Robot(); 25 | Supervisor *robot = new Supervisor(); 26 | int timeStep = (int)robot->getBasicTimeStep(); 27 | cout << timeStep << endl; 28 | 29 | Keyboard keyboard; 30 | keyboard.enable(1); 31 | 32 | GPS* gps = robot->getGPS("gps"); 33 | gps->enable(1); 34 | 35 | Motor *motors[4]; 36 | 37 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 38 | double speed1[4]; 39 | double speed2[4]; 40 | double speed3[4]; 41 | 42 | 43 | for (int i = 0; i < 4; i++) 44 | { 45 | motors[i] = robot->getMotor(wheelsNames[i]); 46 | motors[i]->setPosition(std::numeric_limits::infinity()); 47 | motors[i]->setVelocity(0.0); 48 | speed1[i] = 0; 49 | speed2[i] = 0; 50 | speed3[i] = 0; 51 | } 52 | 53 | double speed_forward[4] = { velocity ,velocity ,velocity ,velocity }; 54 | double speed_backward[4] = { -velocity ,-velocity ,-velocity ,-velocity }; 55 | double speed_leftward[4] = { velocity ,-velocity ,velocity ,-velocity }; 56 | double speed_rightward[4] = { -velocity ,velocity ,-velocity ,velocity }; 57 | 58 | double speed_leftCircle[4] = { velocity2 ,-velocity2 ,-velocity2 ,velocity2 }; 59 | double speed_rightCircle[4] = { -velocity2 ,velocity2 ,velocity2 ,-velocity2 }; 60 | 61 | for(int i = 0; i < 4; ++ i) 62 | speed1[i] = speed_forward[i]; 63 | 64 | while (robot->step(timeStep) != -1) 65 | { 66 | 67 | int keyValue = 0; 68 | keyValue = keyboard.getKey(); 69 | if (keyValue == 'W') 70 | { 71 | for (int i = 0; i < 4; ++i) 72 | { 73 | speed1[i] = speed_forward[i]; 74 | } 75 | } 76 | else if (keyValue == 'S') 77 | { 78 | for (int i = 0; i < 4; ++i) 79 | { 80 | speed1[i] = speed_backward[i]; 81 | } 82 | } 83 | else if (keyValue == 'A') 84 | { 85 | for (int i = 0; i < 4; ++i) 86 | { 87 | speed1[i] = speed_leftward[i]; 88 | } 89 | } 90 | else if (keyValue == 'D') 91 | { 92 | for (int i = 0; i < 4; ++i) 93 | { 94 | speed1[i] = speed_rightward[i]; 95 | } 96 | } 97 | else if (keyValue == 'Q'){ 98 | for (int i = 0; i < 4; ++ i) 99 | speed1[i] = speed_leftCircle[i]; 100 | }else if (keyValue == 'E'){ 101 | for (int i = 0; i < 4; ++ i) 102 | speed1[i] = speed_rightCircle[i]; 103 | }else 104 | { 105 | for (int i = 0; i < 4; ++i) 106 | { 107 | speed1[i] = 0; 108 | } 109 | } 110 | 111 | for (int i = 0; i < 4; ++i) 112 | { 113 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 114 | } 115 | } 116 | 117 | delete robot; 118 | return 0; 119 | } 120 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/tu.cpp: -------------------------------------------------------------------------------- 1 | #include "tu.h" 2 | using namespace std; 3 | 4 | int main(){ 5 | srand(time(0)); 6 | tuP maze("./maze.png"); 7 | maze.findPath(); 8 | // maze.randGenPoint(); 9 | // maze.connectNei(); 10 | maze.drawPoint(); 11 | // maze.savePic("example_point.png"); 12 | maze.drawNei(); 13 | // maze.savePic("example_nei.png"); 14 | maze.drawPath(); 15 | // maze.savePic("example_path.png"); 16 | maze.savePath("example.txt"); 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /PRM_planning/controllers/Move/tu.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | const int ST = 76; // 起始点颜色 9 | const int ED = 36; // 终点颜色 10 | const int REC = 15; // 撒点远离障碍物的切比雪夫距离 11 | const double neiDis = 35; // 邻居判断阈值 12 | const int disX = 20; // 原图分块大小 13 | const int disY = 20; // 原图分块大小 14 | 15 | class tuP{ 16 | int cols, rows; 17 | int sx, sy; 18 | int ex, ey; 19 | int l; 20 | set obs; // 记录障碍物的一维坐标 21 | vector> point; // 记录点坐标 22 | vector> edge; // 记录连边 23 | cv::Mat maze; 24 | 25 | public: 26 | tuP(){ 27 | } 28 | 29 | inline int two2one(int x, int y){ // 将二维坐标转换成一维 30 | return x * cols + y; 31 | } 32 | 33 | inline pair one2two(int id){ // 将一维坐标转换成二维 34 | return {id / cols, id % cols}; 35 | } 36 | 37 | inline double calcDis(int x, int y){ // 计算距离 38 | return sqrt(x * x + y * y); 39 | } 40 | 41 | inline double dis(int x1, int y1, int x2, int y2){ // 计算两个二维点坐标距离 42 | return calcDis(x2 - x1, y2 - y1); 43 | } 44 | 45 | inline double dis(int u, int v){ // 计算两个一维点坐标距离 46 | auto [x1, y1] = one2two(u); 47 | auto [x2, y2] = one2two(v); 48 | return calcDis(x2 - x1, y2 - y1); 49 | } 50 | 51 | tuP(string s){ 52 | sx = ex = -1; 53 | maze = cv::imread(s.c_str(), cv::IMREAD_COLOR); 54 | cols = maze.cols; 55 | rows = maze.rows; 56 | for(int i = 0; i < rows; ++ i) 57 | for(int j = 0; j < cols; ++ j){ // 遍历图 58 | int b = maze.at(i, j)[0]; 59 | int g = maze.at(i, j)[0]; 60 | int r = maze.at(i, j)[0]; 61 | if (b == ST && sx == -1){ // 起点 62 | sx = i; 63 | sy = j; 64 | } 65 | if (b == ED){ // 终点 66 | ex = i; 67 | ey = j; 68 | } 69 | if (b != ST && b != ED && b != 255) // 障碍物 70 | obs.insert(two2one(i, j)); 71 | } 72 | if (sx == -1){ 73 | printf("ERROR: CANNOT FIND Start Point!\n"); 74 | exit(0); 75 | }else{ 76 | point.push_back({sx, sy}); 77 | printf("INFO: Find Start Point: %d %d\n", sx, sy); 78 | } 79 | if (ex == -1){ 80 | printf("ERROR: CANNOT FIND Target Point!\n"); 81 | exit(0); 82 | }else{ 83 | point.push_back({ex, ey}); 84 | printf("INFO: Find Target Point: %d %d\n", ex, ey); 85 | } 86 | } 87 | 88 | inline bool valid(int x, int y){ // 判断当前点是否合法，即是否越界或者是否在障碍物边缘 89 | if (x < 0 && x >= rows) 90 | return false; 91 | if (y < 0 && y >= cols) 92 | return false; 93 | for(int i = -REC; i <= REC; ++ i) 94 | for(int j = -REC; j <= REC; ++ j) 95 | if (obs.find(two2one(x + i, y + j)) != obs.end()) 96 | return false; 97 | return true; 98 | } 99 | 100 | bool checkLine(int x1, int y1, int x2, int y2){ // 判断一条直线上是否有障碍物 101 | double k = 0.01; 102 | for(; k <= 1; k += 0.01) 103 | if (!valid(x1 + (x2 - x1) * k, y1 + (y2 - y1) * k)) 104 | return false; 105 | return true; 106 | } 107 | 108 | void randGenPoint(){ // 随机生成点 109 | for(int i = 0; i < rows; i += disX) 110 | for(int j = 0; j < cols; j += disY){ 111 | int attempCnt = 25; 112 | while(attempCnt--){ 113 | int x = rand() % disX; 114 | int y = rand() % disY; 115 | if (valid(x + i, j + y)){ 116 | point.push_back({x + i, y + j}); 117 | break; 118 | } 119 | } 120 | } 121 | } 122 | 123 | void connectNei(){ // 邻居连边 124 | edge.resize(rows * cols); 125 | for(auto [x, y] : point){ 126 | for(auto [nx, ny] : point){ 127 | if (nx == x && ny == y) 128 | continue; 129 | if (dis(x, y, nx, ny) < neiDis && checkLine(x, y, nx, ny)){ 130 | edge[two2one(x, y)].push_back(two2one(nx, ny)); 131 | edge[two2one(nx, ny)].push_back(two2one(x, y)); 132 | } 133 | } 134 | } 135 | } 136 | 137 | vector path; // 记录最优路径的点的一维坐标 138 | 139 | void findPath(){ // 运用dijkstra寻找一条最短路 140 | randGenPoint(); 141 | connectNei(); 142 | 143 | vector pre(cols * rows); // 记录点的前驱 144 | vector vis(cols * rows, false); 145 | double oo = 1e9 + 7; 146 | vector distance(cols * rows, oo); 147 | priority_queue> team; 148 | team.push({0, two2one(sx, sy)}); 149 | distance[two2one(sx, sy)] = 0; 150 | while(!team.empty()){ // dijkstra主要部分 151 | auto [_, u] = team.top(); 152 | team.pop(); 153 | if (vis[u]) 154 | continue; 155 | vis[u] = true; 156 | for(auto v : edge[u]){ 157 | double w = dis(u, v); 158 | if (distance[v] > distance[u] + w){ 159 | distance[v] = distance[u] + w; 160 | pre[v] = u; 161 | team.push({-distance[v], v}); 162 | } 163 | } 164 | } 165 | if (distance[two2one(ex, ey)] == oo){ 166 | printf("WARNING: CANNOT Find a path from start point to target point!\n"); 167 | exit(0); 168 | } 169 | for(int u = two2one(ex, ey); u != two2one(sx, sy); u = pre[u]) 170 | path.push_back(u); 171 | path.push_back(two2one(sx, sy)); 172 | reverse(path.begin(), path.end()); // 反转，使第一个点为起点 173 | } 174 | 175 | // 以下函数未用到，因为一开始代码未分开，想着找完最短路就可以直接开始寻路，但后来发现无法通过编译（链接出错），因此复制了以下代码到Move.cpp中 176 | 177 | void setLookhead(int x){ 178 | l = x; 179 | } 180 | 181 | int curPoint = -1; 182 | 183 | int findNearestPoint(int x, int y){ 184 | int cc = curPoint; 185 | int cur = two2one(x, y); 186 | double ansdis = 1e9 + 7; 187 | curPoint = 0; 188 | for(size_t i = cc; i < path.size(); ++ i){ 189 | double distance = dis(cur, path[i]); 190 | if (distance < ansdis){ 191 | ansdis = distance; 192 | cc = i; 193 | } 194 | } 195 | // return one2two(path[curPoint]); 196 | return cc; 197 | } 198 | 199 | pair findLookheadPoint(int x, int y){ 200 | int id = findNearestPoint(x, y); 201 | if (id > curPoint) 202 | curPoint = id; 203 | int cur = two2one(x, y); 204 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 205 | ++ curPoint; 206 | } 207 | return one2two(path[curPoint]); 208 | } 209 | 210 | void clear(){ 211 | curPoint = -1; 212 | } 213 | 214 | // 绘制采样点 215 | 216 | void drawPoint(){ 217 | for(auto [y, x] : point){ 218 | cv::rectangle(maze, cv::Point_(x, y), cv::Point_(x + 1, y + 1), cv::Scalar_(255, 0, 0), -1); 219 | } 220 | } 221 | 222 | // 绘制连边 223 | 224 | void drawNei(){ 225 | for(int u = 0 ; u < edge.size(); ++ u) 226 | for(auto v : edge[u]){ 227 | auto [y, x] = one2two(u); 228 | auto [ny, nx] = one2two(v); 229 | cv::line(maze, cv::Point_(x, y), cv::Point_(nx, ny), cv::Scalar_(0,0,255), 1); 230 | } 231 | } 232 | 233 | // 绘制最短路 234 | 235 | void drawPath(){ 236 | if (path.empty()) 237 | findPath(); 238 | for(int i = 0; i < path.size() - 1; ++ i){ 239 | auto [y, x] = one2two(path[i]); 240 | auto [ny, nx] = one2two(path[i + 1]); 241 | cv::line(maze, cv::Point_(x, y), cv::Point_(nx, ny), cv::Scalar_(255,255,0), 3); 242 | } 243 | } 244 | 245 | // 保存图片 246 | 247 | void savePic(string s){ 248 | cv::imwrite(s.c_str(), maze); 249 | } 250 | 251 | // 保存路径 252 | 253 | void savePath(string s){ 254 | FILE* fp = fopen(s.c_str(), "w"); 255 | fprintf(fp, "%d %d %lu\n", rows, cols, path.size()); 256 | for(auto i : path) 257 | fprintf(fp, "%d\n", i); 258 | } 259 | 260 | 261 | }; 262 | -------------------------------------------------------------------------------- /PRM_planning/controllers/display/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | # CXX_SOURCES = Move.cpp ground.cpp 43 | # CXX_SOURCES = main.cpp 44 | ### 45 | ### ---- Compilation options ---- 46 | ### if special compilation flags are necessary: 47 | CFLAGS = -std=c++17 48 | ### 49 | ### ---- Linked libraries ---- 50 | ### if your program needs additional libraries: 51 | # INCLUDE = -I/opt/homebrew/opt/opencv/include/opencv4 52 | # LIBRARIES = -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 53 | ### 54 | ### ---- Linking options ---- 55 | ### if special linking flags are needed: 56 | ### LFLAGS = -s 57 | ### 58 | ### ---- Webots included libraries ---- 59 | ### if you want to use the Webots C API in your C++ controller program: 60 | ### USE_C_API = true 61 | ### 62 | ### ---- Debug mode ---- 63 | ### if you want to display the gcc command line for compilation and link, as 64 | ### well as the rm command details used for cleaning: 65 | VERBOSE = 1 66 | ### 67 | ###----------------------------------------------------------------------------- 68 | 69 | ### Do not modify: this includes Webots global Makefile.include 70 | null := 71 | space := $(null) $(null) 72 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 73 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 74 | -------------------------------------------------------------------------------- /PRM_planning/controllers/display/display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | #define RED 0xBB2222 17 | #define GREEN 0x22BB11 18 | #define BLUE 0x2222BB 19 | 20 | const int lookDis = 20; 21 | 22 | inline pair real2pic(double X, double Y){ 23 | return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)}; 24 | } 25 | 26 | inline pair pic2real(int x, int y, int w, int h){ // 获取像素坐标系在演示坐标系下的坐标 27 | return {h * (1.0 * x / 800), w * (1.0 * y / 600)}; 28 | } 29 | 30 | class Path{ 31 | vector path; 32 | int l; 33 | int cols, rows; 34 | public: 35 | 36 | Path(){} 37 | Path(string s){ 38 | FILE * fp = fopen(s.c_str(), "r"); 39 | int tot = 0; 40 | fscanf(fp, "%d%d%d", &rows, &cols, &tot); 41 | while(tot--){ 42 | int x; 43 | fscanf(fp, "%d", &x); 44 | path.push_back(x); 45 | } 46 | } 47 | 48 | inline int two2one(int x, int y){ 49 | return x * cols + y; 50 | } 51 | 52 | inline pair one2two(int id){ 53 | return {id / cols, id % cols}; 54 | } 55 | 56 | inline double calcDis(int x, int y){ 57 | return sqrt(x * x + y * y); 58 | } 59 | 60 | inline double dis(int x1, int y1, int x2, int y2){ 61 | return calcDis(x2 - x1, y2 - y1); 62 | } 63 | 64 | inline double dis(int u, int v){ 65 | auto [x1, y1] = one2two(u); 66 | auto [x2, y2] = one2two(v); 67 | return calcDis(x2 - x1, y2 - y1); 68 | } 69 | 70 | void clear(){ 71 | curPoint = -1; 72 | } 73 | 74 | void setLookhead(int x){ 75 | l = x; 76 | } 77 | 78 | int curPoint = -1; 79 | 80 | int findNearestPoint(int x, int y){ 81 | int cc = curPoint; 82 | int cur = two2one(x, y); 83 | double ansdis = 1e9 + 7; 84 | curPoint = 0; 85 | for(size_t i = cc; i < path.size(); ++ i){ 86 | double distance = dis(cur, path[i]); 87 | if (distance < ansdis){ 88 | ansdis = distance; 89 | cc = i; 90 | } 91 | } 92 | // return one2two(path[curPoint]); 93 | return cc; 94 | } 95 | 96 | pair findLookheadPoint(int x, int y){ 97 | int id = findNearestPoint(x, y); 98 | if (id > curPoint) 99 | curPoint = id; 100 | int cur = two2one(x, y); 101 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 102 | ++ curPoint; 103 | } 104 | return one2two(path[curPoint]); 105 | } 106 | 107 | void drawpath(int w, int h, Display * display){ 108 | for (auto i : path){ 109 | auto [x, y] = one2two(i); 110 | auto [nX, nY] = pic2real(y, x, w, h); 111 | display->drawPixel(nX, nY); 112 | } 113 | } 114 | }; 115 | 116 | int main() 117 | { 118 | Path maze("../Move/example.txt"); 119 | maze.setLookhead(lookDis); 120 | 121 | 122 | Supervisor *su = new Supervisor(); 123 | int timeStep = (int)su->getBasicTimeStep(); 124 | Display *display = su->getDisplay("display"); 125 | Node * robot = su->getFromDef("Robot"); 126 | 127 | display->setOpacity(1); 128 | 129 | int width = display->getWidth(); 130 | int height = display->getHeight(); 131 | 132 | display->setColor(BLUE); 133 | maze.drawpath(width, height, display); 134 | display->setColor(RED); 135 | 136 | ImageRef* to_store = NULL; 137 | while (su->step(timeStep) != -1) 138 | { 139 | 140 | // 获取小车位置 141 | const double X = robot->getPosition()[0]; 142 | const double Y = robot->getPosition()[1]; 143 | const double Z = robot->getPosition()[2]; 144 | 145 | double real = 2; 146 | 147 | // 模拟行为 148 | 149 | auto [x, y] = real2pic(Y, X); 150 | 151 | auto [ry, rx] = pic2real(x, y, width, height); 152 | 153 | auto [nx, ny] = maze.findLookheadPoint(x, y); 154 | 155 | auto [nY, nX] = pic2real(nx, ny, width, height); 156 | 157 | if (to_store != NULL) // 恢复图片，去除原来的圆圈 158 | display->imagePaste(to_store, 0, 0); 159 | // 绘制目标点 160 | display->drawPixel(nX, nY); 161 | 162 | if (to_store != NULL) 163 | display->imageDelete(to_store); 164 | 165 | // 保存当前图片 166 | to_store = display->imageCopy(0, 0, width, height); 167 | 168 | display->setColor(GREEN); 169 | // 绘制圆圈 170 | display->drawOval(rx, ry, real, real); 171 | display->setColor(RED); 172 | 173 | } 174 | 175 | delete su; 176 | return 0; 177 | } 178 | -------------------------------------------------------------------------------- /PRM_planning/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/PRM_planning/maze.png -------------------------------------------------------------------------------- /PRM_planning/protos/car.wbo: -------------------------------------------------------------------------------- 1 | #VRML_OBJ R2021a utf8 2 | Robot { 3 | translation -6.63565e-17 2.01005 0.039392 4 | rotation -1 6.304952387026931e-13 1.6220961513886875e-13 0.0007995434308963864 5 | children [ 6 | Camera { 7 | translation 0.109025 0.0399999 0.0163641 8 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 9 | children [ 10 | Transform { 11 | translation 0 0 0.02 12 | rotation 1 0 0 1.5708 13 | children [ 14 | Shape { 15 | geometry Cylinder { 16 | height 0.03 17 | radius 0.01 18 | } 19 | } 20 | ] 21 | } 22 | ] 23 | name "camera_left" 24 | antiAliasing TRUE 25 | } 26 | Camera { 27 | translation 0.109025 -0.0400001 0.0163644 28 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 29 | children [ 30 | Transform { 31 | translation 0 0 0.02 32 | rotation 1 0 0 1.5708 33 | children [ 34 | Shape { 35 | geometry Cylinder { 36 | height 0.03 37 | radius 0.01 38 | } 39 | } 40 | ] 41 | } 42 | ] 43 | name "camera_right" 44 | antiAliasing TRUE 45 | } 46 | Camera { 47 | translation 0.109025 0 0.0163644 48 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 49 | children [ 50 | Transform { 51 | translation 0 0 0.02 52 | rotation 1 0 0 1.5708 53 | children [ 54 | Shape { 55 | geometry Cylinder { 56 | height 0.03 57 | radius 0.01 58 | } 59 | } 60 | ] 61 | } 62 | ] 63 | name "camera_mid" 64 | antiAliasing TRUE 65 | } 66 | HingeJoint { 67 | jointParameters HingeJointParameters { 68 | position 3.965440000000003 69 | axis 0 1 0 70 | anchor 0.06 0.07 0 71 | } 72 | device [ 73 | RotationalMotor { 74 | name "motor1" 75 | maxVelocity 100 76 | } 77 | ] 78 | endPoint Solid { 79 | translation 0.06 0.07 0 80 | children [ 81 | DEF WHEEL Shape { 82 | appearance PBRAppearance { 83 | baseColor 0.5 1 1 84 | } 85 | geometry Cylinder { 86 | height 0.02 87 | radius 0.04 88 | } 89 | } 90 | ] 91 | boundingObject USE WHEEL 92 | physics Physics { 93 | } 94 | } 95 | } 96 | HingeJoint { 97 | jointParameters HingeJointParameters { 98 | position 3.965440000000003 99 | axis 0 1 0 100 | anchor -0.06 0.07 0 101 | } 102 | device [ 103 | RotationalMotor { 104 | name "motor2" 105 | maxVelocity 100 106 | } 107 | ] 108 | endPoint Solid { 109 | translation -0.06 0.07 0 110 | children [ 111 | Shape { 112 | appearance PBRAppearance { 113 | baseColor 0.5 1 1 114 | } 115 | geometry Cylinder { 116 | height 0.02 117 | radius 0.04 118 | } 119 | } 120 | ] 121 | name "solid(1)" 122 | boundingObject USE WHEEL 123 | physics Physics { 124 | } 125 | } 126 | } 127 | HingeJoint { 128 | jointParameters HingeJointParameters { 129 | position 3.965440000000003 130 | axis 0 1 0 131 | anchor 0.06 -0.07 0 132 | } 133 | device [ 134 | RotationalMotor { 135 | name "motor3" 136 | maxVelocity 100 137 | } 138 | ] 139 | endPoint Solid { 140 | translation 0.06 -0.07 0 141 | children [ 142 | Shape { 143 | appearance PBRAppearance { 144 | baseColor 0.5 1 1 145 | } 146 | geometry Cylinder { 147 | height 0.02 148 | radius 0.04 149 | } 150 | } 151 | ] 152 | name "solid(2)" 153 | boundingObject USE WHEEL 154 | physics Physics { 155 | } 156 | } 157 | } 158 | HingeJoint { 159 | jointParameters HingeJointParameters { 160 | position 3.965440000000003 161 | axis 0 1 0 162 | anchor -0.06 -0.07 0 163 | } 164 | device [ 165 | RotationalMotor { 166 | name "motor4" 167 | maxVelocity 100 168 | } 169 | ] 170 | endPoint Solid { 171 | translation -0.06 -0.07 0 172 | children [ 173 | Shape { 174 | appearance PBRAppearance { 175 | baseColor 0.5 1 1 176 | } 177 | geometry Cylinder { 178 | height 0.02 179 | radius 0.04 180 | } 181 | } 182 | ] 183 | name "solid(3)" 184 | boundingObject USE WHEEL 185 | physics Physics { 186 | } 187 | } 188 | } 189 | DEF BODY Shape { 190 | appearance PBRAppearance { 191 | } 192 | geometry Box { 193 | size 0.2 0.1 0.05 194 | } 195 | } 196 | ] 197 | boundingObject USE BODY 198 | physics Physics { 199 | density -1 200 | mass 1 201 | } 202 | controller "LineFollowing" 203 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Webots Homework 2 | 3 | 仓库链接：[https://github.com/Lanly109/Webots-Homework](https://github.com/Lanly109/Webots-Homework) 4 | 5 | - 软件版本：Webots 2021a/b, OpenCV 4, clang 13 6 | 7 | ```bash 8 | . 9 | ├── Final_project // 期末大作业源码（感知规划运动） 10 | ├── PRM_planning // PRM 实现的路径规划源码 11 | └── RRT_planning // RRT*实现的路径规划源码 12 | ``` 13 | -------------------------------------------------------------------------------- /RRT_planning/Readme.md: -------------------------------------------------------------------------------- 1 | # Webots RRT*路径规划 2 | 3 | 演示视频：[bilibili](https://www.bilibili.com/video/BV1uL4y1s7yv) 4 | 5 | 实验报告：[Webots——RRT算法](https://blog.lanly.vip/article/9) 6 | 7 | 实验平台：Webots 2021b 8 | 9 | 注：2022a版对坐标系进行了改变，详情点击[此处](https://github.com/cyberbotics/webots/wiki/How-to-adapt-your-world-or-PROTO-to-Webots-R2022a)，故请使用此版本之前的平台，比如2021b。当然你也可以使用该网站提到的的[脚本](https://github.com/cyberbotics/webots/blob/master/scripts/converter/convert_nue_to_enu_rub_to_flu.py)对世界文件进行坐标变换 10 | 11 | ```bash 12 | . 13 | ├── controllers 14 | │   ├── Move // 运动规划控制器 15 | │   │   ├── Makefile // 开了C++17 16 | │   │   ├── Move.cpp // 控制器代码 17 | │   │   ├── example.png // 规划路径一的图 18 | │   │   ├── example.txt // 保存了规划路径一的数据 19 | │   │   ├── maze.png // 原图 20 | │   │   ├── example_path_far.png // 规划路径二的图 21 | │   │   ├── example_path_far.txt // 保存了规划路径二的数据 22 | │   │   ├── example_path_norewrite.png // 规划路径三的图 23 | │   │   ├── example_path_norewrite.txt // 保存了规划路径三的数据 24 | │   │   ├── test.cpp // 源代码，能键盘控制小车上下左右移动 25 | │   │   ├── tu.cpp // RRT算法主程序 26 | │   │   └── tu.h // 实现RRT算法的类 27 | │   └── display 28 | │   ├── Makefile // 开了C++17 29 | │   └── display.cpp // 可视化控制器代码 30 | ├── libraries 31 | ├── maze.png 32 | ├── plugins 33 | ├── protos 34 | ├── Readme.md // 本须知 35 | └── worlds // 世界文件 36 | ``` 37 | 38 | 双击`world/empty.wbt`打开世界。 39 | 40 | ### 编译 41 | 编译 `tu.cpp` 42 | ```bash 43 | g++ tu.cpp `pkg-config --cflags --libs opencv` -std=c++17 -g -o tu # 部分电脑可能是opencv4 44 | ./tu 45 | ``` 46 | 47 | 如非 `bash`环境或未安装`pkg-config`或未对opencv配置，请自行更改为将``` `pkg-config --cflags --libs opencv` ```更改为链接本机`OpenCV`库的参数指令。 48 | 49 | 如在Mac下``` `pkg-config --cflags --libs opencv` ```的运行结果为 50 | ```bash 51 | -I/opt/homebrew/opt/opencv/include/opencv4 -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 52 | ``` 53 | 54 | 通过修改`tu.h`文件下的 `REC, stepDistance, rewriteDistance, fatherDistance, targetDistance`变量可以得到不同的路径图。部分情况可能求解时间超过1s，还请耐心等待。 55 | 56 | **限于系统原因，OpenCV安装时所编译的动态链接库为Arm架构，而Webots暂未对MacOS的M1类型适配，其动态链接库均为x86_64架构。这将导致Webots在生成项目时，在链接动态链接库阶段出现找不到OpenCV相关成员变量的x86_64的动态库，而在手动用命令行编译时，亦出现找不到Webots相关成员变量的Arm的动态库。因此这里将这两个代码分开，即路径规划和巡线代码为不同文件，其中路径规划代码引用了OpenCV库，在命令行编译下能够正常编译成功运行，巡线代码在Webots下亦能正常编译成功运行。而理论上它们在Linux或Windows应能融洽在同一个代码中。** 57 | 58 | ### 控制器代码 59 | 60 | 务必开启 `c++17`标准。 61 | 62 | 通过修改109行的`Path maze("./example.txt");`中的文件名来实现不同的路径寻路。 63 | 64 | 同时也要相应的修改 `display.cpp`文件的118行的`Path maze("../Move/example.txt");`于相应的文件。 65 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | # CXX_SOURCES = Move.cpp ground.cpp 43 | # CXX_SOURCES = main.cpp 44 | ### 45 | ### ---- Compilation options ---- 46 | ### if special compilation flags are necessary: 47 | CFLAGS = -std=c++17 48 | ### 49 | ### ---- Linked libraries ---- 50 | ### if your program needs additional libraries: 51 | # INCLUDE = -I/opt/homebrew/opt/opencv/include/opencv4 52 | # LIBRARIES = -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 53 | ### 54 | ### ---- Linking options ---- 55 | ### if special linking flags are needed: 56 | ### LFLAGS = -s 57 | ### 58 | ### ---- Webots included libraries ---- 59 | ### if you want to use the Webots C API in your C++ controller program: 60 | ### USE_C_API = true 61 | ### 62 | ### ---- Debug mode ---- 63 | ### if you want to display the gcc command line for compilation and link, as 64 | ### well as the rm command details used for cleaning: 65 | VERBOSE = 1 66 | ### 67 | ###----------------------------------------------------------------------------- 68 | 69 | ### Do not modify: this includes Webots global Makefile.include 70 | null := 71 | space := $(null) $(null) 72 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 73 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 74 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/Move.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | const int lookDis = 20; 17 | const double k = 1; 18 | const double velocity = 40; 19 | const double velocity2 = 30; 20 | 21 | inline pair real2pic(double X, double Y){ // 模拟现实左边转换成像素坐标 22 | return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)}; 23 | } 24 | 25 | inline pair pic2real(int x, int y){ // 像素坐标转换成模拟世界坐标 26 | return {3 - 6 * x * 1.0 / 800, -2.25 + 4.5 * y * 1.0 / 600}; 27 | } 28 | 29 | class Path{ 30 | vector path; // 读取路径 31 | int l; 32 | int cols, rows; 33 | public: 34 | 35 | Path(){} 36 | Path(string s){ 37 | FILE * fp = fopen(s.c_str(), "r"); 38 | int tot = 0; 39 | fscanf(fp, "%d%d%d", &rows, &cols, &tot); 40 | while(tot--){ 41 | int x; 42 | fscanf(fp, "%d", &x); 43 | path.push_back(x); 44 | } 45 | } 46 | 47 | inline int two2one(int x, int y){ // 将二维坐标转换成一维 48 | return x * cols + y; 49 | } 50 | 51 | inline pair one2two(int id){ // 将一维坐标转换成二维 52 | return {id / cols, id % cols}; 53 | } 54 | 55 | inline double calcDis(int x, int y){ // 计算距离 56 | return sqrt(x * x + y * y); 57 | } 58 | 59 | inline double dis(int x1, int y1, int x2, int y2){ // 计算两个二维点坐标距离 60 | return calcDis(x2 - x1, y2 - y1); 61 | } 62 | 63 | inline double dis(int u, int v){ // 计算两个一维点坐标距离 64 | auto [x1, y1] = one2two(u); 65 | auto [x2, y2] = one2two(v); 66 | return calcDis(x2 - x1, y2 - y1); 67 | } 68 | 69 | void clear(){ 70 | curPoint = -1; 71 | } 72 | 73 | void setLookhead(int x){ // 设置lookahead distance 74 | l = x; 75 | } 76 | 77 | int curPoint = -1; 78 | 79 | int findNearestPoint(int x, int y){ // 寻找距离当前位置最近的点 80 | int cc = curPoint; 81 | int cur = two2one(x, y); 82 | double ansdis = 1e9 + 7; 83 | curPoint = 0; 84 | for(size_t i = cc; i < path.size(); ++ i){ 85 | double distance = dis(cur, path[i]); 86 | if (distance < ansdis){ 87 | ansdis = distance; 88 | cc = i; 89 | } 90 | } 91 | // return one2two(path[curPoint]); 92 | return cc; 93 | } 94 | 95 | pair findLookheadPoint(int x, int y){ // 寻找距离当前位置大于lookahead distance的最近的点 96 | int id = findNearestPoint(x, y); 97 | if (id > curPoint) 98 | curPoint = id; 99 | int cur = two2one(x, y); 100 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 101 | ++ curPoint; 102 | } 103 | return one2two(path[curPoint]); 104 | } 105 | }; 106 | 107 | int main() 108 | { 109 | Path maze("./example.txt"); 110 | maze.setLookhead(lookDis); 111 | 112 | // Robot *robot = new Robot(); 113 | Supervisor *robot = new Supervisor(); 114 | 115 | int timeStep = (int)robot->getBasicTimeStep(); 116 | cout << timeStep << endl; 117 | 118 | Keyboard keyboard; 119 | keyboard.enable(1); 120 | 121 | GPS* gps = robot->getGPS("gps"); 122 | gps->enable(1); 123 | 124 | Motor *motors[4]; 125 | DistanceSensor *dismotors[4]; 126 | 127 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 128 | char distanceNames[4][8] = {"front", "back", "left", "right"}; 129 | double speed1[4]; 130 | double speed2[4]; 131 | double speed3[4]; 132 | 133 | 134 | for (int i = 0; i < 4; i++) 135 | { // 初始化电机和距离传感器 136 | motors[i] = robot->getMotor(wheelsNames[i]); 137 | dismotors[i] = robot->getDistanceSensor(distanceNames[i]); 138 | dismotors[i]->enable(1); 139 | motors[i]->setPosition(std::numeric_limits::infinity()); 140 | motors[i]->setVelocity(0.0); 141 | speed1[i] = 0; 142 | speed2[i] = 0; 143 | speed3[i] = 0; 144 | } 145 | 146 | double speed_forward[4] = { velocity ,velocity ,velocity ,velocity }; 147 | double speed_backward[4] = { -velocity ,-velocity ,-velocity ,-velocity }; 148 | double speed_leftward[4] = { velocity ,-velocity ,velocity ,-velocity }; 149 | double speed_rightward[4] = { -velocity ,velocity ,-velocity ,velocity }; 150 | 151 | double speed_leftCircle[4] = { velocity2 ,-velocity2 ,-velocity2 ,velocity2 }; 152 | double speed_rightCircle[4] = { -velocity2 ,velocity2 ,velocity2 ,-velocity2 }; 153 | 154 | for(int i = 0; i < 4; ++ i) 155 | speed1[i] = speed_forward[i]; 156 | 157 | //规定小车的朝向 158 | 159 | double orix = -1; 160 | double oriy = 0; 161 | double oriz = 0; 162 | 163 | while (robot->step(timeStep) != -1) 164 | { 165 | // const double* rotation = field->getSFRotation(); 166 | // cout << field->getSFBool() << endl; 167 | 168 | double X = gps->getValues()[0]; 169 | double Y = gps->getValues()[1]; 170 | double Z = gps->getValues()[2]; 171 | double v = gps->getSpeed(); 172 | // 获取小车坐标以及速度 173 | 174 | double dismo[4]; 175 | for(int i = 0; i < 4; ++ i){ // 获取距离传感器的值 176 | dismo[i] = dismotors[i]->getValue(); 177 | } 178 | 179 | printf("Speed: %lf\n", v); 180 | // 根据速度设置Lookahead distance 181 | maze.setLookhead(min(30.0, v * k + lookDis)); 182 | 183 | // 计算当前坐标在像素坐标系的坐标 184 | auto [x, y] = real2pic(Y, X); 185 | 186 | // 找到目标点 187 | auto [nx, ny] = maze.findLookheadPoint(x, y); 188 | 189 | // 换算成模拟世界坐标 190 | auto [nY, nX] = pic2real(nx, ny); 191 | 192 | // 计算距离 193 | double l = maze.dis(Y, X, nX, nY); 194 | 195 | // 计算以圆上行走的半径 196 | double R = l * l / 2 / min(abs(Y - nY), abs(X - nX)); 197 | // 判断是否直线，直线则加速 198 | double vv = ((R == 0 || R > 2) ? 3 : 1); 199 | cout << vv << endl; 200 | 201 | // printf("%lf %lf %lf %lf\n", X, Y, nX, nY); 202 | // 判断当前应该向前向后以及向左向右 203 | int f = nX < X ? 1 : -1; 204 | int r = nY > Y ? 1 : -1; 205 | printf("%s %s\n", f == 1 ? "Forward" : "Back", r == 1 ? "Right" : "Left"); 206 | for(int i = 0; i < 4; ++ i){ // 小车速度 207 | // speed1[i] = f * speed_forward[i] * fabs(nX - X); 208 | // speed2[i] = r * speed_rightward[i] * fabs(nY - Y); 209 | speed1[i] = f * speed_forward[i] * fabs(nX - X) * vv * min(1.0, (f == 1 ? dismo[0] : dismo[1])); 210 | speed2[i] = r * speed_rightward[i] * fabs(nY - Y) * vv * min(1.0, (r == 1 ? dismo[3] : dismo[2])); 211 | } 212 | 213 | // 判断当前小车朝向 214 | const double curx = robot->getSelf()->getOrientation()[0]; 215 | const double cury = robot->getSelf()->getOrientation()[1]; 216 | const double curz = robot->getSelf()->getOrientation()[2]; 217 | printf("%lf %lf %lf\n", curx, cury, curz); 218 | printf("%lf %lf %lf\n", orix, oriy, oriz); 219 | // 如果偏离朝向，则根据偏离方向，设定自旋速度，并降低原先的行进速度 220 | if (abs(curx - orix) > 0.1){ 221 | if (cury < 0){ 222 | printf("LEFT rec\n"); 223 | for(int i = 0; i < 4; ++ i){ 224 | speed1[i] *= 0.01; 225 | speed2[i] *= 0.01; 226 | speed3[i] = speed_leftCircle[i] * abs(curx - orix); 227 | } 228 | }else{ 229 | printf("RIGHT rec\n"); 230 | for(int i = 0; i < 4; ++ i){ 231 | speed1[i] *= 0.01; 232 | speed2[i] *= 0.01; 233 | speed3[i] = speed_rightCircle[i] * abs(curx - orix); 234 | } 235 | } 236 | }else{ 237 | for(int i = 0; i < 4; ++ i) 238 | speed3[i] = 0; 239 | } 240 | 241 | // int keyValue = 0; 242 | // keyValue = keyboard.getKey(); 243 | // if (keyValue == 'W') 244 | // { 245 | // for (int i = 0; i < 4; ++i) 246 | // { 247 | // speed1[i] = speed_forward[i]; 248 | // } 249 | // } 250 | // else if (keyValue == 'S') 251 | // { 252 | // for (int i = 0; i < 4; ++i) 253 | // { 254 | // speed1[i] = speed_backward[i]; 255 | // } 256 | // } 257 | // else if (keyValue == 'A') 258 | // { 259 | // for (int i = 0; i < 4; ++i) 260 | // { 261 | // speed1[i] = speed_leftward[i]; 262 | // } 263 | // } 264 | // else if (keyValue == 'D') 265 | // { 266 | // for (int i = 0; i < 4; ++i) 267 | // { 268 | // speed1[i] = speed_rightward[i]; 269 | // } 270 | // } 271 | // else if (keyValue == 'Q'){ 272 | // for (int i = 0; i < 4; ++ i) 273 | // speed1[i] = speed_leftCircle[i]; 274 | // }else if (keyValue == 'E'){ 275 | // for (int i = 0; i < 4; ++ i) 276 | // speed1[i] = speed_rightCircle[i]; 277 | // }else 278 | // { 279 | // for (int i = 0; i < 4; ++i) 280 | // { 281 | // speed1[i] = 0; 282 | // } 283 | // } 284 | // 285 | for (int i = 0; i < 4; ++i) // 设置速度 286 | { 287 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 288 | } 289 | } 290 | 291 | delete robot; 292 | return 0; 293 | } 294 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example.txt: -------------------------------------------------------------------------------- 1 | 800 600 90 2 | 20343 3 | 26336 4 | 31732 5 | 46118 6 | 53311 7 | 68305 8 | 73102 9 | 88690 10 | 95285 11 | 100680 12 | 103677 13 | 118073 14 | 129468 15 | 143858 16 | 146244 17 | 143834 18 | 145608 19 | 145578 20 | 146164 21 | 144959 22 | 150331 23 | 153903 24 | 153893 25 | 153889 26 | 156878 27 | 162853 28 | 162828 29 | 161006 30 | 160393 31 | 161577 32 | 159756 33 | 159147 34 | 160322 35 | 160913 36 | 162084 37 | 163271 38 | 163859 39 | 165643 40 | 168042 41 | 175240 42 | 182440 43 | 189640 44 | 196841 45 | 199241 46 | 205838 47 | 213045 48 | 214259 49 | 220871 50 | 228085 51 | 230513 52 | 229919 53 | 232339 54 | 235362 55 | 236573 56 | 239594 57 | 247394 58 | 264795 59 | 271997 60 | 288194 61 | 294194 62 | 295965 63 | 297752 64 | 300727 65 | 305508 66 | 307304 67 | 314478 68 | 320457 69 | 325255 70 | 332454 71 | 336654 72 | 341454 73 | 345656 74 | 352856 75 | 360055 76 | 367256 77 | 372056 78 | 379255 79 | 385855 80 | 388254 81 | 395452 82 | 399654 83 | 411054 84 | 419455 85 | 423655 86 | 432057 87 | 439256 88 | 456065 89 | 458461 90 | 462051 91 | 465638 92 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_far.txt: -------------------------------------------------------------------------------- 1 | 800 600 369 2 | 20343 3 | 20338 4 | 20933 5 | 20928 6 | 21523 7 | 23914 8 | 28107 9 | 32902 10 | 35897 11 | 40694 12 | 44287 13 | 48481 14 | 50880 15 | 53273 16 | 52668 17 | 53863 18 | 58660 19 | 61061 20 | 63462 21 | 68261 22 | 73061 23 | 77860 24 | 80260 25 | 85061 26 | 88066 27 | 89869 28 | 94067 29 | 96467 30 | 101268 31 | 104871 32 | 109073 33 | 113868 34 | 117461 35 | 122261 36 | 127059 37 | 129459 38 | 134259 39 | 139058 40 | 143858 41 | 146258 42 | 148050 43 | 152244 44 | 154641 45 | 158234 46 | 157029 47 | 155824 48 | 154619 49 | 155214 50 | 157606 51 | 160598 52 | 161189 53 | 158186 54 | 158777 55 | 158772 56 | 158767 57 | 158159 58 | 157554 59 | 155150 60 | 152746 61 | 150941 62 | 151536 63 | 153327 64 | 155123 65 | 156318 66 | 156313 67 | 157505 68 | 156900 69 | 156295 70 | 155690 71 | 156281 72 | 155673 73 | 155068 74 | 155063 75 | 154455 76 | 154450 77 | 152645 78 | 149639 79 | 150834 80 | 150226 81 | 149621 82 | 152013 83 | 153208 84 | 155605 85 | 158002 86 | 159195 87 | 161592 88 | 160383 89 | 157979 90 | 156174 91 | 154965 92 | 156156 93 | 159149 94 | 161547 95 | 160338 96 | 158533 97 | 155531 98 | 152529 99 | 149528 100 | 147723 101 | 148318 102 | 150109 103 | 148301 104 | 150692 105 | 151887 106 | 153680 107 | 155476 108 | 159671 109 | 162665 110 | 161460 111 | 159651 112 | 159646 113 | 160239 114 | 164434 115 | 166833 116 | 171632 117 | 174032 118 | 178832 119 | 183632 120 | 188433 121 | 193234 122 | 198034 123 | 202835 124 | 205235 125 | 210035 126 | 212435 127 | 217235 128 | 221431 129 | 223831 130 | 228632 131 | 231034 132 | 232842 133 | 232850 134 | 233458 135 | 232265 136 | 232869 137 | 234677 138 | 235885 139 | 234693 140 | 234101 141 | 234105 142 | 235911 143 | 234715 144 | 233523 145 | 231126 146 | 225729 147 | 223332 148 | 219739 149 | 217347 150 | 214954 151 | 215562 152 | 217370 153 | 219178 154 | 217384 155 | 215591 156 | 217398 157 | 219799 158 | 224004 159 | 228806 160 | 231207 161 | 236610 162 | 236614 163 | 234222 164 | 233626 165 | 234233 166 | 234840 167 | 232447 168 | 231855 169 | 232462 170 | 232470 171 | 228875 172 | 228284 173 | 233087 174 | 237289 175 | 242090 176 | 246889 177 | 251690 178 | 256488 179 | 261289 180 | 265486 181 | 270289 182 | 275090 183 | 279891 184 | 283486 185 | 288285 186 | 293086 187 | 297290 188 | 301488 189 | 306291 190 | 311092 191 | 315290 192 | 319488 193 | 323690 194 | 328489 195 | 332689 196 | 336885 197 | 341686 198 | 347087 199 | 349488 200 | 353692 201 | 357297 202 | 359104 203 | 359711 204 | 361515 205 | 365118 206 | 367525 207 | 366933 208 | 364540 209 | 361548 210 | 362755 211 | 360962 212 | 358565 213 | 354370 214 | 353174 215 | 350173 216 | 347173 217 | 344173 218 | 341173 219 | 338173 220 | 335173 221 | 332173 222 | 329173 223 | 326173 224 | 323173 225 | 320173 226 | 317173 227 | 314173 228 | 311173 229 | 308175 230 | 306383 231 | 305787 232 | 305191 233 | 303999 234 | 304607 235 | 305814 236 | 306422 237 | 307030 238 | 304637 239 | 301644 240 | 302248 241 | 303455 242 | 305864 243 | 304073 244 | 305281 245 | 307087 246 | 306491 247 | 305899 248 | 307105 249 | 305913 250 | 302917 251 | 299925 252 | 298133 253 | 296341 254 | 297549 255 | 301753 256 | 305358 257 | 307758 258 | 312558 259 | 317356 260 | 322155 261 | 326952 262 | 329351 263 | 333548 264 | 337743 265 | 342544 266 | 347344 267 | 352742 268 | 355142 269 | 359941 270 | 364740 271 | 367140 272 | 367731 273 | 366526 274 | 365321 275 | 366516 276 | 369508 277 | 370103 278 | 370694 279 | 373091 280 | 377285 281 | 380878 282 | 379073 283 | 380264 284 | 385060 285 | 389860 286 | 394660 287 | 397060 288 | 401862 289 | 406665 290 | 412063 291 | 416860 292 | 421660 293 | 425863 294 | 428264 295 | 431269 296 | 436070 297 | 439663 298 | 440858 299 | 442053 300 | 442648 301 | 443243 302 | 442638 303 | 442633 304 | 443828 305 | 443823 306 | 446215 307 | 445006 308 | 444997 309 | 441989 310 | 439581 311 | 442573 312 | 443768 313 | 441359 314 | 440154 315 | 440749 316 | 440744 317 | 441935 318 | 442530 319 | 442521 320 | 441916 321 | 442511 322 | 443706 323 | 446097 324 | 446692 325 | 446087 326 | 444882 327 | 443677 328 | 444872 329 | 444867 330 | 443658 331 | 440051 332 | 438246 333 | 437641 334 | 438236 335 | 438831 336 | 441223 337 | 442418 338 | 443613 339 | 443608 340 | 443003 341 | 440599 342 | 439994 343 | 438185 344 | 437580 345 | 437571 346 | 437566 347 | 437561 348 | 439953 349 | 438144 350 | 437539 351 | 437534 352 | 438729 353 | 438124 354 | 438715 355 | 437510 356 | 439903 357 | 442303 358 | 444697 359 | 442892 360 | 440488 361 | 440479 362 | 438671 363 | 438666 364 | 439261 365 | 440456 366 | 440451 367 | 440446 368 | 444040 369 | 446439 370 | 451239 371 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_nei.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_nei.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_nei_far.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_nei_far.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_nei_no_rewrite.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_nei_no_rewrite.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_norewrite.txt: -------------------------------------------------------------------------------- 1 | 800 600 146 2 | 20343 3 | 25142 4 | 29939 5 | 35328 6 | 38916 7 | 44905 8 | 50902 9 | 55098 10 | 61692 11 | 65885 12 | 70678 13 | 76671 14 | 82662 15 | 91057 16 | 98255 17 | 103055 18 | 111455 19 | 118059 20 | 125258 21 | 127657 22 | 134853 23 | 141455 24 | 143852 25 | 149242 26 | 151636 27 | 157629 28 | 164222 29 | 169017 30 | 175010 31 | 177398 32 | 173185 33 | 171380 34 | 164777 35 | 159370 36 | 156361 37 | 154551 38 | 152141 39 | 151531 40 | 150926 41 | 149712 42 | 147902 43 | 146097 44 | 147883 45 | 150870 46 | 151465 47 | 150851 48 | 150837 49 | 155625 50 | 159818 51 | 163411 52 | 165198 53 | 165793 54 | 163983 55 | 162174 56 | 160965 57 | 156157 58 | 153753 59 | 152540 60 | 153135 61 | 156725 62 | 159122 63 | 160909 64 | 159704 65 | 161491 66 | 162686 67 | 159674 68 | 156065 69 | 153657 70 | 148847 71 | 148842 72 | 151828 73 | 154225 74 | 161418 75 | 168617 76 | 173417 77 | 180618 78 | 185419 79 | 190220 80 | 197420 81 | 204024 82 | 207629 83 | 214231 84 | 221434 85 | 228636 86 | 234047 87 | 234654 88 | 234666 89 | 235274 90 | 234686 91 | 235890 92 | 234104 93 | 232312 94 | 227523 95 | 222132 96 | 222744 97 | 223352 98 | 227565 99 | 231177 100 | 234188 101 | 240195 102 | 247396 103 | 254596 104 | 256996 105 | 264196 106 | 268996 107 | 277397 108 | 282196 109 | 289396 110 | 293589 111 | 298377 112 | 302565 113 | 302553 114 | 300744 115 | 303132 116 | 304318 117 | 308511 118 | 313903 119 | 318097 120 | 319883 121 | 319271 122 | 318661 123 | 321058 124 | 326453 125 | 333045 126 | 335446 127 | 342647 128 | 345049 129 | 351054 130 | 353455 131 | 360057 132 | 364856 133 | 371457 134 | 378657 135 | 387056 136 | 392457 137 | 399055 138 | 406856 139 | 411655 140 | 417655 141 | 424255 142 | 430254 143 | 438650 144 | 445842 145 | 450036 146 | 455427 147 | 459625 148 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_path.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_path_far.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_path_far.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_path_norewrite.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_path_norewrite.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_point.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_point.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_point_far.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_point_far.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/example_point_norewrite.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/example_point_norewrite.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/controllers/Move/maze.png -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | const int lookDis = 20; 17 | const double k = 1; 18 | const double velocity = 10; 19 | const double velocity2 = 10; 20 | 21 | int main() 22 | { 23 | 24 | // Robot *robot = new Robot(); 25 | Supervisor *robot = new Supervisor(); 26 | int timeStep = (int)robot->getBasicTimeStep(); 27 | cout << timeStep << endl; 28 | 29 | Keyboard keyboard; 30 | keyboard.enable(1); 31 | 32 | GPS* gps = robot->getGPS("gps"); 33 | gps->enable(1); 34 | 35 | Motor *motors[4]; 36 | 37 | char wheelsNames[4][8] = {"motor1", "motor2", "motor3", "motor4"}; 38 | double speed1[4]; 39 | double speed2[4]; 40 | double speed3[4]; 41 | 42 | 43 | for (int i = 0; i < 4; i++) 44 | { 45 | motors[i] = robot->getMotor(wheelsNames[i]); 46 | motors[i]->setPosition(std::numeric_limits::infinity()); 47 | motors[i]->setVelocity(0.0); 48 | speed1[i] = 0; 49 | speed2[i] = 0; 50 | speed3[i] = 0; 51 | } 52 | 53 | double speed_forward[4] = { velocity ,velocity ,velocity ,velocity }; 54 | double speed_backward[4] = { -velocity ,-velocity ,-velocity ,-velocity }; 55 | double speed_leftward[4] = { velocity ,-velocity ,velocity ,-velocity }; 56 | double speed_rightward[4] = { -velocity ,velocity ,-velocity ,velocity }; 57 | 58 | double speed_leftCircle[4] = { velocity2 ,-velocity2 ,-velocity2 ,velocity2 }; 59 | double speed_rightCircle[4] = { -velocity2 ,velocity2 ,velocity2 ,-velocity2 }; 60 | 61 | for(int i = 0; i < 4; ++ i) 62 | speed1[i] = speed_forward[i]; 63 | 64 | while (robot->step(timeStep) != -1) 65 | { 66 | 67 | int keyValue = 0; 68 | keyValue = keyboard.getKey(); 69 | if (keyValue == 'W') 70 | { 71 | for (int i = 0; i < 4; ++i) 72 | { 73 | speed1[i] = speed_forward[i]; 74 | } 75 | } 76 | else if (keyValue == 'S') 77 | { 78 | for (int i = 0; i < 4; ++i) 79 | { 80 | speed1[i] = speed_backward[i]; 81 | } 82 | } 83 | else if (keyValue == 'A') 84 | { 85 | for (int i = 0; i < 4; ++i) 86 | { 87 | speed1[i] = speed_leftward[i]; 88 | } 89 | } 90 | else if (keyValue == 'D') 91 | { 92 | for (int i = 0; i < 4; ++i) 93 | { 94 | speed1[i] = speed_rightward[i]; 95 | } 96 | } 97 | else if (keyValue == 'Q'){ 98 | for (int i = 0; i < 4; ++ i) 99 | speed1[i] = speed_leftCircle[i]; 100 | }else if (keyValue == 'E'){ 101 | for (int i = 0; i < 4; ++ i) 102 | speed1[i] = speed_rightCircle[i]; 103 | }else 104 | { 105 | for (int i = 0; i < 4; ++i) 106 | { 107 | speed1[i] = 0; 108 | } 109 | } 110 | 111 | for (int i = 0; i < 4; ++i) 112 | { 113 | motors[i]->setVelocity(speed1[i] + speed2[i] + speed3[i]); 114 | } 115 | } 116 | 117 | delete robot; 118 | return 0; 119 | } 120 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/tu.cpp: -------------------------------------------------------------------------------- 1 | #include "tu.h" 2 | // #include 3 | #include 4 | #include 5 | // #include 6 | using namespace std; 7 | 8 | tuP maze("./maze.png"); 9 | 10 | // void myhandle(int s){ 11 | // printf("Catch exception, interupting, saveing and exiting...\n"); 12 | // maze.drawPoint(); 13 | // maze.savePic("qwq_point.png"); 14 | // maze.drawNei(); 15 | // maze.savePic("qwq_line.png"); 16 | // exit(0); 17 | // } 18 | 19 | int main(){ 20 | // struct sigaction sigIntHandler; 21 | // 22 | // sigIntHandler.sa_handler = myhandle; 23 | // sigemptyset(&sigIntHandler.sa_mask); 24 | // sigIntHandler.sa_flags = 0; 25 | // 26 | // sigaction(SIGINT, &sigIntHandler, NULL); 27 | srand(time(0)); 28 | maze.findPath(); 29 | // maze.randGenPoint(); 30 | // maze.connectNei(); 31 | maze.drawPoint(); 32 | maze.savePic("example_point.png"); 33 | maze.drawNei(); 34 | maze.savePic("example_nei.png"); 35 | maze.drawPath(); 36 | maze.savePic("example_path.png"); 37 | maze.savePath("example.txt"); 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /RRT_planning/controllers/Move/tu.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | const int ST = 76; // 起始点颜色 10 | const int ED = 36; // 终点颜色 11 | const int REC = 15; // 撒点远离障碍物的切比雪夫距离 12 | const int stepDistance = 5; // 步长距离 13 | const int rewriteDistance = 30; // 重布线的点的距离 14 | const int fatherDistance = 15; // 父亲选择的距离 15 | const int targetDistance = 10; // 距离终点的距离 16 | 17 | class tuP{ 18 | int cols, rows; 19 | int sx, sy; 20 | int ex, ey; 21 | int l; 22 | set obs; // 记录障碍物的一维坐标 23 | vector> edge; // 父亲指向儿子的边 24 | cv::Mat maze; 25 | 26 | public: 27 | tuP(){ 28 | } 29 | 30 | inline int two2one(int x, int y){ // 将二维坐标转换成一维 31 | return x * cols + y; 32 | } 33 | 34 | inline pair one2two(int id){ // 将一维坐标转换成二维 35 | return {id / cols, id % cols}; 36 | } 37 | 38 | inline double calcDis(int x, int y){ // 计算距离 39 | return sqrt(x * x + y * y); 40 | } 41 | 42 | inline double dis(int x1, int y1, int x2, int y2){ // 计算两个二维点坐标距离 43 | return calcDis(x2 - x1, y2 - y1); 44 | } 45 | 46 | inline double dis(int u, int v){ // 计算两个一维点坐标距离 47 | auto [x1, y1] = one2two(u); 48 | auto [x2, y2] = one2two(v); 49 | return calcDis(x2 - x1, y2 - y1); 50 | } 51 | 52 | tuP(string s){ 53 | sx = ex = -1; 54 | maze = cv::imread(s.c_str(), cv::IMREAD_COLOR); 55 | cols = maze.cols; 56 | rows = maze.rows; 57 | for(int i = 0; i < rows; ++ i) 58 | for(int j = 0; j < cols; ++ j){ // 遍历图 59 | int b = maze.at(i, j)[0]; 60 | int g = maze.at(i, j)[0]; 61 | int r = maze.at(i, j)[0]; 62 | if (b == ST && sx == -1){ // 起点 63 | sx = i; 64 | sy = j; 65 | } 66 | if (b == ED){ // 终点 67 | ex = i; 68 | ey = j; 69 | } 70 | if (b != ST && b != ED && b != 255){ // 障碍物 71 | for(int x = -REC; x <= REC; ++ x) 72 | for(int y = -REC; y <= REC; ++ y) 73 | obs.insert(two2one(i + x, j + y)); 74 | } 75 | } 76 | if (sx == -1){ 77 | printf("ERROR: CANNOT FIND Start Point!\n"); 78 | exit(0); 79 | }else{ 80 | printf("INFO: Find Start Point: %d %d\n", sx, sy); 81 | } 82 | if (ex == -1){ 83 | printf("ERROR: CANNOT FIND Target Point!\n"); 84 | exit(0); 85 | }else{ 86 | printf("INFO: Find Target Point: %d %d\n", ex, ey); 87 | } 88 | } 89 | 90 | inline bool valid(int x, int y){ // 判断当前点是否合法，即是否越界或者是否在障碍物边缘 91 | if (x < 0 || x >= rows) 92 | return false; 93 | if (y < 0 || y >= cols) 94 | return false; 95 | return obs.find(two2one(x, y)) == obs.end(); 96 | // 大大的优化，速度提升了REC*REC倍 97 | // 98 | // for(int i = -REC; i <= REC; ++ i) 99 | // for(int j = -REC; j <= REC; ++ j) 100 | // if (obs.find(two2one(x + i, y + j)) != obs.end()) 101 | // return false; 102 | // return true; 103 | } 104 | 105 | bool checkLine(int x1, int y1, int x2, int y2){ // 判断一条直线上是否有障碍物 106 | double k = 0.01; 107 | for(; k <= 1; k += 0.01) 108 | if (!valid(x1 + (x2 - x1) * k, y1 + (y2 - y1) * k)) 109 | return false; 110 | return true; 111 | } 112 | 113 | bool checkLine(int p1, int p2){ // 判断一条直线上是否有障碍物 114 | auto [x1, y1] = one2two(p1); 115 | auto [x2, y2] = one2two(p2); 116 | return checkLine(x1, y1, x2, y2); 117 | } 118 | 119 | vector path; // 记录最优路径的点的一维坐标 120 | vector fa; 121 | set pointSet; 122 | 123 | // 随机生成点 124 | int randPoint(){ 125 | int x = rand() % rows; 126 | int y = rand() % cols; 127 | return two2one(x, y); 128 | } 129 | 130 | // 寻找pointSet中距离point最近的点 131 | int getNearestPoint(int point, set &pointSet){ 132 | int target = -1; 133 | double targetDis = 1e9 + 7; 134 | for(auto p : pointSet){ 135 | double distance = dis(point, p); 136 | if (distance < targetDis){ 137 | targetDis = distance; 138 | target = p; 139 | } 140 | } 141 | return target; 142 | } 143 | 144 | // 步长扩展点 145 | pair goStep(int curPoint, int dirPoint){ 146 | double distance = dis(curPoint, dirPoint); 147 | auto [curX, curY] = one2two(curPoint); 148 | auto [dirX, dirY] = one2two(dirPoint); 149 | // 比例关系 150 | int nxtX = curX + 1.0 * (dirX - curX) * stepDistance / distance; 151 | int nxtY = curY + 1.0 * (dirY - curY) * stepDistance / distance; 152 | int nxtPoint = two2one(nxtX, nxtY); 153 | // 扩展点不在障碍物，未出界，连线无障碍，非点集点 154 | return {valid(nxtX, nxtY) && checkLine(nxtX, nxtY, curX, curY) && pointSet.find(nxtPoint) == pointSet.end(), nxtPoint}; 155 | } 156 | 157 | // 重新选择父亲 158 | int getBestFatherPoint(int point, set& pointSet, vector& pointDistance){ 159 | int target = 0; 160 | double targetDis = 1e9 + 7; 161 | for(auto p : pointSet){ // 枚举点集的点 162 | double distance = dis(point, p); 163 | // 距离在方圆内，且从该点到起点距离更小 164 | if (distance < fatherDistance && checkLine(point, p) && pointDistance[p] + distance < targetDis){ 165 | targetDis = pointDistance[p] + distance; 166 | target = p; 167 | } 168 | } 169 | return target; 170 | } 171 | 172 | // 更新距离 173 | void update(int point, vector& pointDistance){ 174 | for(auto nxtPoint : edge[point]){ 175 | pointDistance[nxtPoint] = dis(point, nxtPoint) + pointDistance[point]; 176 | update(nxtPoint, pointDistance); 177 | } 178 | } 179 | 180 | // 重布线 181 | void rewrite(int point, set& pointSet, vector& pointDistance){ 182 | for(auto p : pointSet){ // 枚举点集的点 183 | double distance = dis(point, p); 184 | // 判断距离是否在方圆内，且之间连边无障碍物 185 | if (distance > rewriteDistance || !checkLine(point, p)) 186 | continue; 187 | // 如果距离更优 188 | if (pointDistance[p] > pointDistance[point] + distance){ 189 | // 更新距离 190 | pointDistance[p] = pointDistance[point] + distance; 191 | // 删除原先边 192 | edge[fa[p]].erase(p); 193 | fa[p] = point; 194 | // 建立新的边 195 | edge[point].insert(p); 196 | // 更新点p之后的点的最短距离 197 | update(p, pointDistance); 198 | } 199 | } 200 | } 201 | 202 | // 计算到终点距离，判断是否在终点附近 203 | bool nearTargetPoint(int point){ 204 | return dis(point, two2one(ex, ey)) < targetDistance; 205 | } 206 | 207 | void findPath(){ 208 | fa.resize(rows * cols); 209 | fill(fa.begin(), fa.end(), 0); 210 | // 距离起点的数据 211 | vector distance(rows * cols, 1e9 + 7); 212 | // 记录从父亲走向儿子的有向边 213 | edge.resize(rows * cols); 214 | int st = two2one(sx, sy); 215 | distance[st] = 0; 216 | int ed = 0; 217 | // 已扩展点集 218 | pointSet.insert(st); 219 | while(true){ 220 | // 随机采样点 221 | int samplePoint = randPoint(); 222 | // 在点集中找到距离采样点最近的点 223 | int nearstPoint = getNearestPoint(samplePoint, pointSet); 224 | // 以采样点方向以stepDistance步长得到新点，valid表示该点是否合法，即该点是否位于障碍物或超出界，或已在扩展点集里，或沿途有障碍物 225 | auto [valid, newPoint] = goStep(nearstPoint, samplePoint); 226 | if (!valid) 227 | continue; 228 | // 重新选择父亲点，选一条从st -> fatherPoint -> newPoint最近的fatherPoint 229 | int fatherPoint = getBestFatherPoint(newPoint, pointSet, distance); 230 | // 指定父亲 231 | fa[newPoint] = fatherPoint; 232 | // 更新距离 233 | distance[newPoint] = distance[fatherPoint] + dis(newPoint, fatherPoint); 234 | // 建立新有向边 235 | edge[fatherPoint].insert(newPoint); 236 | // 扩展点集合 237 | pointSet.insert(newPoint); 238 | // 重布线操作 239 | rewrite(newPoint, pointSet, distance); 240 | // 判断是否在终点附近 241 | if (nearTargetPoint(newPoint)){ 242 | ed = newPoint; 243 | break; 244 | } 245 | } 246 | // 记录路径 247 | for(int point = ed; point != st; point = fa[point]) 248 | path.push_back(point); 249 | path.push_back(st); 250 | reverse(path.begin(), path.end()); 251 | } 252 | 253 | // 以下函数未用到，因为一开始代码未分开，想着找完最短路就可以直接开始寻路，但后来发现无法通过编译（链接出错），因此复制了以下代码到Move.cpp中 254 | 255 | void setLookhead(int x){ 256 | l = x; 257 | } 258 | 259 | int curPoint = -1; 260 | 261 | int findNearestPoint(int x, int y){ 262 | int cc = curPoint; 263 | int cur = two2one(x, y); 264 | double ansdis = 1e9 + 7; 265 | curPoint = 0; 266 | for(size_t i = cc; i < path.size(); ++ i){ 267 | double distance = dis(cur, path[i]); 268 | if (distance < ansdis){ 269 | ansdis = distance; 270 | cc = i; 271 | } 272 | } 273 | // return one2two(path[curPoint]); 274 | return cc; 275 | } 276 | 277 | pair findLookheadPoint(int x, int y){ 278 | int id = findNearestPoint(x, y); 279 | if (id > curPoint) 280 | curPoint = id; 281 | int cur = two2one(x, y); 282 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 283 | ++ curPoint; 284 | } 285 | return one2two(path[curPoint]); 286 | } 287 | 288 | void clear(){ 289 | curPoint = -1; 290 | } 291 | 292 | void drawPoint(){ 293 | for(auto u : pointSet){ 294 | auto [y, x] = one2two(u); 295 | cv::rectangle(maze, cv::Point_(x, y), cv::Point_(x + 1, y + 1), cv::Scalar_(255, 0, 0), -1); 296 | } 297 | } 298 | 299 | // 绘制连边 300 | 301 | void drawNei(){ 302 | int st = two2one(sx, sy); 303 | for(auto u : pointSet){ 304 | if (u == st) 305 | continue; 306 | int v = fa[u]; 307 | auto [y, x] = one2two(u); 308 | auto [ny, nx] = one2two(v); 309 | cv::line(maze, cv::Point_(x, y), cv::Point_(nx, ny), cv::Scalar_(0,0,255), 1); 310 | } 311 | } 312 | 313 | // 绘制最短路 314 | 315 | void drawPath(){ 316 | if (path.empty()) 317 | findPath(); 318 | for(int i = 0; i < path.size() - 1; ++ i){ 319 | auto [y, x] = one2two(path[i]); 320 | auto [ny, nx] = one2two(path[i + 1]); 321 | cv::line(maze, cv::Point_(x, y), cv::Point_(nx, ny), cv::Scalar_(255,255,0), 3); 322 | } 323 | } 324 | 325 | // 保存图片 326 | 327 | void savePic(string s){ 328 | cv::imwrite(s.c_str(), maze); 329 | } 330 | 331 | // 保存路径 332 | 333 | void savePath(string s){ 334 | FILE* fp = fopen(s.c_str(), "w"); 335 | fprintf(fp, "%d %d %lu\n", rows, cols, path.size()); 336 | for(auto i : path) 337 | fprintf(fp, "%d\n", i); 338 | } 339 | 340 | 341 | }; 342 | -------------------------------------------------------------------------------- /RRT_planning/controllers/display/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2020 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | # CXX_SOURCES = Move.cpp ground.cpp 43 | # CXX_SOURCES = main.cpp 44 | ### 45 | ### ---- Compilation options ---- 46 | ### if special compilation flags are necessary: 47 | CFLAGS = -std=c++17 48 | ### 49 | ### ---- Linked libraries ---- 50 | ### if your program needs additional libraries: 51 | # INCLUDE = -I/opt/homebrew/opt/opencv/include/opencv4 52 | # LIBRARIES = -L/opt/homebrew/opt/opencv/lib -lopencv_gapi -lopencv_stitching -lopencv_alphamat -lopencv_aruco -lopencv_barcode -lopencv_bgsegm -lopencv_bioinspired -lopencv_ccalib -lopencv_dnn_objdetect -lopencv_dnn_superres -lopencv_dpm -lopencv_face -lopencv_freetype -lopencv_fuzzy -lopencv_hfs -lopencv_img_hash -lopencv_intensity_transform -lopencv_line_descriptor -lopencv_mcc -lopencv_quality -lopencv_rapid -lopencv_reg -lopencv_rgbd -lopencv_saliency -lopencv_sfm -lopencv_stereo -lopencv_structured_light -lopencv_phase_unwrapping -lopencv_superres -lopencv_optflow -lopencv_surface_matching -lopencv_tracking -lopencv_highgui -lopencv_datasets -lopencv_text -lopencv_plot -lopencv_videostab -lopencv_videoio -lopencv_viz -lopencv_wechat_qrcode -lopencv_xfeatures2d -lopencv_shape -lopencv_ml -lopencv_ximgproc -lopencv_video -lopencv_dnn -lopencv_xobjdetect -lopencv_objdetect -lopencv_calib3d -lopencv_imgcodecs -lopencv_features2d -lopencv_flann -lopencv_xphoto -lopencv_photo -lopencv_imgproc -lopencv_core 53 | ### 54 | ### ---- Linking options ---- 55 | ### if special linking flags are needed: 56 | ### LFLAGS = -s 57 | ### 58 | ### ---- Webots included libraries ---- 59 | ### if you want to use the Webots C API in your C++ controller program: 60 | ### USE_C_API = true 61 | ### 62 | ### ---- Debug mode ---- 63 | ### if you want to display the gcc command line for compilation and link, as 64 | ### well as the rm command details used for cleaning: 65 | VERBOSE = 1 66 | ### 67 | ###----------------------------------------------------------------------------- 68 | 69 | ### Do not modify: this includes Webots global Makefile.include 70 | null := 71 | space := $(null) $(null) 72 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 73 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 74 | -------------------------------------------------------------------------------- /RRT_planning/controllers/display/display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | using namespace webots; 15 | 16 | #define RED 0xBB2222 17 | #define GREEN 0x22BB11 18 | #define BLUE 0x2222BB 19 | 20 | const int lookDis = 20; 21 | 22 | inline pair real2pic(double X, double Y){ 23 | return {int((3 - X) / 6 * 800), int((Y + 2.25) / 4.5 * 600)}; 24 | } 25 | 26 | inline pair pic2real(int x, int y, int w, int h){ // 获取像素坐标系在演示坐标系下的坐标 27 | return {h * (1.0 * x / 800), w * (1.0 * y / 600)}; 28 | } 29 | 30 | class Path{ 31 | vector path; 32 | int l; 33 | int cols, rows; 34 | public: 35 | 36 | Path(){} 37 | Path(string s){ 38 | FILE * fp = fopen(s.c_str(), "r"); 39 | int tot = 0; 40 | fscanf(fp, "%d%d%d", &rows, &cols, &tot); 41 | while(tot--){ 42 | int x; 43 | fscanf(fp, "%d", &x); 44 | path.push_back(x); 45 | } 46 | } 47 | 48 | inline int two2one(int x, int y){ 49 | return x * cols + y; 50 | } 51 | 52 | inline pair one2two(int id){ 53 | return {id / cols, id % cols}; 54 | } 55 | 56 | inline double calcDis(int x, int y){ 57 | return sqrt(x * x + y * y); 58 | } 59 | 60 | inline double dis(int x1, int y1, int x2, int y2){ 61 | return calcDis(x2 - x1, y2 - y1); 62 | } 63 | 64 | inline double dis(int u, int v){ 65 | auto [x1, y1] = one2two(u); 66 | auto [x2, y2] = one2two(v); 67 | return calcDis(x2 - x1, y2 - y1); 68 | } 69 | 70 | void clear(){ 71 | curPoint = -1; 72 | } 73 | 74 | void setLookhead(int x){ 75 | l = x; 76 | } 77 | 78 | int curPoint = -1; 79 | 80 | int findNearestPoint(int x, int y){ 81 | int cc = curPoint; 82 | int cur = two2one(x, y); 83 | double ansdis = 1e9 + 7; 84 | curPoint = 0; 85 | for(size_t i = cc; i < path.size(); ++ i){ 86 | double distance = dis(cur, path[i]); 87 | if (distance < ansdis){ 88 | ansdis = distance; 89 | cc = i; 90 | } 91 | } 92 | // return one2two(path[curPoint]); 93 | return cc; 94 | } 95 | 96 | pair findLookheadPoint(int x, int y){ 97 | int id = findNearestPoint(x, y); 98 | if (id > curPoint) 99 | curPoint = id; 100 | int cur = two2one(x, y); 101 | while(curPoint + 1 < path.size() && dis(cur, path[curPoint]) < l){ 102 | ++ curPoint; 103 | } 104 | return one2two(path[curPoint]); 105 | } 106 | 107 | void drawpath(int w, int h, Display * display){ 108 | for (auto i : path){ 109 | auto [x, y] = one2two(i); 110 | auto [nX, nY] = pic2real(y, x, w, h); 111 | display->drawPixel(nX, nY); 112 | } 113 | } 114 | }; 115 | 116 | int main() 117 | { 118 | Path maze("../Move/example.txt"); 119 | maze.setLookhead(lookDis); 120 | 121 | 122 | Supervisor *su = new Supervisor(); 123 | int timeStep = (int)su->getBasicTimeStep(); 124 | Display *display = su->getDisplay("display"); 125 | Node * robot = su->getFromDef("Robot"); 126 | 127 | display->setOpacity(1); 128 | 129 | int width = display->getWidth(); 130 | int height = display->getHeight(); 131 | 132 | display->setColor(BLUE); 133 | maze.drawpath(width, height, display); 134 | display->setColor(RED); 135 | 136 | ImageRef* to_store = NULL; 137 | while (su->step(timeStep) != -1) 138 | { 139 | 140 | // 获取小车位置 141 | const double X = robot->getPosition()[0]; 142 | const double Y = robot->getPosition()[1]; 143 | const double Z = robot->getPosition()[2]; 144 | 145 | double real = 2; 146 | 147 | // 模拟行为 148 | 149 | auto [x, y] = real2pic(Y, X); 150 | 151 | auto [ry, rx] = pic2real(x, y, width, height); 152 | 153 | auto [nx, ny] = maze.findLookheadPoint(x, y); 154 | 155 | auto [nY, nX] = pic2real(nx, ny, width, height); 156 | 157 | if (to_store != NULL) // 恢复图片，去除原来的圆圈 158 | display->imagePaste(to_store, 0, 0); 159 | // 绘制目标点 160 | display->drawPixel(nX, nY); 161 | 162 | if (to_store != NULL) 163 | display->imageDelete(to_store); 164 | 165 | // 保存当前图片 166 | to_store = display->imageCopy(0, 0, width, height); 167 | 168 | display->setColor(GREEN); 169 | // 绘制圆圈 170 | display->drawOval(rx, ry, real, real); 171 | display->setColor(RED); 172 | 173 | } 174 | 175 | delete su; 176 | return 0; 177 | } 178 | -------------------------------------------------------------------------------- /RRT_planning/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Lanly109/Webots-Homework/1e11af771062c5a85cfd118604775f6c59f85316/RRT_planning/maze.png -------------------------------------------------------------------------------- /RRT_planning/protos/car.wbo: -------------------------------------------------------------------------------- 1 | #VRML_OBJ R2021a utf8 2 | Robot { 3 | translation -6.63565e-17 2.01005 0.039392 4 | rotation -1 6.304952387026931e-13 1.6220961513886875e-13 0.0007995434308963864 5 | children [ 6 | Camera { 7 | translation 0.109025 0.0399999 0.0163641 8 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 9 | children [ 10 | Transform { 11 | translation 0 0 0.02 12 | rotation 1 0 0 1.5708 13 | children [ 14 | Shape { 15 | geometry Cylinder { 16 | height 0.03 17 | radius 0.01 18 | } 19 | } 20 | ] 21 | } 22 | ] 23 | name "camera_left" 24 | antiAliasing TRUE 25 | } 26 | Camera { 27 | translation 0.109025 -0.0400001 0.0163644 28 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 29 | children [ 30 | Transform { 31 | translation 0 0 0.02 32 | rotation 1 0 0 1.5708 33 | children [ 34 | Shape { 35 | geometry Cylinder { 36 | height 0.03 37 | radius 0.01 38 | } 39 | } 40 | ] 41 | } 42 | ] 43 | name "camera_right" 44 | antiAliasing TRUE 45 | } 46 | Camera { 47 | translation 0.109025 0 0.0163644 48 | rotation -0.4469680445966859 0.4469680445966859 0.7748800773144385 -1.8231153071795863 49 | children [ 50 | Transform { 51 | translation 0 0 0.02 52 | rotation 1 0 0 1.5708 53 | children [ 54 | Shape { 55 | geometry Cylinder { 56 | height 0.03 57 | radius 0.01 58 | } 59 | } 60 | ] 61 | } 62 | ] 63 | name "camera_mid" 64 | antiAliasing TRUE 65 | } 66 | HingeJoint { 67 | jointParameters HingeJointParameters { 68 | position 3.965440000000003 69 | axis 0 1 0 70 | anchor 0.06 0.07 0 71 | } 72 | device [ 73 | RotationalMotor { 74 | name "motor1" 75 | maxVelocity 100 76 | } 77 | ] 78 | endPoint Solid { 79 | translation 0.06 0.07 0 80 | children [ 81 | DEF WHEEL Shape { 82 | appearance PBRAppearance { 83 | baseColor 0.5 1 1 84 | } 85 | geometry Cylinder { 86 | height 0.02 87 | radius 0.04 88 | } 89 | } 90 | ] 91 | boundingObject USE WHEEL 92 | physics Physics { 93 | } 94 | } 95 | } 96 | HingeJoint { 97 | jointParameters HingeJointParameters { 98 | position 3.965440000000003 99 | axis 0 1 0 100 | anchor -0.06 0.07 0 101 | } 102 | device [ 103 | RotationalMotor { 104 | name "motor2" 105 | maxVelocity 100 106 | } 107 | ] 108 | endPoint Solid { 109 | translation -0.06 0.07 0 110 | children [ 111 | Shape { 112 | appearance PBRAppearance { 113 | baseColor 0.5 1 1 114 | } 115 | geometry Cylinder { 116 | height 0.02 117 | radius 0.04 118 | } 119 | } 120 | ] 121 | name "solid(1)" 122 | boundingObject USE WHEEL 123 | physics Physics { 124 | } 125 | } 126 | } 127 | HingeJoint { 128 | jointParameters HingeJointParameters { 129 | position 3.965440000000003 130 | axis 0 1 0 131 | anchor 0.06 -0.07 0 132 | } 133 | device [ 134 | RotationalMotor { 135 | name "motor3" 136 | maxVelocity 100 137 | } 138 | ] 139 | endPoint Solid { 140 | translation 0.06 -0.07 0 141 | children [ 142 | Shape { 143 | appearance PBRAppearance { 144 | baseColor 0.5 1 1 145 | } 146 | geometry Cylinder { 147 | height 0.02 148 | radius 0.04 149 | } 150 | } 151 | ] 152 | name "solid(2)" 153 | boundingObject USE WHEEL 154 | physics Physics { 155 | } 156 | } 157 | } 158 | HingeJoint { 159 | jointParameters HingeJointParameters { 160 | position 3.965440000000003 161 | axis 0 1 0 162 | anchor -0.06 -0.07 0 163 | } 164 | device [ 165 | RotationalMotor { 166 | name "motor4" 167 | maxVelocity 100 168 | } 169 | ] 170 | endPoint Solid { 171 | translation -0.06 -0.07 0 172 | children [ 173 | Shape { 174 | appearance PBRAppearance { 175 | baseColor 0.5 1 1 176 | } 177 | geometry Cylinder { 178 | height 0.02 179 | radius 0.04 180 | } 181 | } 182 | ] 183 | name "solid(3)" 184 | boundingObject USE WHEEL 185 | physics Physics { 186 | } 187 | } 188 | } 189 | DEF BODY Shape { 190 | appearance PBRAppearance { 191 | } 192 | geometry Box { 193 | size 0.2 0.1 0.05 194 | } 195 | } 196 | ] 197 | boundingObject USE BODY 198 | physics Physics { 199 | density -1 200 | mass 1 201 | } 202 | controller "LineFollowing" 203 | } --------------------------------------------------------------------------------