├── .gitignore ├── CITATION.cff ├── DRL-PID ├── buffer.py ├── data │ └── train │ │ ├── map_0 │ │ ├── actor_loss_list.txt │ │ ├── critic_loss_list.txt │ │ ├── score_data.txt │ │ ├── success_record.txt │ │ └── value_loss_list.txt │ │ ├── rate_plot.py │ │ └── score_plot.py ├── env_feedback.py ├── line_follower.py ├── main.py ├── model_testing.py ├── move_car.py ├── networks.py ├── sac_torch.py ├── tmp │ └── sac │ │ ├── actor_sac │ │ ├── critic_1_sac │ │ ├── critic_2_sac │ │ ├── target_value_sac │ │ └── value_sac └── utils.py ├── Image ├── car.png ├── env.jpg ├── framework.png ├── map_0.png ├── score_plot.png ├── searching algorithm.png ├── structure.png └── success_rate.png ├── LICENSE.md ├── README.md ├── my_ground_plane ├── materials │ ├── scripts │ │ └── my_ground_plane.material │ └── textures │ │ └── MyImage.png ├── model.config └── model.sdf └── summit_description ├── CMakeLists.txt ├── Maps └── path.world ├── launch ├── pioneer.launch └── summit.launch ├── meshes ├── base │ └── summit_base.dae ├── p3at_meshes │ ├── Coordinates │ ├── axle.stl │ ├── back_sonar.stl │ ├── chassis.stl │ ├── front_sonar.stl │ ├── left_hubcap.stl │ ├── right_hubcap.stl │ ├── top.stl │ └── wheel.stl └── wheels │ ├── omni_wheel_1.dae │ └── omni_wheel_2.dae ├── msg └── EnvInfo.msg ├── package.xml ├── robot ├── pioneer3at.urdf.xacro └── summit.urdf.xacro ├── script ├── PID │ ├── line_detect.py │ └── pid_tracker.py └── gotopoint │ ├── goToPoint.py │ └── goToPoint_omni.py ├── srv └── StartUp.srv └── urdf ├── base ├── summit_base.gazebo.xacro └── summit_base.urdf.xacro ├── camera ├── camera.urdf.xarco └── model.sdf ├── pioneer3at.urdf └── wheels └── omni_wheel.urdf.xacro /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | .vscode 3 | **.pyc 4 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Ruan" 5 | given-names: "Yudi" 6 | orcid: "https://orcid.org/0000-0001-8101-0697" 7 | title: "DRL-PID" 8 | version: 1.0 9 | doi: 10.5281/zenodo.7338823 10 | date-released: 2021.6.7 11 | url: "https://github.com/blakcapple/DRL-PID.git" -------------------------------------------------------------------------------- /DRL-PID/buffer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class ReplayBuffer(): 4 | def __init__(self, max_size, input_shape, n_actions): 5 | self.mem_size = max_size 6 | self.mem_cntr = 0 7 | self.state_memory = np.zeros((self.mem_size, *input_shape)) 8 | self.new_state_memory = np.zeros((self.mem_size, *input_shape)) 9 | self.action_memory = np.zeros((self.mem_size, n_actions)) 10 | self.reward_memory = np.zeros(self.mem_size) 11 | self.terminal_memory = np.zeros(self.mem_size, dtype=np.bool) 12 | 13 | def store_transition(self, state, action, reward, state_, done): 14 | index = self.mem_cntr % self.mem_size 15 | 16 | self.state_memory[index] = state 17 | self.new_state_memory[index] = state_ 18 | self.action_memory[index] = action 19 | self.reward_memory[index] = reward 20 | self.terminal_memory[index] = done 21 | 22 | self.mem_cntr += 1 23 | 24 | def sample_buffer(self, batch_size): 25 | max_mem = min(self.mem_cntr, self.mem_size) 26 | 27 | batch = np.random.choice(max_mem, batch_size) 28 | 29 | states = self.state_memory[batch] 30 | states_ = self.new_state_memory[batch] 31 | actions = self.action_memory[batch] 32 | rewards = self.reward_memory[batch] 33 | dones = self.terminal_memory[batch] 34 | 35 | return states, actions, rewards, states_, dones 36 | 37 | 38 | -------------------------------------------------------------------------------- /DRL-PID/data/train/map_0/score_data.txt: -------------------------------------------------------------------------------- 1 | [6.867556078275824, 6.717600365858392, 7.7588388986791905, 7.741041054228667, 6.74062300395588, 5.885996199476857, 7.625130707729735, 6.726391857663739, 7.772828734473075, 7.669360345889794, 6.6253101036120245, 6.8037145729401285, 7.841465508332099, 7.723725783891412, 6.762228782530643, 6.908003439971681, 7.6276045755075526, 6.819958505103456, 6.818650779652694, 6.122035597356431, 6.65078860739354, 9.45553517353104, 14.393699773221119, 2.8579384623737836, 3.509147741873683, 1.5733938417988282, 3.583715433986775, 5.135103897647177, 5.965882761310386, 5.997948206980352, 7.3680788312990195, 9.398948743252783, 7.744129766065397, 12.37835130424373, 6.834224978963681, 11.984737404648065, 6.982854659310799, 8.005794273839284, 7.663792081853549, 5.756209297904851, 2.8371340921252726, 8.652824626592352, 8.943897456599139, 12.062497539924237, 8.754117293683144, 8.978548975345678, 6.888209840667363, 7.117202614208182, 7.981315174258436, 7.8098877755101554] 2 | [8.00480857159392, 7.6405268542461116, 8.175992762310477, 4.474518578419481, 7.690615621749082, 8.341972382673308, 8.71039821235264, 6.7905219059753, 8.072207137342653, 8.792250206938418, 10.104300175764852, 9.051550212323534, 14.340802915381772, 6.69803304603526, 59.38132669486137, 66.82438424921173, 70.31461298514341, 38.2920007290185, 82.25663541702308, 81.77776082775416, 81.62352266918583, 76.25748647034156, 5.3197870318925276, 30.725723155587012, 4.412910986323952, 1.2950103028902804, 34.026823186596644, 30.833575323665173, 29.407098347913376, 4.556186789626292, 30.104018572837674, 26.308230153567624, 82.24863148136245, 67.55982500654211, 63.48643541995678, 47.26634455981157, 24.62725932188107, 81.55574599547724, 25.4358598813118, 14.475733646339688, 9.758262793307724, 6.527942830697267, -1.5075085372468013, 7.877577458266469, 14.48671784081543, 9.568880982410796, 8.723530170384542, 7.2042864892767025, 7.350703667434278, 8.875885566094839] 3 | [6.051764078729931, 8.478766764953296, 6.56116924227034, 8.52535768933666, 7.454604423890707, 10.400088587835338, 9.435561616971357, 6.154958553147736, 7.580419335522912, 6.800539759211601, 7.346346494495251, 8.982648791090615, 3.838482012072058, 5.067911508043121, 3.551738387690241, 6.448236093173314, 2.6526137732942257, 3.5916539221493853, 5.870148588278024, 0.7077164464810606, 3.5041087990276854, 7.277671917562081, 5.846011388114542, 8.484613958513453, 2.41927623904527, 4.670259740199846, 4.758199165206049, 6.630728060149734, 6.983172356351888, 6.728278088143696, 5.940445556046416, 5.5818033792121575, 2.2204985184745993, 7.881240995161155, 6.749715122593527, 2.8168114352154667, 6.402163360600884, 5.586916088621489, 5.713279427443668, 5.805618777120927, 3.714092741889427, 1.7511061792751859, 1.883769219025405, 1.5905557046473282, 4.601448618796503, 5.594807215064462, 1.3766091700304992, 2.6224478083749982, 3.9240324795640866, 2.8746691916791676] 4 | [5.753261910532519, 5.518959641976789, 3.4353491582469626, 0.24139125690858165, 1.7577517656484325, 2.8772666946385215, 5.838115483870203, 6.012980778593574, 5.95346097890941, 1.6931124443131464, 2.8026633648620587, 0.3784705778554551, 1.6005365899623882, 2.6090589694876947, 2.800026608268409, 6.004710566133474, 3.8633607042915727, 4.5443870920458505, -0.1673682510408554, 4.695668743009648, 5.0620832970627845, 3.352974950131971, 2.299309745720289, 0.6019031988238401, 3.710595685582426, 1.5509561599775772, 0.6393649509447146, 0.04354136473530712, 0.61635832692017, -1.8953878344911388, 5.056132257413076, 2.596429242229357, -1.7829315670153094, -0.24749983393475183, -0.40815948463152907, -0.21000629073000887, 2.573565423119037, 4.096091257684023, -0.4830634518126846, -1.3880315177667701, 0.23244625484458048, 6.087217672066243, 0.13056099720207204, -0.3737623097224452, 2.547024419140138, -1.3282211539820619, 2.0423221569519416, 1.6980696989601478, -1.7145239863148092, -0.3740469544078131] 5 | [-0.495104258727606, 0.4480487389223615, -0.9833736583613701, 7.042807345851806, 1.3663079953634956, 2.5553257541324896, 5.832580532506249, 4.4038142109977585, -0.42533558910383196, 0.8625462170673934, -1.8871120614278762, 3.2439894596951913, 3.5291155799351195, 0.004347090452180069, 4.780974146467091, 0.6890066269627377, 3.889253059480751, 4.414301004188994, -1.7285264699422678, 0.488165291991014, 0.12729200472459912, 6.8159978929371, 3.112428716551454, 0.5642644026545591, 2.62773338680213, -0.9755554687292429, 10.048832415843608, 3.4635876517484103, 7.1954444494686705, 4.605930704071724, 1.5195929883417723, 3.409669233834796, 5.629371347337308, 4.845408750219423, 6.237293022379291, 4.753249922177197, 5.815410589467504, 6.50039792163216, 4.549684607116493, 2.4250992038966235, 5.433062985109059, 5.5350388347904715, 6.9637570358632, 4.6370454155991006, 4.564922058185951, 7.212889389371433, 5.280831739119025, 6.72590072801453, -0.2841192286468832, 5.565189136101504] 6 | [8.091838456206338, 6.5075406253629815, -0.3612589861300499, 6.690379264274082, 12.030640150033488, 6.400110167116882, 6.586276100486547, 3.3924615286507773, 7.269236845149196, 1.4666689849779075, 3.94002795412465, 5.65628101250093, 7.802079002003971, 9.151814899475994, 7.574636893249819, 9.595514945089588, 5.78724316358822, 6.033069335331852, 14.683865052537044, 11.191091912640847, 9.838260117075608, 18.00840484525512, 5.447493568685799, 6.615403449006708, 9.040244416093968, 14.41458243980227, 16.009500323329277, 8.863573191443491, 13.546377041767993, 15.858084575752343, 17.382821403571768, 9.54807491314359, 45.530729181417705, 16.31787885442205, 77.81722071471341, 12.426234588608473, 76.0867562347254, 77.1245979211262, 73.874914820987, 74.28538044389516, 76.30342592563103, 78.30349428150114, 80.09360302470058, 79.98414243521347, 77.19660650365296, 78.27688267095556, 73.3635604174157, 78.03040682568844, 77.29757479005019, 80.51632221738826] 7 | [74.78994658593308, 74.06736092044932, 79.60936921750955, 80.71459973526278, 78.88274176442557, 78.94171406036756, 77.16928724193885, 78.28375584279704, 80.68688876193792, 80.7713598287882, 77.46918223593586, 64.49284068642064, 79.50094665705898, 81.49435194200387, 79.46349381959588, 79.20234639252465, 63.87142704768962, 74.68183474178096, 78.52705703863386, 81.57911990028322, 81.59535103476861, 81.32054413112606, 81.38286523943411, 79.86577767629157, 78.06544146413063, 78.18621849741785, 81.86864335321644, 81.85626595352021, 80.37176734000808, 79.86130691527222, 80.69394827435275, 80.78929994910322, 80.97738845201515, 79.16398475985588, 76.13435073694414, 81.48485581901484, 81.54680759220605, 81.18895802904146, 80.72888717732015, 71.48343592254409, 81.9276010803896, 81.16719810724042, 81.02187693370573, 80.8337950776966, 75.78418300501777, 81.43302401029965, 80.8039289006405, 79.04563365032972, 79.99657990802991, 76.70906860516024] 8 | [81.5389786747763, 80.08695341489576, 77.3585609113266, 78.12236429510169, 77.26965963767576, 81.71373508703391, 80.79312792172684, 81.23345457591434, 79.52721701344325, 79.64423212941954, 82.09925569150897, 81.46619476285464, 79.91632697656561, 79.42328364952996, 81.37821576004526, 81.81382764060912, 81.57825434409651, 79.72345628177102, 79.12719891019205, 81.3768080788755, 81.09858099099085, 81.30511140665926, 81.66137446495783, 80.26448871208754, 81.16215592573835, 80.17703912290823, 80.36079711360104, 81.88681778652365, 80.53593962024902, 80.88661941763804, 80.35838623717275, 81.41792412408354, 80.43268983174866, 80.4212362797608, 78.43810002189474, 81.71860620304426, 81.63447367819593, 80.56530730152338, 80.54463116012613, 78.06872062455085, 81.32263366732974, 81.59152996691857, 81.58560397482877, 79.70993036308212, 77.82854439862228, 79.04069541954927, 81.88363892549617, 81.30528115476122, 80.41296485385972, 81.18197718499069] 9 | [81.92938002837207, 81.35066032722781, 79.04580180109629, 78.88218535883465, 78.52851362144484, 81.90709934956678, 78.98263914600899, 79.75544666220188, 74.0982786654088, 76.62923538173952, 81.2087058977284, 78.30864296907046, 81.36999775183594, 81.53112998788184, 77.39175792922408, 78.94451862326217, 74.86323964114932, 82.07195933951353, 80.67945313235288, 80.90079622134192, 78.84045608736729, 78.7146397084669, 81.75391214826206, 80.60788730483819, 81.24716920228494, 81.04513019225539, 77.97782507678042, 74.83906276924016, 82.00162304301972, 81.51988400178477, 79.94491458464636, 80.10934658610392, 65.4518491709504, 77.75988175810835, 82.02474639357723, 82.052658070351, 81.56041950783701, 79.96769291127231, 77.73516682797434, 81.96728126832699, 80.86951721452381, 81.64760993661284, 81.066232301517, 79.34273337840054, 81.37823822642339, 80.26024885498616, 81.8991424653715, 81.45873061351675, 81.89790364566022, 76.87695983166213] 10 | [75.58093800072189, 76.92898807698839, 72.64513297911164, 81.70165663136507, 81.96818018146917, 80.46176629946362, 77.35742898621764, 77.39774945474878, 77.73837444239777, 76.23002091278413, 81.15457208674536, 81.46657045551831, 81.65153416423738, 81.18170148608935, 78.736787135643, 79.14661536108122, 80.0535378975448, 82.05582454998817, 81.85499147318126, 81.15608138849719, 79.6681749439619, 80.3261563988034, 82.49995882756286, 81.9648575235421, 80.7962380836972, 82.25893354058755, 81.1766957099154, 80.70288598785461, 80.34817520528755, 81.07303506104984, 80.35667151512969, 80.3283135805835, 81.09491361983497, 80.43910242461816, 73.08261376711356, 82.36570425264709, 81.56290655259775, 80.83113967648775, 80.79008610494863, 79.98696850381751, 77.17210003796703, 78.39905986379587, 82.29732911466765, 81.34185546477929, 78.56212695410275, 80.38010310414401, 81.80332032433425, 75.89236767615101, 81.2871870739091, 82.37611057721656] 11 | [81.81266634928409, 79.51708912320663, 78.64218463425729, 80.05436146383313, 81.91591983760087, 80.8050495922965, 80.83621663418903, 80.33912141375022, 81.211658181776, 79.69495061494334, 81.37444919242257, 80.9558110746065, 79.28343342208622, 80.20212789463928, 75.88314614850007, 77.74753753299089, 82.48371193606121, 81.7476356514126, 81.89368872979314, 81.63767849926727, 80.72969118202126, 78.51028269520368, 79.29543957556503, 81.9180450675434, 81.60275925344102, 81.6566337399122, 82.00542077159372, 79.9310976632069, 79.99207362600755, 77.8456617573282, 81.79472807692699, 80.31001385918808, 81.82243744652294, 81.35643547774607, 79.90376860783968, 80.98753566415375, 81.03521027541986, 81.97530779804167, 81.79925911723929, 82.14217340843366, 81.26523210102751, 80.98876729686903, 81.76051829099724, 82.6475188037927, 80.91983479946535, 81.09114530214922, 80.58104770550369, 81.28383137088012, 80.04354702548427, 82.31804523553926] 12 | [81.84318855967453, 81.03671820212531, 82.15817487714514, 81.27207673709592, 76.03352930485376, 82.33238763922468, 80.95823464052381, 81.16375796123069, 81.45515245069407, 81.48678196437473, 80.23039555894135, 80.62560558103455, 81.64042641677841, 81.23229772367696, 80.75111472535738, 80.51197145711211, 81.89697723963795, 81.94178157258158, 81.34588466807425, 80.57731740600845, 81.9689078301901, 80.05753954662464, 80.04998619097967, 79.39166281899361, 80.58130044961358, 80.05831393513651, 80.23614270946503, 79.32604356010684, 81.0351646032981, 80.40855549906566, 81.18985220779741, 80.97181282935904, 78.9115652327661, 79.4265124776002, 78.29703108348042, 80.99204890323195, 81.18076985009841, 80.78251686393742, 80.85106789465381, 81.031239866264, 79.82092578638101, 79.78991902864065, 81.68789586573482, 81.47024361845678, 81.7767509291831, 80.22806997171976, 81.34257641694272, 80.37510995709208, 81.60865058326756, 81.96717582557604] 13 | [82.23646059357188, 80.57498003080825, 77.10559723591626, 79.99076359238799, 81.52118563240128, 82.19888908916428, 81.67245255606983, 78.89710878553235, 82.00165570205388, 66.68544094244004, 80.61553173443764, 82.26772648862054, 82.13491521125391, 81.96897172726962, 80.59966918602511, 81.34092181498114, 75.96637355794843, 78.69351948420994, 81.38052608605105, 81.86551180595437, 80.21703189290622, 80.49401181732995, 79.23298436925427, 82.50856247351473, 81.36904598518942, 81.8810373639346, 78.81764860162593, 76.59429836120525, 81.53006272093059, 81.6874291014744, 81.8881360861094, 80.32474509826282, 80.25390505186787, 77.32633748393832, 80.23314336834788, 82.16926102096828, 80.9086970531988, 79.03139734523677, 80.97223889561081, 81.47513619377595, 81.8706158898626, 81.69862216518781, 80.62495110432246, 78.89080573965578, 79.63163591597711, 79.81636986892148, 80.37561632691909, 81.79552053726084, 80.95395798556223, 81.1476324580965] 14 | [80.75258473379706, 78.82891252397759, 79.86140951113555, 77.12630241672453, 80.78416511509775, 80.02559802448077, 79.04637085231879, 81.46779380839646, 81.47019455207864, 79.33652631473802, 80.65634246167699, 80.87909536821914, 78.84247445260608, 80.81293624487348, 73.76712544528117, 78.02439794937122, 80.26453331144137, 81.07516246980285, 79.16616267119325, 80.25548431670076, 79.45654211062612, 75.70673819676176, 81.04798361334859, 80.84034130751596, 80.77810445916869, 78.90352595435249, 80.88972925044436, 76.80085967132227, 81.02627029270269, 81.28143191598853, 79.8705833668533, 78.78904927944784, 76.72754493803802, 76.11987202141574, 81.19689917810304, 81.2328663396885, 79.05692557310084, 79.52882854106834, 80.34993771507601, 79.57583799302773, 79.7689782653579, 80.20766732213829, 81.03230315263126, 80.7593364762813, 80.60579561950567, 79.73593354597604, 79.87224760312543, 79.88069033446028, 81.52994763772328, 81.15059968622285] 15 | [79.52292939191157, 77.24399675307541, 80.09276271521004, 81.53986560727398, 81.07875386227924, 81.79980952329912, 80.22955295162711, 80.73750449863306, 80.3037506522034, 79.16612154911284, 79.86890853103087, 79.80546940320056, 80.37978529108695, 79.60623741692362, 81.72296765239712, 81.28907495072124, 78.41612886060905, 81.46140577159117, 80.7194028503759, 79.62902409459898, 79.78319609492239, 79.22803399942626, 79.54985602234844, 81.52333937208246, 82.0138801594642, 79.88933277405044, 81.04118401900654, 79.44399350242087, 77.08657099536194, 81.41886725639507, 81.25905091962308, 79.69179343962988, 79.0905035683239, 78.43774872447206, 80.36788207774052, 79.83392324127192, 81.23776175708939, 79.1838982362966, 80.7554629846835, 80.51367574229671, 75.0528426672014, 80.8572573603242, 80.45127155228177, 81.04281349165474, 79.0494016760408, 80.41112485027955, 78.17416655076859, 78.74580242062626, 80.11589061802411, 81.86040819456075] 16 | [81.86638376334056, 79.6348436795128, 79.48297354278937, 76.29359540078292, 78.86572110104441, 81.1675570123139, 80.4416538208477, 80.9331597010058, 81.11360720164521, 80.49009816525403, 78.68317272646469, 77.97765872203762, 80.69134250821955, 80.42885292232991, 78.5597832178548, 80.11092587775751, 75.62117647031549, 80.5275319020374, 79.77917954109861, 79.28760411550489, 80.52940771137091, 78.8598878400786, 71.72730087258186, 80.37031942746007, 80.58583715362883, 81.38124334511953, 80.203907373936, 79.69116612207941, 77.8677898614343, 79.38577111901535, 77.09967519862656, 81.12776063087055, 80.59788438219951, 81.0086995492155, 79.3232168403936, 79.99961368098553, 77.8787394355569, 80.79374680745656, 80.43092709732785, 80.81660938331193, 80.93115889829464, 79.0441611411738, 79.02928940594893, 78.73890050230081, 80.48232932556766, 79.72814930975049, 79.49032976198154, 81.03076632655534, 76.72768107231354, 77.97013959717916] 17 | [81.58952148455097, 81.71564012146193, 80.52993651973944, 80.72761772119432, 80.53691727917828, 77.90751263496384, 79.57713218558975, 81.37105280742237, 81.02954250430525, 80.6121821758427, 76.92705219008388, 80.19147692897853, 78.99986289374851, 81.9133917614012, 81.029813564666, 81.52427845951148, 79.47803259672125, 80.41817214647537, 79.6902882494342, 79.30533632706627, 80.62774990150719, 81.79752524638893, 81.1768763592793, 80.44939086371896, 78.40142940044869, 75.30949726637523, 76.88334884435206, 81.1600100761681, 79.67794975971854, 77.57491114795212, 79.84954924822371, 76.77073933713706, 76.81150397899138, 80.7923350657346, 80.26412003987792, 78.99060741919028, 80.01190516381757, 81.3294569760656, 76.57354521633312, 78.03746757100534, 80.68757349913999, 80.86911362234083, 81.67525887887265, 80.79002100816837, 78.13873791490013, 64.51747660988269, 80.62169946178682, 78.94923498243223, 80.09949900695314, 80.10413710702434] 18 | [78.09808754446141, 75.6968951791364, 81.26283104472209, 79.24785414272475, 79.83111489311712, 78.48397584436864, 77.5972198296277, 78.29733149908417, 80.87170748046339, 81.18146647036566, 79.47694837129445, 80.09672204999728, 79.19052815841944, 72.87041141541059, 79.27617472966212, 80.82481057522035, 80.92804880779347, 80.10283653903068, 80.74715402018764, 79.35953542036441, 79.63845026972061, 81.04357460999448, 81.14644056886186, 80.76927391676381, 80.33077720044278, 80.20838338743057, 79.7697113376866, 77.79092009912516, 79.97323487997826, 80.65924913376581, 81.20082452464887, 80.07298388472265, 79.87565684500296, 78.75780999193522, 79.41490578271075, 80.14071183611686, 80.52174797969917, 78.33461718295861, 80.30316047803464, 78.96262216015623, 80.55415423915532, 80.79879224353667, 80.42107352264738, 79.5084190148643, 79.23862896431861, 77.6531717043113, 76.74816842049435, 81.526055176063, 80.6232351255664, 80.48559225986222] 19 | [80.11422510380135, 79.4898091212621, 77.46731640203662, 78.06533969084364, 80.68873169211382, 81.21822049108958, 80.16863409083123, 79.61213208856023, 78.70299271054505, 77.73428486161964, 81.2013519106295, 80.71598331933006, 80.657192678501, 81.042878375518, 80.14258251003126, 77.36253167861967, 73.8368968217026, 75.00814568317664, 79.91040286313054, 81.98702741956255, 79.81472033976054, 80.65148775952272, 78.3848252609474, 77.51278537492135, 80.05239625354295, 81.26877711056629, 80.70237124385443, 79.32824037756544, 78.43113249766897, 78.96923057828145, 80.30864343426077, 80.12633098452825, 80.04736482182828, 79.28022152683224, 78.64766874135347, 78.70466247837851, 77.7913818291887, 80.06695284763697, 80.30225037998787, 80.05028971433529, 80.71837519625846, 81.05741904098765, 72.35517023035082, 77.61049632285876, 80.5112427866439, 80.26894901219805, 80.80302933022391, 79.51060504182986, 78.32820181892335, 78.64547111075574] 20 | [77.9245900803311, 81.12874279324483, 81.40136193759932, 79.7728751097854, 76.78506314448687, 77.54140650757756, 79.55164425008405, 80.82928852683006, 80.07618621750878, 78.81863887176013, 80.35658564431829, 77.42273606020794, 79.77671964585149, 80.85613698098747, 81.03505036543196, 80.19790268512547, 79.5526583073926, 78.88363709028795, 75.96729337689959, 79.27337397132513, 81.8613991260238, 80.24821036988592, 80.26788926531627, 78.72436991905403, 78.85030145528565, 76.71983604363895, 78.53844810834944, 80.92275634616216, 78.74748044181088, 79.73723179445916, 80.65185681057231, 78.36084302675545, 80.55019180338847, 80.13613218227658, 81.22143812635098, 78.77900520995942, 81.09662587032591, 79.19123666216237, 79.24775618892656, 80.01964990041148, 80.72032216732114, 80.69759240737753, 77.87084958125352, 80.20828556324257, 77.98851900034829, 78.82106994434749, 81.45899889381575, 80.77859587424011, 80.31136744977462, 80.19535285301629] 21 | -------------------------------------------------------------------------------- /DRL-PID/data/train/map_0/success_record.txt: -------------------------------------------------------------------------------- 1 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 2 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 3 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 4 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 5 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 6 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 7 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 8 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 9 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 10 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 11 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 12 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 13 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 14 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 15 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 16 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 17 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 18 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 19 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 20 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] 21 | -------------------------------------------------------------------------------- /DRL-PID/data/train/rate_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | def plot_learning_curve(x, scores, figure_file,average_time=100): 6 | running_avg = np.zeros(len(scores)) 7 | for i in range(len(running_avg)): 8 | running_avg[i] = np.mean(scores[max(0, i-average_time):(i+1)]) 9 | plt.plot(x, running_avg) 10 | plt.savefig(figure_file) 11 | 12 | 13 | figure_file = 'success_rate.png' 14 | file_name = 'success_record.txt' 15 | 16 | def plot_data(file_name,figure_file,average_time=100): 17 | scores = [] 18 | with open(file_name) as f : 19 | file = f.readlines() 20 | # print(file) 21 | for index, x in enumerate(file): 22 | x = x.strip() 23 | x = x.strip('[]') 24 | x = x.split(", ") 25 | scores.append(x) 26 | # print(scores) 27 | y=[] 28 | for i in range(len(scores)): 29 | for j in range(len(scores[i])): 30 | y.append(scores[i][j]) 31 | y = np.array(y) 32 | y = y.astype(float) 33 | x = [i for i in range(len(y))] 34 | plot_learning_curve(x,y,figure_file,average_time) 35 | 36 | plot_data(file_name,figure_file,1) 37 | 38 | # print(b) 39 | -------------------------------------------------------------------------------- /DRL-PID/data/train/score_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | def plot_learning_curve(x, scores, figure_file,average_time=100): 6 | running_avg = np.zeros(len(scores)) 7 | for i in range(len(running_avg)): 8 | running_avg[i] = np.mean(scores[max(0, i-average_time):(i+1)]) 9 | plt.plot(x, running_avg) 10 | plt.savefig(figure_file) 11 | 12 | 13 | figure_file = 'score_plot.png' 14 | file_name = 'score_data.txt' 15 | 16 | def plot_data(file_name,figure_file,average_time=100): 17 | scores = [] 18 | with open(file_name) as f : 19 | file = f.readlines() 20 | for index, x in enumerate(file): 21 | x = x.strip() 22 | x = x.strip('[]') 23 | x = x.split(", ") 24 | scores.append(x) 25 | scores = np.array(scores) 26 | scores = scores.astype(float) 27 | y = np.zeros((scores.shape[0])*scores.shape[1]) 28 | for i in range(0, scores.shape[0]): 29 | for j in range(scores.shape[1]): 30 | y[(i)*50+j] = scores[i][j] 31 | 32 | x = [i for i in range(len(y))] 33 | plot_learning_curve(x,y,figure_file,average_time) 34 | 35 | plot_data(file_name,figure_file,1) 36 | 37 | # print(b) 38 | -------------------------------------------------------------------------------- /DRL-PID/env_feedback.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | import cv2 4 | from cv_bridge import CvBridge, CvBridgeError 5 | from sensor_msgs.msg import Image 6 | from summit_description.msg import EnvInfo 7 | import numpy as np 8 | from summit_description.srv import StartUp, StartUpResponse 9 | import time 10 | import math 11 | 12 | global img_height,img_width 13 | img_height = 72 14 | img_width = 128 15 | 16 | 17 | class EnvFeedback: 18 | 19 | def __init__(self): 20 | 21 | self.bridge = CvBridge() 22 | 23 | self.active = False 24 | 25 | self.image_sub = rospy.Subscriber("iRobot/camera/image_raw", Image, 26 | self.camera_callback) 27 | 28 | self.detect_pub = rospy.Publisher("env_feedback", EnvInfo, queue_size=10) 29 | 30 | self.detect_msg = EnvInfo() 31 | 32 | self.pub_rate = rospy.Rate(10) 33 | 34 | self.step = 0 35 | 36 | self.active = False 37 | 38 | self.pose_list=[[0,0],] 39 | 40 | self.distance = 0 41 | 42 | self.time = time.time() 43 | 44 | self.detect_msg.success_flag = 0 45 | 46 | self.detect_msg.failed_flag = 0 47 | 48 | self.center_x = np.zeros(5) 49 | self.center_y = np.zeros(5) 50 | 51 | def pre_cal(self, h): 52 | h_list = [0] 53 | for i in range(5): 54 | h_list.append((i+1)*h/5) 55 | return h_list 56 | 57 | #calculate distance 58 | def dis_cal(self): 59 | length = len(self.pose_list) 60 | dis = 0 61 | for i in range(length-1): 62 | a = self.pose_list[i+1][0]-self.pose_list[i+1][0] 63 | b = self.pose_list[i+1][1]-self.pose_list[i+1][1] 64 | dis +=math.sqrt((a**2+b**2)) 65 | return dis 66 | 67 | 68 | def camera_callback(self, data): 69 | if not self.active: 70 | return 71 | try: 72 | cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8') 73 | 74 | except CvBridgeError as e : 75 | print(e) 76 | 77 | cv_image = cv2.resize(cv_image, (img_width, img_height), \ 78 | interpolation=cv2.INTER_CUBIC) 79 | 80 | # ostu threshold 81 | gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 82 | threshold = self.ostu(gray) 83 | threshold = min(180,threshold) 84 | mask = cv2.inRange(gray, 0, threshold) 85 | 86 | left_list = self.search_leftbound(mask) # search left boundary of the tack 87 | right_list = self.search_rightbound(mask) # search right boundary of the tack 88 | if right_list == -1 or left_list == -1: 89 | self.detect_msg.failed_flag = True 90 | else: 91 | self.detect_msg.failed_flag = False 92 | if not self.detect_msg.failed_flag: 93 | 94 | img_len = min(len(left_list),len(right_list)) - 1 95 | for i in range(5): 96 | self.center_x[i] = (right_list[int(img_len*i/4)] + left_list[int(img_len*i/4)])/2 97 | self.center_y[i] = img_len/4.0*i 98 | for i in range(5): 99 | cv2.circle(mask, (int(self.center_x[i]), img_height-int(self.center_y[i])), 2, (0,0,0), -1) 100 | self.center_x = np.flip(self.center_x, 0) 101 | k = self.cur_fit(self.center_y, self.center_x) 102 | self.detect_msg.error_k = 0.9*k + 0.1*self.detect_msg.error_k 103 | self.detect_msg.cx = (self.center_x-np.ones(5)*img_width/2.0)/(img_width/2.0) 104 | self.detect_msg.cy = self.center_y/img_height 105 | error = self.detect_msg.cx[2] 106 | self.detect_msg.error_x = 0.9*error + 0.1*self.detect_msg.error_x 107 | # print(self.detect_msg.error_k) 108 | 109 | cv2.imshow("mask",mask) 110 | cv2.imshow("origin_image", cv_image) 111 | cv2.waitKey(1) 112 | 113 | def medium_filter(self,image): 114 | img = image[:] 115 | for i in range(1,image.shape[0]-1): 116 | for j in range(1,image.shape[1]-1): 117 | temp = np.zeros((3,3)) 118 | for x in range(3): 119 | for y in range(3): 120 | temp[x,y] = image[i+x-1,j+y-1] 121 | img[i,j] = np.median(temp) 122 | return img 123 | 124 | 125 | 126 | def cur_fit(self,x,y): 127 | """fitting function""" 128 | 129 | x = np.array(x) 130 | y = np.array(y) 131 | avg_x = float(x.sum() / x.shape[0]) 132 | avg_y = float(y.sum() / y.shape[0]) 133 | xy = x*y 134 | avg_xy = float(xy.sum() / xy.shape[0]) 135 | sqrx = x*x 136 | k = (avg_xy-avg_y*avg_x) 137 | if not sqrx.sum()==0 and not avg_x==0: 138 | k = k / ((sqrx.sum()/sqrx.shape[0]-avg_x**2)) 139 | else: 140 | k = self.detect_msg.error_k 141 | return k 142 | 143 | # ostu thresh find 144 | def ostu(self,image): 145 | blur = cv2.GaussianBlur(image,(5,5),0) 146 | 147 | # find normalized_histogram, and its cumulative distribution function 148 | hist = cv2.calcHist([blur],[0],None,[256],[0,256]) 149 | hist_norm = hist.ravel()/hist.sum() 150 | Q = hist_norm.cumsum() 151 | bins = np.arange(256) 152 | fn_min = np.inf 153 | thresh = -1 154 | for i in range(1,256): 155 | p1,p2 = np.hsplit(hist_norm,[i]) # probabilities 156 | q1,q2 = Q[i],Q[255]-Q[i] # cum sum of classes 157 | if q1 < 1.e-6 or q2 < 1.e-6: 158 | continue 159 | b1,b2 = np.hsplit(bins,[i]) # weights 160 | # finding means and variances 161 | m1,m2 = np.sum(p1*b1)/q1, np.sum(p2*b2)/q2 162 | v1,v2 = np.sum(((b1-m1)**2)*p1)/q1,np.sum(((b2-m2)**2)*p2)/q2 163 | # calculates the minimization function 164 | fn = v1*q1 + v2*q2 165 | if fn < fn_min: 166 | fn_min = fn 167 | thresh = i 168 | return thresh 169 | 170 | 171 | def start_callback(self,req): 172 | self.active = req.active 173 | res = StartUpResponse() 174 | if self.active: 175 | res.result = "Go" 176 | else: 177 | res.result = "Stop" 178 | 179 | return res 180 | 181 | def search_leftbound(self, image): 182 | left_list=[] 183 | (height, width) = image.shape 184 | for i in range(width): 185 | if image[height-1][i]==255: 186 | left_list.append(i) 187 | break 188 | if len(left_list)==0: 189 | return -1 190 | for h in range(2,height+1): 191 | end_flag = False 192 | if image[height-h][left_list[-1]] == 255: 193 | if left_list[-1]==0: 194 | left_list.append(0) 195 | continue 196 | for i in range(1,left_list[-1]+1): 197 | if image[height-h][left_list[-1]-i]==0: 198 | left_list.append(left_list[-1]-i+1) 199 | break 200 | if (left_list[-1]-i) == 0: 201 | left_list.append(0) 202 | else : 203 | for i in range(1,width-left_list[-1]): 204 | if image[height-h][left_list[-1]+i]==255: 205 | left_list.append(left_list[-1]+i) 206 | break 207 | if (left_list[-1]+i)==(width-1): 208 | end_flag = True 209 | if end_flag == True: 210 | break 211 | return left_list 212 | 213 | def search_rightbound(self,image): 214 | right_list=[] 215 | (height, width) = image.shape 216 | for i in range(width): 217 | if image[height-1][width-1-i]==255: 218 | right_list.append(width-1-i) 219 | break 220 | if len(right_list)==0: 221 | return -1 222 | for h in range(2,height+1): 223 | end_flag = False 224 | if image[height-h][right_list[-1]] == 255: 225 | if right_list[-1] == width-1: 226 | right_list.append(width-1) 227 | continue 228 | for i in range(1,width-right_list[-1]): 229 | if image[height-h][right_list[-1]+i]==0: 230 | right_list.append(right_list[-1]+i-1) 231 | break 232 | if (right_list[-1]+i) == (width-1): 233 | right_list.append(width-1) 234 | else : 235 | for i in range(1,right_list[-1]+1): 236 | if image[height-h][right_list[-1]-i]==255: 237 | right_list.append(right_list[-1]-i) 238 | break 239 | if (right_list[-1]-i)==0: 240 | end_flag = True 241 | if end_flag == True: 242 | break 243 | 244 | return right_list 245 | 246 | def cal_curve_error(self,x1,x2,x3,y1,y2,y3): 247 | a = np.sqrt((x2-x3)**2+(y2-y3)**2) #distance x2 x3 248 | b = np.sqrt((x1-x3)**2+(y1-y3)**2) #distance x1 x3 249 | c = np.sqrt((x1-x2)**2+(y1-y2)**2) #distance x1 x2 250 | 251 | if not a==0 and not b==0 and not c==0: 252 | 253 | cosB = (a**2+c**2-b**2)/(2*a*c) 254 | if cosB > 1 : 255 | cosB=1 256 | if cosB < -1 : 257 | cosB=-1 258 | if (x1+x2)/2= 5 : 296 | # rospy.loginfo("error_x = " + str(image_object.detect_msg.error_x)) 297 | # rospy.loginfo("error_k = "+ str(image_object.detect_msg.error_k)) 298 | # rospy.loginfo("dis = "+ str(image_object.detect_msg.distance)) 299 | count = 0 300 | count += 1 301 | # rospy.loginfo("v_x = " + str(image_object.detect_msg.v_x)) 302 | # rospy.loginfo("w_z = " + str(image_object.detect_msg.w_z)) 303 | image_object.pub_rate.sleep() 304 | 305 | 306 | if __name__ == '__main__': 307 | main() 308 | 309 | 310 | 311 | -------------------------------------------------------------------------------- /DRL-PID/line_follower.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from move_car import MoveCar 6 | import numpy as np 7 | import math 8 | from summit_description.msg import EnvInfo 9 | import time 10 | import matplotlib.pyplot as plt 11 | from std_srvs.srv import Empty 12 | from nav_msgs.msg import Odometry 13 | 14 | #max v_x=1; 15 | class LineFollower(object): 16 | 17 | def __init__(self): 18 | 19 | self.node = rospy.init_node('line_following_node', anonymous=True) 20 | self.rate = rospy.Rate(10) 21 | self.detect_sub = rospy.Subscriber("env_feedback", EnvInfo, \ 22 | self.move_callback) 23 | self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty) 24 | self.pos_sub = rospy.Subscriber('/odom', Odometry, self.pos_callback) 25 | self.move = MoveCar() 26 | self.car_twist = Twist() 27 | self.pose_list=[[0,0],] 28 | self.distance = 0 29 | self.time = time.time() 30 | # infomation to control car 31 | self.error = list(np.zeros(5)) 32 | self.error_k = list(np.zeros(5)) 33 | self.angular_list=[0,0,0] 34 | self.last_error = 0 # error made by last action 35 | self.v_x =0 36 | self.w_z =0 37 | self.cx = np.zeros(5) 38 | self.cy = np.zeros(5) 39 | # the action take 40 | self.kp = 0 41 | self.ki = 0 42 | self.kd = 0 43 | self.kp2 = 0 44 | self.ki2 = 0 45 | self.kd = 0 46 | # tradintional pid gain 47 | self._kp = 4 48 | self._kd = 0.2 49 | self._ki = 0.05 #0.005*3 50 | self._kp2 = 2 51 | self._kd2 = 0.2 52 | self._ki2 = 0 53 | # folloing will return to agent 54 | self.reward = 0 55 | self.state = np.zeros(13) 56 | #shape 57 | self.action_space = np.zeros(6) 58 | self.state_space = np.zeros(13) 59 | self.max_action = 10 60 | self.time_step = 0 61 | self.errorx_list = [] #error list 62 | self.reset_flag = True 63 | self.success_record = [] # record the success time 64 | 65 | self.done = False 66 | self.failed_flag = False 67 | self.success_flag = False 68 | self.sum_errork=0 69 | self.sum_errorx=0 70 | 71 | def pos_callback(self, data): 72 | """linear and angular""" 73 | x = data.pose.pose.position.x 74 | y = data.pose.pose.position.y 75 | dist = (x-self.pose_list[-1][0])**2+(y-self.pose_list[-1][1])**2 76 | self.distance += math.sqrt(dist) 77 | # if self.distance >= goal: 78 | # self.detect_msg.success_flag = 1 79 | # print("success!") 80 | 81 | self.pose_list.append([x, y]) 82 | 83 | 84 | self.v_x = data.twist.twist.linear.x 85 | self.w_z = data.twist.twist.angular.z 86 | 87 | if abs(x) <= 0.5 and abs(y) <= 0.5 and \ 88 | (time.time()-self.time) > 20: 89 | self.success_flag = True 90 | 91 | def reset(self): 92 | print('mean_v:', self.distance/(time.time()-self.time)) 93 | self.reset_world() 94 | if self.failed_flag: 95 | self.success_record.append(0) 96 | print("fail!") 97 | if self.success_flag: 98 | self.success_record.append(1) 99 | print("success!") 100 | self.error = list(np.zeros(5)) 101 | self.error_k = list(np.zeros(5)) 102 | self.angular_list=[0,0,0] 103 | self.last_error = 0 104 | self.v_x = 0 105 | self.w_z = 0 106 | self.reward = 0 107 | self.state = np.zeros(13) 108 | self.time_step = 0 109 | self.stop() 110 | time.sleep(3) 111 | self.time = time.time() 112 | self.errorx_list=[] 113 | self.done = False 114 | self.failed_flag = False 115 | self.success_flag = False 116 | self.pose_list = [[0,0],] 117 | self.distance = 0 118 | self.last_distance =0 119 | self.sum_errork=0 120 | self.sum_errorx=0 121 | 122 | return self.state 123 | 124 | def move_callback(self,data): 125 | 126 | self.error.pop() 127 | self.error.insert(0, data.error_x) 128 | self.error_k.pop() 129 | self.error_k.insert(0, data.error_k) 130 | self.cx = np.array(data.cx) 131 | self.cy = np.array(data.cy) 132 | self.failed_flag = data.failed_flag 133 | self.done = self.success_flag or self.failed_flag 134 | 135 | self.errorx_list.append(self.error[0]) 136 | 137 | def _reward_calculate(self): 138 | 139 | var_e = np.var(self.error) 140 | 141 | reward = 2*(self.distance - self.last_distance) - 0.1 - 10*var_e 142 | 143 | if self.done : 144 | if self.success_flag: 145 | reward = 5 146 | else: 147 | reward = -5 148 | 149 | self.last_error = self.error[0] 150 | self.last_distance = self.distance 151 | return reward 152 | 153 | def _pid2_calculate(self): 154 | 155 | er1_k = self.error_k[0] 156 | er2_k = self.error_k[1] 157 | er3_k = self.error_k[2] 158 | add_wz_2 = self._kp2 * (er1_k - er2_k) + self._ki2 * er1_k + \ 159 | self._kd2 * (er1_k - 2 * er2_k + er3_k) 160 | return add_wz_2 161 | 162 | def _pid_calculate(self): 163 | 164 | er1 = self.error[0] 165 | er2 = self.error[1] 166 | er3 = self.error[2] 167 | 168 | self.car_twist.linear.x = 2-abs(self.error[0])*0.5 169 | 170 | add_wz = -(self._kp * (er1 - er2) + self._ki * er1 + \ 171 | self._kd * (er1 - 2 * er2 + er3)) 172 | angular_z = self.car_twist.angular.z 173 | angular_z += (add_wz+0.5*self._pid2_calculate()) 174 | # angular_z = max(-np.pi, min(np.pi, angular_z)) # clip(-0.1, 0.1) 175 | self.car_twist.angular.z = self.car_twist.angular.z*0.1+0.9*angular_z 176 | 177 | # def _pid2_rl(self): 178 | 179 | # er1_k = self.error_k[0] 180 | # er2_k = self.error_k[1] 181 | # er3_k = self.error_k[2] 182 | # self.sum_errork +=er1_k 183 | # wz_2 = self.kp2*er1_k + self.kd2*(er1_k-er2_k) 184 | # return wz_2 185 | 186 | # def _pid_rl(self): 187 | 188 | # er1 = self.error[0] 189 | # er2 = self.error[1] 190 | # er3 = self.error[2] 191 | # self.sum_errorx +=er1 192 | # self.car_twist.linear.x = 2-abs(self.error[0])*0.5 193 | 194 | # wz_1 = -(self.kp*er1+self.kd*(er1-er2)) 195 | # angular_z = wz_1 + self._pid2_rl()*0.5 196 | # angular_z = max(-np.pi, min(np.pi, angular_z)) # clip(-1, 1) 197 | # self.car_twist.angular.z = self.car_twist.angular.z*0.1+0.9*angular_z 198 | # self.angular_list.pop() 199 | # self.angular_list.insert(0,self.car_twist.angular.z) 200 | 201 | def _pid2_rl(self): 202 | 203 | er1_k = self.error_k[0] 204 | er2_k = self.error_k[1] 205 | er3_k = self.error_k[2] 206 | add_wz_2 = self.kp2 * (er1_k - er2_k) + self.ki2 * er1_k + \ 207 | self.kd2 * (er1_k - 2 * er2_k + er3_k) 208 | return add_wz_2 209 | 210 | def _pid_rl(self): 211 | 212 | er1 = self.error[0] 213 | er2 = self.error[1] 214 | er3 = self.error[2] 215 | 216 | self.car_twist.linear.x = 2-abs(self.error[0])*0.5 217 | 218 | add_wz = -(self.kp * (er1 - er2) + self.ki * er1 + \ 219 | self.kd * (er1 - 2 * er2 + er3)) 220 | angular_z = self.car_twist.angular.z 221 | angular_z += (add_wz+0.5*self._pid2_rl()) 222 | angular_z = max(-np.pi, min(np.pi, angular_z)) # clip(-0.1, 0.1) 223 | self.car_twist.angular.z = self.car_twist.angular.z*0.1+0.9*angular_z 224 | 225 | 226 | def control(self,sac_pid=True): 227 | if not sac_pid: 228 | self._pid_calculate() 229 | else: 230 | self._pid_rl() 231 | # rospy.loginfo("ANGULAR VALUE SENT -->"+str(self.car_twist.angular.z)) 232 | # rospy.loginfo("error -->"+str(self.error[0])) 233 | self.move.move_robot(self.car_twist) 234 | 235 | def feedback(self): 236 | # state and reward 237 | self.state = np.append(self.cx,self.cy) 238 | # self.state1 = np.append(self.error,self.error_k) 239 | # self.state = np.append(self.state, self.state1) 240 | state2 = [self.v_x,self.w_z,self.error_k[0], self.distance/45.0] 241 | self.state = np.append(self.state, state2) 242 | self.reward = self._reward_calculate() 243 | 244 | return self.state, self.reward, self.done 245 | 246 | def update_pid(self,kp,kd,kp2,kd2,ki=None,ki2=None): 247 | if ki and ki2: 248 | self.kp = kp 249 | self.kd = kd 250 | self.ki = ki 251 | self.kp2 = kp2 252 | self.ki2 = ki2 253 | self.kd2 = kd2 254 | else: 255 | self.kp = kp 256 | self.kd = kd 257 | self.kp2 = kp2 258 | self.kd2 = kd2 259 | 260 | # print('P1:%.1f'%self.kp,' D1: %.1f'%self.kd,' I1: %.1f'%self.ki) 261 | # print('P2:%.1f'%self.kp2,' D2: %.1f'%self.kd2,' I2: %.1f'%self.ki2) 262 | 263 | def stop(self): 264 | self.car_twist.linear.x = 0.0 265 | self.car_twist.angular.z = 0.0 266 | self.move.move_robot(self.car_twist) 267 | 268 | def main(): 269 | 270 | rospy.init_node("line_following_node", anonymous=True) 271 | line_follower= LineFollower() 272 | rate = rospy.Rate(10) 273 | line_follower.reset() 274 | 275 | def shutdown(): 276 | print("shutdown!") 277 | line_follower.stop() 278 | 279 | rospy.on_shutdown(shutdown) 280 | while not rospy.is_shutdown(): 281 | line_follower.control(sac_pid=False) 282 | if line_follower.done: 283 | total_time = time.time()-line_follower.time 284 | print(line_follower.distance/(total_time)) 285 | line_follower.reset() 286 | rate.sleep() 287 | 288 | 289 | 290 | 291 | if __name__ == '__main__': 292 | main() 293 | -------------------------------------------------------------------------------- /DRL-PID/main.py: -------------------------------------------------------------------------------- 1 | from line_follower import LineFollower 2 | import numpy as np 3 | from sac_torch import Agent 4 | import rospy 5 | from summit_description.srv import StartUp, StartUpRequest # service call 6 | from utils import plot_learning_curve 7 | from utils import str2bool 8 | import torch as T 9 | import argparse 10 | 11 | if __name__ == '__main__': 12 | 13 | #parameter setting 14 | parser = argparse.ArgumentParser(description='track_car') 15 | parser.add_argument('--gamma', type=float, default=0.99, metavar='G', 16 | help='discount factor for reward (default: 0.99)') 17 | parser.add_argument('--tau', type=float, default=0.005, metavar='G', 18 | help='target smoothing coefficient(tao) (default: 0.005)') 19 | parser.add_argument('--lr', type=float, default=0.0003, metavar='G', 20 | help='learning rate (default: 0.0003)') 21 | parser.add_argument('--batch_size', type=int, default=256, metavar='N', 22 | help='batch size (default: 256)') 23 | parser.add_argument('--hidden_size', type=int, default=256, metavar='N', 24 | help='hidden size (default: 256)') 25 | parser.add_argument('--reward_scale', type=int, default=15, metavar='N', 26 | help='reward_scale (default:10)') 27 | parser.add_argument('--train', type=str2bool, default=True, metavar='N', 28 | help='if train (default:True)') 29 | parser.add_argument('--episode', type=int, default=1000, metavar='N', 30 | help='episode (default:1000)') 31 | parser.add_argument('--load', type=str2bool, default=False, metavar='N', 32 | help='if load (default:False)') 33 | parser.add_argument('--warmup',type=str2bool, default=False, metavar='N', 34 | help='if warmup (default:False') 35 | parser.add_argument('--RL',type=str2bool, default=True,metavar='N', 36 | help = 'if use RL(defaul:True)') 37 | parser.add_argument('--action',type=int, default=6, metavar='N', 38 | help='action number(default=6)') 39 | parser.add_argument('--seed',type=int, default=123456, metavar='N', 40 | help='action number(default=123456)') 41 | args = parser.parse_args() 42 | 43 | #set random seed 44 | T.manual_seed(args.seed) 45 | np.random.seed(args.seed) 46 | T.cuda.manual_seed(args.seed) 47 | 48 | load_point = args.load 49 | train_mode = args.train 50 | episode = args.episode 51 | 52 | env_id = 'path_1' 53 | rospy.wait_for_service('/Activate') 54 | service_call = rospy.ServiceProxy('/Activate', StartUp) 55 | response = service_call(True) 56 | print(response) 57 | 58 | agent = Agent(alpha=args.lr,beta=args.lr,n_actions=args.action,gamma=args.gamma, 59 | reward_scale=args.reward_scale, layer1_size=args.hidden_size, 60 | layer2_size=args.hidden_size, tau=args.tau,batch_size=args.batch_size) 61 | env = LineFollower() 62 | 63 | score = 0 64 | score_history = [] 65 | score_save = [] 66 | mean_error = [] 67 | best_score = 0 68 | 69 | 70 | def shutdown(): 71 | print("shutdown!") 72 | env.stop() 73 | service_call(False) 74 | 75 | rospy.on_shutdown(shutdown) 76 | 77 | if load_point: 78 | agent.load_models() 79 | 80 | value_list=np.array([]) 81 | print(T.cuda.is_available()) 82 | 83 | for i in range(episode): 84 | 85 | env.reset() 86 | observation, _ , done= env.feedback() 87 | done = False 88 | score = 0 89 | step=0 90 | while not done: 91 | time_step=0 92 | action = agent.choose_action(observation, warmup=args.warmup, 93 | evaluate=args.load,action=args.action) 94 | if args.action==6: 95 | env.update_pid(kp=(action[0])*4,kd=(action[1]),\ 96 | kp2=(action[2])*4, kd2=(action[3]),ki=(action[4]), 97 | ki2=(action[5])) 98 | else: 99 | env.update_pid(kp=(action[0])*4,kd=(action[1]),\ 100 | kp2=(action[2])*4, kd2=(action[3])) 101 | while time_step < 3: 102 | done = env.done 103 | if done: 104 | #env.stop() 105 | break 106 | env.control(sac_pid=args.RL) 107 | env.rate.sleep() 108 | time_step +=1 109 | step +=1 110 | observation_, reward, done = env.feedback() 111 | score += reward 112 | agent.remember(observation, action, reward, observation_, done) 113 | observation = observation_ 114 | if train_mode: 115 | agent.learn() 116 | 117 | score_history.append(score) 118 | score_save.append(score) 119 | avg_score = np.mean(score_history[-50:]) 120 | if best_score <= avg_score and train_mode and i>=50: 121 | best_score = avg_score 122 | agent.save_models() 123 | 124 | print('episode = ', i, 'score = ', score, 'avg_score = ', avg_score,'step:',step) 125 | if (i+1) % 50 == 0: 126 | with open('data/train/score_data.txt','a') as file_object: 127 | file_object.write(str(score_save)+'\n') 128 | with open('data/train/success_record.txt','a') as file_object: 129 | file_object.write(str(env.success_record)+'\n') 130 | score_save=[] 131 | env.success_record=[] 132 | #record lose 133 | if (i+1) % 10 == 0 : 134 | with open('data/train/value_loss_list.txt','a') as file_object: 135 | file_object.write(str(agent.value_loss_list)+'\n') 136 | with open('data/train/actor_loss_list.txt','a') as file_object: 137 | file_object.write(str(agent.actor_loss_list)+'\n') 138 | with open('data/train/critic_loss_list.txt','a') as file_object: 139 | file_object.write(str(agent.critic_loss_list)+'\n') 140 | agent.value_loss_list=[] 141 | agent.actor_loss_list=[] 142 | agent.critic_loss_list=[] 143 | 144 | if load_point: 145 | with open('data/test/error_RL-PID.txt','a') as file_object: 146 | file_object.write(str(env.errorx_list)+'\n') 147 | if not args.RL: 148 | with open('data/test/error-PID.txt','a') as file_object: 149 | file_object.write(str(env.errorx_list)+'\n') 150 | # if load_point: 151 | # with open('data/test/score_path4.txt','a') as file_object: 152 | # file_object.write(str(score_history)) 153 | # if not args.RL: 154 | # with open('data/test/PID_path4.txt','a') as file_object: 155 | # file_object.write(str(score_history)) -------------------------------------------------------------------------------- /DRL-PID/model_testing.py: -------------------------------------------------------------------------------- 1 | from line_follower import LineFollower 2 | import numpy as np 3 | from sac_torch import Agent 4 | import rospy 5 | from summit_description.srv import StartUp, StartUpRequest # service call 6 | from utils import plot_learning_curve 7 | from utils import str2bool 8 | import torch as T 9 | import argparse 10 | 11 | if __name__ == '__main__': 12 | 13 | #parameter setting 14 | parser = argparse.ArgumentParser(description='track_car') 15 | parser.add_argument('--gamma', type=float, default=0.99, metavar='G', 16 | help='discount factor for reward (default: 0.99)') 17 | parser.add_argument('--tau', type=float, default=0.005, metavar='G', 18 | help='target smoothing coefficient(tao) (default: 0.005)') 19 | parser.add_argument('--lr', type=float, default=0.0003, metavar='G', 20 | help='learning rate (default: 0.0003)') 21 | parser.add_argument('--batch_size', type=int, default=256, metavar='N', 22 | help='batch size (default: 256)') 23 | parser.add_argument('--hidden_size', type=int, default=256, metavar='N', 24 | help='hidden size (default: 256)') 25 | parser.add_argument('--reward_scale', type=int, default=15, metavar='N', 26 | help='reward_scale (default:10)') 27 | parser.add_argument('--train', type=str2bool, default=True, metavar='N', 28 | help='if train (default:True)') 29 | parser.add_argument('--episode', type=int, default=1000, metavar='N', 30 | help='episode (default:1000)') 31 | parser.add_argument('--load', type=str2bool, default=False, metavar='N', 32 | help='if load (default:False)') 33 | parser.add_argument('--warmup',type=str2bool, default=False, metavar='N', 34 | help='if warmup (default:False') 35 | parser.add_argument('--RL',type=str2bool, default=True,metavar='N', 36 | help = 'if use RL(defaul:True)') 37 | parser.add_argument('--action',type=int, default=6, metavar='N', 38 | help='action number(default=6)') 39 | 40 | 41 | args = parser.parse_args() 42 | 43 | 44 | load_point = args.load 45 | train_mode = args.train 46 | env_id = 'path_1' 47 | rospy.wait_for_service('/Activate') 48 | service_call = rospy.ServiceProxy('/Activate', StartUp) 49 | response = service_call(True) 50 | print(response) 51 | agent = Agent(alpha=args.lr,beta=args.lr,n_actions=args.action,gamma=args.gamma, 52 | reward_scale=args.reward_scale, layer1_size=args.hidden_size, 53 | layer2_size=args.hidden_size, tau=args.tau,) 54 | env = LineFollower() 55 | episode = args.episode 56 | score = 0 57 | score_history = [] 58 | score_save = [] 59 | 60 | def shutdown(): 61 | print("shutdown!") 62 | env.stop() 63 | service_call(False) 64 | 65 | rospy.on_shutdown(shutdown) 66 | mean_error = [] 67 | agent.load_models() 68 | agent.save_models() 69 | 70 | print(T.cuda.is_available()) 71 | 72 | 73 | env.reset() 74 | observation, _ , done= env.feedback() 75 | done = False 76 | score = 0 77 | step=0 78 | while not done: 79 | time_step=0 80 | if args.RL: 81 | 82 | action = agent.choose_action(observation, warmup=args.warmup, 83 | evaluate=args.load,action=args.action) 84 | if args.action==6: 85 | env.update_pid(kp=(action[0]+1)*2,kd=(action[1]+1)*2,\ 86 | kp2=(action[2]+1)*2, kd2=(action[3]+1)*2,ki=(action[4]+1)/2, 87 | ki2=(action[5]+1)/2) 88 | else: 89 | env.update_pid(kp=(action[0]+1)*2,kd=(action[1]+1)*2,\ 90 | kp2=(action[2]+1)*2, kd2=(action[3]+1)*2) 91 | while time_step < 3: 92 | done = env.done 93 | if done: 94 | #env.stop() 95 | break 96 | env.control(sac_pid=args.RL) 97 | env.rate.sleep() 98 | time_step +=1 99 | step +=1 100 | observation_, reward, done = env.feedback() 101 | score += reward 102 | observation = observation_ 103 | 104 | 105 | score_save.append(score) 106 | avg_score = np.mean(score_history[-50:]) 107 | 108 | print('score = %.2f' % score, 'step:',step) 109 | if args.RL: 110 | with open('data/test/test_RL-PID.txt','a') as file_object: 111 | file_object.write(str(env.errorx_list)+'\n') 112 | else: 113 | with open('data/test/test_PID.txt','a') as file_object: 114 | file_object.write(str(env.errorx_list)+'\n') 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /DRL-PID/move_car.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import Twist 4 | import numpy 5 | 6 | class MoveCar(object): 7 | def __init__(self): 8 | 9 | self.cmd_vel_pub = rospy.Publisher('/cmd_vel',Twist, queue_size=10) 10 | 11 | def move_robot(self, twist_object): 12 | self.cmd_vel_pub.publish(twist_object) 13 | 14 | 15 | def clean_class(self): 16 | twist_object = Twist() 17 | twist_object.linear.x = 0.0 18 | twist_object.angular.z = 0.0 19 | self.move_robot(twist_object) 20 | self.shutdown_dectected = True 21 | -------------------------------------------------------------------------------- /DRL-PID/networks.py: -------------------------------------------------------------------------------- 1 | import os 2 | import torch as T 3 | import torch.nn.functional as F 4 | import torch.nn as nn 5 | import torch.optim as optim 6 | from torch.distributions.normal import Normal 7 | import numpy as np 8 | 9 | class CriticNetwork(nn.Module): 10 | def __init__(self, beta, input_dims, n_actions, fc1_dims=256, fc2_dims=256, 11 | name='critic', chkpt_dir='tmp/sac'): 12 | super(CriticNetwork, self).__init__() 13 | self.input_dims = input_dims 14 | self.fc1_dims = fc1_dims 15 | self.fc2_dims = fc2_dims 16 | self.n_actions = n_actions 17 | self.name = name 18 | self.checkpoint_dir = chkpt_dir 19 | self.checkpoint_file = os.path.join(self.checkpoint_dir, name+'_sac') 20 | 21 | self.fc1 = nn.Linear(self.input_dims[0]+n_actions, self.fc1_dims) 22 | self.fc2 = nn.Linear(self.fc1_dims, self.fc2_dims) 23 | self.q = nn.Linear(self.fc2_dims, 1) 24 | 25 | self.optimizer = optim.Adam(self.parameters(), lr=beta) 26 | self.device = T.device('cuda:0' if T.cuda.is_available() else 'cpu') 27 | 28 | self.to(self.device) 29 | 30 | def forward(self, state, action): 31 | action_value = self.fc1(T.cat([state, action], dim=1)) 32 | action_value = F.relu(action_value) 33 | action_value = self.fc2(action_value) 34 | action_value = F.relu(action_value) 35 | 36 | q = self.q(action_value) 37 | 38 | return q 39 | 40 | def save_checkpoint(self): 41 | T.save(self.state_dict(), self.checkpoint_file) 42 | 43 | def load_checkpoint(self): 44 | self.load_state_dict(T.load(self.checkpoint_file)) 45 | 46 | class ValueNetwork(nn.Module): 47 | def __init__(self, beta, input_dims, fc1_dims=256, fc2_dims=256, 48 | name='value', chkpt_dir='tmp/sac'): 49 | super(ValueNetwork, self).__init__() 50 | self.input_dims = input_dims 51 | self.fc1_dims = fc1_dims 52 | self.fc2_dims = fc2_dims 53 | self.name = name 54 | self.checkpoint_dir = chkpt_dir 55 | self.checkpoint_file = os.path.join(self.checkpoint_dir, name+'_sac') 56 | 57 | self.fc1 = nn.Linear(*self.input_dims, self.fc1_dims) 58 | self.fc2 = nn.Linear(self.fc1_dims, fc2_dims) 59 | self.v = nn.Linear(self.fc2_dims, 1) 60 | 61 | self.optimizer = optim.Adam(self.parameters(), lr=beta) 62 | self.device = T.device('cuda:0' if T.cuda.is_available() else 'cpu') 63 | 64 | self.to(self.device) 65 | 66 | def forward(self, state): 67 | state_value = self.fc1(state) 68 | state_value = F.relu(state_value) 69 | state_value = self.fc2(state_value) 70 | state_value = F.relu(state_value) 71 | 72 | v = self.v(state_value) 73 | 74 | return v 75 | 76 | def save_checkpoint(self): 77 | T.save(self.state_dict(), self.checkpoint_file) 78 | 79 | def load_checkpoint(self): 80 | self.load_state_dict(T.load(self.checkpoint_file)) 81 | 82 | class ActorNetwork(nn.Module): 83 | def __init__(self, alpha, input_dims, max_action, fc1_dims=256, 84 | fc2_dims=256, n_actions=2, name='actor', chkpt_dir='tmp/sac'): 85 | super(ActorNetwork, self).__init__() 86 | self.input_dims = input_dims 87 | self.fc1_dims = fc1_dims 88 | self.fc2_dims = fc2_dims 89 | self.n_actions = n_actions 90 | self.name = name 91 | self.checkpoint_dir = chkpt_dir 92 | if not os.path.exists(self.checkpoint_dir): 93 | os.makedirs(self.checkpoint_dir) 94 | self.checkpoint_file = os.path.join(self.checkpoint_dir, name+'_sac') 95 | self.max_action = max_action 96 | self.reparam_noise = 1e-6 97 | 98 | self.fc1 = nn.Linear(*self.input_dims, self.fc1_dims) 99 | self.fc2 = nn.Linear(self.fc1_dims, self.fc2_dims) 100 | self.mu = nn.Linear(self.fc2_dims, self.n_actions) 101 | self.sigma = nn.Linear(self.fc2_dims, self.n_actions) 102 | 103 | self.optimizer = optim.Adam(self.parameters(), lr=alpha) 104 | self.device = T.device('cuda:0' if T.cuda.is_available() else 'cpu') 105 | 106 | self.to(self.device) 107 | 108 | def forward(self, state): 109 | prob = self.fc1(state) 110 | prob = F.relu(prob) 111 | prob = self.fc2(prob) 112 | prob = F.relu(prob) 113 | 114 | mu = self.mu(prob) 115 | sigma = self.sigma(prob) 116 | 117 | sigma = T.clamp(sigma, min=self.reparam_noise, max=1) 118 | 119 | return mu, sigma 120 | 121 | def sample_normal(self, state, reparameterize=True): 122 | mu, sigma = self.forward(state) 123 | probabilities = Normal(mu, sigma) 124 | 125 | if reparameterize: 126 | actions = probabilities.rsample() 127 | else: 128 | actions = probabilities.sample() 129 | 130 | action = T.tanh(actions)*T.tensor(self.max_action).to(self.device) 131 | log_probs = probabilities.log_prob(actions) 132 | log_probs -= T.log(1-action.pow(2)+self.reparam_noise) 133 | log_probs = log_probs.sum(1, keepdim=True) 134 | 135 | return action, log_probs 136 | 137 | def sample_mu(self, state): 138 | mu, sigma = self.forward(state) 139 | action = T.tanh(mu)*T.tensor(self.max_action).to(self.device) 140 | return action 141 | 142 | def save_checkpoint(self): 143 | T.save(self.state_dict(), self.checkpoint_file, 144 | _use_new_zipfile_serialization=False) 145 | 146 | def load_checkpoint(self): 147 | self.load_state_dict(T.load(self.checkpoint_file)) 148 | 149 | -------------------------------------------------------------------------------- /DRL-PID/sac_torch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import torch as T 3 | import torch.nn.functional as F 4 | import numpy as np 5 | from buffer import ReplayBuffer 6 | from networks import ActorNetwork, CriticNetwork, ValueNetwork 7 | 8 | class Agent(): 9 | def __init__(self, alpha=0.0003, beta=0.0003, input_dims=[14], 10 | env=None, gamma=0.99, n_actions=2, max_size=1000000, tau=0.005, 11 | layer1_size=256, layer2_size=256, batch_size=256, reward_scale=15): 12 | self.gamma = gamma 13 | self.tau = tau 14 | self.memory = ReplayBuffer(max_size, input_dims, n_actions) 15 | self.batch_size = batch_size 16 | self.n_actions = n_actions 17 | 18 | self.actor = ActorNetwork(alpha, input_dims=[13], n_actions=n_actions, 19 | name='actor', max_action=1) 20 | self.critic_1 = CriticNetwork(beta, input_dims, n_actions=n_actions, 21 | name='critic_1') 22 | self.critic_2 = CriticNetwork(beta, input_dims, n_actions=n_actions, 23 | name='critic_2') 24 | self.value = ValueNetwork(beta, input_dims, name='value') 25 | self.target_value = ValueNetwork(beta, input_dims, name='target_value') 26 | 27 | self.scale = reward_scale 28 | self.update_network_parameters(tau=1) 29 | #record loss 30 | self.value_loss_list = np.array([]) 31 | self.critic_loss_list = np.array([]) 32 | self.actor_loss_list = np.array([]) 33 | 34 | def choose_action(self, observation, warmup=False, evaluate=False, action=6): 35 | observation = observation[:-1] 36 | state = T.Tensor([observation]).to(self.actor.device) 37 | if evaluate: 38 | return self.actor.sample_mu(state).cpu().detach().numpy()[0] 39 | if warmup and self.memory.mem_cntr<=self.batch_size: 40 | actions = (T.rand(1,action)-0.5)*2 41 | else: 42 | actions, _ = self.actor.sample_normal(state, reparameterize=False) 43 | 44 | return actions.cpu().detach().numpy()[0] 45 | 46 | def remember(self, state, action, reward, new_state, done): 47 | self.memory.store_transition(state, action, reward, new_state, done) 48 | 49 | def update_network_parameters(self, tau=None): 50 | if tau is None: 51 | tau = self.tau 52 | 53 | target_value_params = self.target_value.named_parameters() 54 | value_params = self.value.named_parameters() 55 | 56 | target_value_state_dict = dict(target_value_params) 57 | value_state_dict = dict(value_params) 58 | 59 | for name in value_state_dict: 60 | value_state_dict[name] = tau*value_state_dict[name].clone() + \ 61 | (1-tau)*target_value_state_dict[name].clone() 62 | 63 | self.target_value.load_state_dict(value_state_dict) 64 | 65 | def save_models(self): 66 | print('.... saving models ....') 67 | self.actor.save_checkpoint() 68 | self.value.save_checkpoint() 69 | self.target_value.save_checkpoint() 70 | self.critic_1.save_checkpoint() 71 | self.critic_2.save_checkpoint() 72 | 73 | def load_models(self): 74 | print('.... loading models ....') 75 | self.actor.load_checkpoint() 76 | self.value.load_checkpoint() 77 | self.target_value.load_checkpoint() 78 | self.critic_1.load_checkpoint() 79 | self.critic_2.load_checkpoint() 80 | 81 | def learn(self): 82 | if self.memory.mem_cntr < self.batch_size: 83 | return 84 | 85 | state, action, reward, new_state, done = \ 86 | self.memory.sample_buffer(self.batch_size) 87 | actor_state = T.zeros(self.batch_size,13).to(self.actor.device) 88 | reward = T.tensor(reward, dtype=T.float).to(self.actor.device) 89 | done = T.tensor(done).to(self.actor.device) 90 | state_ = T.tensor(new_state, dtype=T.float).to(self.actor.device) 91 | state = T.tensor(state, dtype=T.float).to(self.actor.device) 92 | action = T.tensor(action, dtype=T.float).to(self.actor.device) 93 | 94 | value = self.value(state).view(-1) 95 | value_ = self.target_value(state_).view(-1) 96 | value_[done] = 0.0 97 | 98 | for i in range(len(state)): 99 | actor_state[i] = state[i][:-1] 100 | 101 | actions, log_probs = self.actor.sample_normal(actor_state, reparameterize=False) 102 | log_probs = log_probs.view(-1) 103 | q1_new_policy = self.critic_1.forward(state, actions) 104 | q2_new_policy = self.critic_2.forward(state, actions) 105 | critic_value = T.min(q1_new_policy, q2_new_policy) 106 | critic_value = critic_value.view(-1) 107 | 108 | self.value.optimizer.zero_grad() 109 | value_target = critic_value - log_probs 110 | value_loss = 0.5 * F.mse_loss(value, value_target) 111 | value_loss.backward(retain_graph=True) 112 | self.value.optimizer.step() 113 | 114 | actions, log_probs = self.actor.sample_normal(actor_state, reparameterize=True) 115 | log_probs = log_probs.view(-1) 116 | q1_new_policy = self.critic_1.forward(state, actions) 117 | q2_new_policy = self.critic_2.forward(state, actions) 118 | critic_value = T.min(q1_new_policy, q2_new_policy) 119 | critic_value = critic_value.view(-1) 120 | 121 | actor_loss = log_probs - critic_value 122 | actor_loss = T.mean(actor_loss) 123 | self.actor.optimizer.zero_grad() 124 | actor_loss.backward(retain_graph=True) 125 | self.actor.optimizer.step() 126 | 127 | self.critic_1.optimizer.zero_grad() 128 | self.critic_2.optimizer.zero_grad() 129 | q_hat = self.scale*reward + self.gamma*value_ 130 | q1_old_policy = self.critic_1.forward(state, action).view(-1) 131 | q2_old_policy = self.critic_2.forward(state, action).view(-1) 132 | critic_1_loss = 0.5 * F.mse_loss(q1_old_policy, q_hat) 133 | critic_2_loss = 0.5 * F.mse_loss(q2_old_policy, q_hat) 134 | 135 | critic_loss = critic_1_loss + critic_2_loss 136 | critic_loss.backward() 137 | self.critic_1.optimizer.step() 138 | self.critic_2.optimizer.step() 139 | 140 | self.update_network_parameters() 141 | 142 | #record loss 143 | critic_loss = critic_loss.cpu().detach().numpy() 144 | value_loss = value_loss.cpu().detach().numpy() 145 | actor_loss = actor_loss.cpu().detach().numpy() 146 | self.critic_loss_list = np.append(self.critic_loss_list,critic_loss) 147 | self.actor_loss_list = np.append(self.actor_loss_list, actor_loss) 148 | self.value_loss_list = np.append(self.value_loss_list, value_loss) 149 | 150 | 151 | -------------------------------------------------------------------------------- /DRL-PID/tmp/sac/actor_sac: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/DRL-PID/tmp/sac/actor_sac -------------------------------------------------------------------------------- /DRL-PID/tmp/sac/critic_1_sac: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/DRL-PID/tmp/sac/critic_1_sac -------------------------------------------------------------------------------- /DRL-PID/tmp/sac/critic_2_sac: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/DRL-PID/tmp/sac/critic_2_sac -------------------------------------------------------------------------------- /DRL-PID/tmp/sac/target_value_sac: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/DRL-PID/tmp/sac/target_value_sac -------------------------------------------------------------------------------- /DRL-PID/tmp/sac/value_sac: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/DRL-PID/tmp/sac/value_sac -------------------------------------------------------------------------------- /DRL-PID/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | def plot_learning_curve(x, scores, figure_file,time,title='average reward'): 5 | running_avg = np.zeros(len(scores)) 6 | for i in range(len(running_avg)): 7 | running_avg[i] = np.mean(scores[max(0, i-time):(i+1)]) 8 | plt.plot(x, running_avg) 9 | plt.title(title) 10 | plt.savefig(figure_file) 11 | 12 | def str2bool(v): 13 | if v.lower() in ('yes','true','t','y','1'): 14 | return True 15 | elif v.lower() in ('no','false','f','n','0'): 16 | return False 17 | else: 18 | raise argparse.ArgumentTypeError('Boolean value expected') 19 | -------------------------------------------------------------------------------- /Image/car.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/car.png -------------------------------------------------------------------------------- /Image/env.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/env.jpg -------------------------------------------------------------------------------- /Image/framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/framework.png -------------------------------------------------------------------------------- /Image/map_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/map_0.png -------------------------------------------------------------------------------- /Image/score_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/score_plot.png -------------------------------------------------------------------------------- /Image/searching algorithm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/searching algorithm.png -------------------------------------------------------------------------------- /Image/structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/structure.png -------------------------------------------------------------------------------- /Image/success_rate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/Image/success_rate.png -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ruan Yudi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DRL-PID 2 | 3 | ## Intro 4 | 5 | It's a framework that focuses on the dynamic adjustment of the PID parameters of the controlled objects using **Deep Reinforcement Learning Algorithm**, which in this project, I used **Soft Actor Critic** (SAC) as the basic DRL algorithm and achieved great results. 6 | 7 | Specifically, this project is using DRL-PID framework in the **auto driving scenario** (track following), but this framework can expand to any scene that needs dynamically tuning PID parameters without human intervention. The **main advantage** of this framework in my opinion is it can **search the optimal PID tuning policy automatically**, especially when too many PID parameters are coupled together, and manual adjustment is difficult. But it also suffers from the unexplainable deep learning procedure and can not provide the theory guarantee. 8 | 9 | **It's my bachelor graduation project and I hope the open source code can benefit the future research in the relevant domains.** 10 | 11 | **If you find this project helpful, star on it :star:** 12 | 13 | ## Method 14 | 15 | #### Overview 16 | 17 | The training framework: 18 | 19 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/framework.png) 20 | 21 | The execution framework: 22 | 23 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/structure.png) 24 | 25 | #### Line Detection 26 | 27 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/searching%20algorithm.png) 28 | 29 | #### ROS Node 30 | 31 | RL training is implemented in the **ROS** and **gazebo** platform 32 | 33 | The training system operates using the ROS topic communication mechanism. 34 | 35 | The overall system mainly includes **environment detection node** and **tracking control node**. 36 | 37 | Environment detection node: **Subscribe to the topics released by the camera**, pre-process the images detected by the camera, and obtain state information; at the same time, judge whether the smart car deviates from the track by judging the length of the left and right boundary point sets obtained by the image search algorithm, and store these information as ROS messages to post on the Environment Feedback topic. 38 | 39 | tracking control node: Subscribe to the environmental feedback topic and the topic **odom** released by GAZEBO that contains the pose information of the car; **output angular velocity information and linear velocity** information to the topic **cmd_vel**, and **cmd_vel** controls the movement of the smart car simulation model. 40 | Finally, GAZEBO subscribes to the **cmd_vel** topic to visualize the motion of the smart car simulation model in real-time in the tracking environment , eventually form a **complete control closed-loop structure**. 41 | 42 | ## Installation 43 | 44 | * ROS (16.04 or 20.04 has been tested alright) 45 | * Gazebo 46 | 47 | * Pytorch 48 | 49 | ## Quick Start 50 | 51 | **summit_description** contains the ROS package needed in this project, install this in your own ROS workspace first and add it to ROS path. 52 | 53 | **launch the environment** 54 | 55 | ``` 56 | roslaunch summit_description summit.launch 57 | ``` 58 | 59 | **env_feedback.py** contains the algorithm that detects the track using camera and processes the original environment detection into the state information. 60 | 61 | **launch the env detection node** 62 | 63 | ``` 64 | python env_feedback.py 65 | ``` 66 | 67 | after the preparation steps, you can launch the main node to start training. 68 | 69 | **start training** 70 | 71 | ``` 72 | python main.py 73 | ``` 74 | 75 | ## Code Structure 76 | 77 | **my_ground_plane** 78 | 79 | the path model used in the project, put this into the .gazebo/model folder. You can change the map by replacing the **MyImage.png** in the /materials/textures 80 | 81 | **summit description** 82 | 83 | * contain robot description, necessary ros message, ros service file and launch file 84 | 85 | **DRL-PID** 86 | 87 | * main.py : main file for training 88 | * model_testing.py : test trained model 89 | * sac_torch.py : SAC algorithm 90 | * network.py: network definition 91 | * env_feedback.py: the env pre-process file 92 | * line_follower.py: the environment definition 93 | 94 | ## Experiment 95 | 96 | **map_0** 97 | 98 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/map_0.png) 99 | 100 | **episode reward** 101 | 102 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/score_plot.png) 103 | 104 | **episode success rate** 105 | 106 | ![image](https://raw.githubusercontent.com/blakcapple/DRL-PID/main/Image/success_rate.png) 107 | 108 | ## Reference 109 | 110 | [A Self-adaptive SAC-PID Control Approach based on Reinforcement Learning for Mobile Robots](https://arxiv.org/pdf/2103.10686.pdf) (this project is highly based on this paper) 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /my_ground_plane/materials/scripts/my_ground_plane.material: -------------------------------------------------------------------------------- 1 | material MyGroundPlane/Image 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | ambient 1 1 1 1.000000 8 | diffuse 1 1 1 1.000000 9 | specular 0.03 0.03 0.03 1.000000 10 | 11 | texture_unit 12 | { 13 | texture MyImage.png 14 | } 15 | } 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /my_ground_plane/materials/textures/MyImage.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/my_ground_plane/materials/textures/MyImage.png -------------------------------------------------------------------------------- /my_ground_plane/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | My Ground Plane 4 | 1.0 5 | model.sdf 6 | My textured ground plane. 7 | 8 | 9 | -------------------------------------------------------------------------------- /my_ground_plane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 20 10 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | false 24 | 25 | 26 | 0 0 1 27 | 20 10 28 | 29 | 30 | 31 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /summit_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(summit_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | controller_manager 12 | gazebo_ros 13 | joint_state_publisher 14 | robot_state_publisher 15 | rospy 16 | rviz 17 | message_generation 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | EnvInfo.msg 57 | ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | StartUp.srv 63 | ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs # Or other packages containing msgs 76 | # ) 77 | 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES summit_description 115 | CATKIN_DEPENDS controller_manager gazebo_ros joint_state_publisher robot_state_publisher rospy rviz message_runtime 116 | # DEPENDS system_lib 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | # include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/summit_description.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/summit_description_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_summit_description.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /summit_description/Maps/path.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 15 | 16 | 0 0 -9.8 17 | 6e-06 2.3e-05 -4.2e-05 18 | 19 | 20 | 0.001 21 | 1 22 | 1000 23 | 24 | 25 | 0.4 0.4 0.4 1 26 | 0.7 0.7 0.7 1 27 | 1 28 | 29 | 30 | EARTH_WGS84 31 | 0 32 | 0 33 | 0 34 | 0 35 | 36 | 37 | 2917 327000000 38 | 34 563125474 39 | 1617791007 72572134 40 | 34021 41 | 42 | 0.072362 3.58357 0 0 -0 0 43 | 1 1 1 44 | 45 | 0.072362 3.58357 0 0 -0 0 46 | 0 0 0 0 -0 0 47 | 0 0 0 0 -0 0 48 | 0 0 0 0 -0 0 49 | 50 | 51 | 52 | 0 0 10 0 -0 0 53 | 54 | 55 | 56 | 57 | 0.208236 8.60475 19.6702 -0 1.42697 -1.36434 58 | orbit 59 | perspective 60 | 61 | 62 | 63 | 1 64 | 65 | 66 | 67 | 68 | 0 0 1 69 | 25 12 70 | 71 | 72 | 73 | 74 | 75 | 100 76 | 50 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 10 88 | 89 | 90 | 0 91 | 92 | 93 | 0 0 1 94 | 25 12 95 | 96 | 97 | 98 | 103 | 104 | 105 | 0 106 | 0 107 | 108 | 0.072362 3.58357 0 0 -0 0 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /summit_description/launch/pioneer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /summit_description/launch/summit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | d 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /summit_description/meshes/base/summit_base.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | VCGLab 6 | VCGLib | MeshLab 7 | 8 | Y_UP 9 | Fri Nov 27 08:11:04 2015 10 | Fri Nov 27 08:11:04 2015 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0.289292 -0.195869 0.25 0.291393 -0.190492 0.27 0.291393 -0.190492 0.25 0.292899 -0.200378 0.27 0.298607 -0.199508 0.25 0.297101 -0.189622 0.27 0.300708 -0.194131 0.27 0.298607 -0.199508 0.27 0.289292 -0.195869 0.27 -0.1475 -2.77556e-17 0.25 -0.301 -2.77556e-17 0.25 -0.301 0.195 0.25 -0.301 0.201 0.25 0.00536805 0.195 0.25 0.1475 -2.77556e-17 0.25 0.00360668 0.190492 0.25 -0.002101 0.189622 0.25 -0.00570768 0.194131 0.25 -0.1475 0.195 0.25 -0.1475 0.201 0.25 -0.289292 0.195869 0.25 0.002101 0.200378 0.25 0.1475 0.201 0.25 -0.00360668 0.199508 0.25 0.1475 0.195 0.25 0.291393 0.199508 0.25 0.300854 0.201 0.25 0.300368 0.195 0.25 0.1475 -0.201 0.25 -0.1475 -0.200689 0.25 -0.301 -0.200689 0.25 -0.297101 -0.200378 0.25 -0.300708 -0.195869 0.25 -0.298607 -0.190492 0.25 -0.289292 -0.194131 0.25 -0.00360668 -0.190492 0.25 0.1475 -0.200689 0.25 0.00570768 -0.194131 0.25 0.300708 -0.194131 0.25 0.292899 -0.200378 0.25 0.300854 -0.200689 0.25 0.297101 -0.189622 0.25 0.300854 -2.77556e-17 0.25 0.300854 0.195 0.25 0.301 0.195 0.25 0.297101 0.200378 0.27 0.297101 0.200378 0.25 0.289292 0.194131 0.27 0.289632 0.195 0.25 0.291393 0.199508 0.27 0.289292 0.194131 0.25 0.298607 0.190492 0.27 0.292899 0.189622 0.27 0.298607 0.190492 0.25 0.292899 0.189622 0.25 0.300708 0.195869 0.25 0.300708 0.195869 0.27 0.002101 -0.189622 0.27 -0.00570768 -0.195869 0.25 -0.002101 -0.200378 0.25 0.00360668 -0.199508 0.25 -0.002101 -0.200378 0.27 0.00360668 -0.199508 0.27 0.002101 -0.189622 0.25 0.00570768 -0.194131 0.27 -0.00570768 -0.195869 0.27 -0.00360668 -0.190492 0.27 0.002101 0.200378 0.27 -0.00360668 0.199508 0.27 -0.00536805 0.195 0.25 0.00360668 0.190492 0.27 0.00570768 0.195869 0.25 0.00570768 0.195869 0.27 -0.002101 0.189622 0.27 -0.00570768 0.194131 0.27 -0.292899 -0.189622 0.27 -0.292899 -0.189622 0.25 -0.300708 -0.195869 0.27 -0.291393 -0.199508 0.27 -0.297101 -0.200378 0.27 -0.291393 -0.199508 0.25 -0.289292 -0.194131 0.27 -0.298607 -0.190492 0.27 -0.298607 0.199508 0.25 -0.292899 0.200378 0.25 -0.300708 0.194131 0.25 -0.300708 0.194131 0.27 -0.300368 0.195 0.25 -0.298607 0.199508 0.27 -0.297101 0.189622 0.27 -0.291393 0.190492 0.25 -0.297101 0.189622 0.25 -0.289632 0.195 0.25 -0.291393 0.190492 0.27 -0.289292 0.195869 0.27 -0.292899 0.200378 0.27 0.332007 -0.192015 0.144 -0.376244 0.177701 0.144 -0.324034 0.20087 0.144 0.2985 -0.1045 0.139 0.2985 -0.1045 0.034 -0.2985 -0.1045 0.034 0.2985 0.1045 0.139 -0.2985 0.1045 0.139 -0.2985 0.1045 0.034 0.2985 0.1045 0.034 -0.2985 -0.1045 0.139 0.301 -0.201 0.139 0.301 -0.0702415 0.139 0.414058 -0.231705 0.139 0.352622 -0.274724 0.139 -0.301 0.201 0.139 0.301 -0.201 0.144 0.300854 -0.201 0.25 -0.301 -0.201 0.139 -0.1475 -0.201 0.25 0.301 -0.201 0.25 0.301 -0.200689 0.25 0.301 -2.77556e-17 0.25 0.301 0.201 0.139 0.301 0.201 0.25 -0.301 0.201 0.144 -0.301 0.0702415 0.144 -0.301 -0.201 0.25 -0.301 0.0702415 0.139 -0.314808 0.220719 0.144 -0.352622 0.274724 0.139 -0.352622 0.274724 0.144 -0.414058 0.231705 0.144 -0.414058 0.231705 0.139 0.314808 -0.220719 0.144 0.414058 -0.231705 0.144 0.301 -0.0702415 0.144 -0.36419 0.177686 0.144 -0.341648 0.185011 0.144 -0.313148 0.234374 0.148 -0.314393 0.246225 0.144 -0.314393 0.246225 0.148 -0.324034 0.267878 0.148 -0.341648 0.283738 0.148 -0.387762 0.288584 0.144 -0.408288 0.276733 0.144 -0.387762 0.288584 0.148 -0.42222 0.257558 0.144 -0.427148 0.234374 0.144 -0.427148 0.234374 0.148 -0.42222 0.21119 0.148 -0.408288 0.192015 0.144 -0.387762 0.180164 0.144 -0.387762 0.180164 0.148 -0.314393 0.222523 0.148 -0.324034 0.20087 0.148 -0.341648 0.185011 0.148 -0.408288 0.192015 0.148 -0.36419 0.177686 0.148 -0.42222 0.257558 0.148 -0.408288 0.276733 0.148 -0.36419 0.291062 0.148 -0.314393 0.222523 0.144 -0.324034 0.267878 0.144 -0.341648 0.283738 0.144 -0.36419 0.291062 0.144 -0.42222 0.21119 0.144 0.313148 -0.234374 0.148 0.332007 -0.276733 0.144 0.352534 -0.288584 0.144 0.376106 -0.291062 0.144 0.352534 -0.288584 0.148 0.398648 -0.283738 0.144 0.416262 -0.267878 0.148 0.427148 -0.234374 0.148 0.425902 -0.222523 0.148 0.416262 -0.20087 0.148 0.416262 -0.20087 0.144 0.376106 -0.177686 0.148 0.376106 -0.177686 0.144 0.352534 -0.180164 0.144 0.352534 -0.180164 0.148 0.318076 -0.21119 0.144 0.318076 -0.257558 0.148 0.425902 -0.246225 0.148 0.332007 -0.276733 0.148 0.398648 -0.283738 0.148 0.376106 -0.291062 0.148 0.318076 -0.21119 0.148 0.332007 -0.192015 0.148 0.398648 -0.185011 0.148 0.313148 -0.234374 0.144 0.318076 -0.257558 0.144 0.416262 -0.267878 0.144 0.376244 -0.177701 0.144 0.352622 -0.274724 0.144 0.425902 -0.246225 0.144 0.398648 -0.185011 0.144 0.425902 -0.222523 0.144 -0.24 0.114 0.105608 0.195858 0.2 0.139 -0.253656 0.114 0.0682224 0.150538 0.114 0.0211299 0.1475 0.114 -3.60524e-17 0.155732 0.114 -0.0341625 0.191344 0.114 0.0682224 0.24 0.114 0.105608 0.0508291 0.114 0.03 -0.0508291 0.114 0.03 -0.173385 0.114 0.0566812 -0.211826 0.114 0.0742366 -0.155732 0.109 -0.0341625 -0.150538 0.114 -0.0211299 -0.233174 0.114 0.0742366 -0.211826 0.119 0.0742366 -0.191344 0.114 0.0682224 -0.159406 0.114 0.0405481 -0.150538 0.119 0.0211299 -0.150538 0.114 0.0211299 -0.1475 0.119 -2.12521e-17 -0.1475 0.114 -2.12521e-17 -0.155732 0.114 -0.0341625 -0.159406 0.109 -0.0405481 -0.191344 0.109 -0.0682224 -0.191344 0.119 -0.0682224 -0.211826 0.119 -0.0742366 -0.233174 0.109 -0.0742366 -0.233174 0.119 -0.0742366 -0.253656 0.109 -0.0682224 -0.271615 0.109 -0.0566812 -0.285594 0.119 -0.0405481 -0.2975 0.119 6.30246e-18 -0.2975 0.109 -1.20672e-17 -0.294462 0.109 0.0211299 -0.294462 0.119 0.0211299 -0.271615 0.109 0.0566812 -0.271615 0.119 0.0566812 0.253656 0.114 0.0682224 0.150538 0.119 0.0211299 0.159406 0.114 0.0405481 0.173385 0.114 0.0566812 0.173385 0.119 0.0566812 0.211826 0.114 0.0742366 0.211826 0.119 0.0742366 0.233174 0.114 0.0742366 0.233174 0.119 0.0742366 0.159406 0.119 -0.0405481 0.150538 0.114 -0.0211299 0.150538 0.119 -0.0211299 0.263201 0.114 0.0629952 0.271615 0.119 0.0566812 0.285594 0.119 0.0405481 0.285594 0.109 0.0405481 0.294462 0.109 0.0211299 0.294462 0.119 0.0211299 0.2975 0.109 -4.52373e-17 0.294462 0.119 -0.0211299 0.285594 0.109 -0.0405481 0.285594 0.119 -0.0405481 0.271615 0.119 -0.0566812 0.271615 0.109 -0.0566812 0.253656 0.109 -0.0682224 0.253656 0.119 -0.0682224 0.233174 0.119 -0.0742366 0.233174 0.109 -0.0742366 0.211826 0.109 -0.0742366 0.191344 0.109 -0.0682224 0.173385 0.109 -0.0566812 0.155732 0.109 -0.0341625 0.0508291 0.109 0.03 -0.24 0.109 0.139 0.271615 0.109 0.0566812 0.263201 0.109 0.0629952 0.24 0.109 0.105608 0.159406 0.109 -0.0405481 -0.263201 0.109 0.0629952 -0.285594 0.109 0.0405481 -0.294462 0.109 -0.0211299 -0.285594 0.109 -0.0405481 -0.211826 0.109 -0.0742366 -0.173385 0.109 -0.0566812 0.294462 0.109 -0.0211299 -0.0508291 0.109 0.03 0.24 0.114 0.134 0.24 0.109 0.139 -0.25 0.2 0.139 -0.195858 0.2 0.139 -0.235858 0.24 0.139 -0.25 0.24 0.139 0.235858 0.24 0.139 0.24 0.178 0.139 -0.195858 0.2 0.134 0.195858 0.2 0.134 0.24 0.178 0.134 -0.24 0.114 0.134 0.25 0.24 0.134 0.253656 0.119 0.0682224 0.191344 0.119 0.0682224 0.2975 0.119 -4.52373e-17 0.159406 0.119 0.0405481 0.211826 0.119 -0.0742366 0.191344 0.119 -0.0682224 0.173385 0.119 -0.0566812 0.1475 0.119 -1.76827e-17 -0.253656 0.119 -0.0682224 -0.173385 0.119 -0.0566812 -0.191344 0.119 0.0682224 -0.159406 0.119 -0.0405481 -0.173385 0.119 0.0566812 -0.159406 0.119 0.0405481 -0.150538 0.119 -0.0211299 -0.233174 0.119 0.0742366 -0.271615 0.119 -0.0566812 -0.294462 0.119 -0.0211299 -0.253656 0.119 0.0682224 -0.285594 0.119 0.0405481 0.235858 0.24 0.134 0.25 0.24 0.139 0.25 0.2 0.134 0.25 0.2 0.139 -0.25 0.24 0.134 -0.235858 0.24 0.134 -0.24 0.178 0.134 -0.25 0.2 0.134 -0.24 0.178 0.139 -0.263201 0.114 0.0629952 -0.24 0.109 0.105608 -0.24 -0.178 0.134 -0.24 -0.178 0.139 0.195858 -0.2 0.139 0.195858 -0.2 0.134 -0.24 -0.114 0.134 -0.150538 -0.114 0.0211299 0.0508291 -0.114 0.03 0.1475 -0.114 -5.44221e-17 0.159406 -0.114 0.0405481 -0.191344 -0.114 0.0682224 -0.0508291 -0.114 0.03 -0.271615 -0.109 0.0566812 -0.150538 -0.114 -0.0211299 -0.1475 -0.114 -2.88239e-18 -0.1475 -0.119 -2.12521e-17 -0.159406 -0.114 0.0405481 -0.173385 -0.114 0.0566812 -0.173385 -0.119 0.0566812 -0.211826 -0.119 0.0742366 -0.211826 -0.114 0.0742366 -0.233174 -0.114 0.0742366 -0.233174 -0.119 0.0742366 -0.253656 -0.114 0.0682224 -0.253656 -0.119 0.0682224 -0.271615 -0.119 0.0566812 -0.294462 -0.109 0.0211299 -0.2975 -0.109 -3.04369e-17 -0.294462 -0.109 -0.0211299 -0.294462 -0.119 -0.0211299 -0.271615 -0.109 -0.0566812 -0.253656 -0.119 -0.0682224 -0.233174 -0.109 -0.0742366 -0.233174 -0.119 -0.0742366 -0.211826 -0.119 -0.0742366 -0.211826 -0.109 -0.0742366 -0.191344 -0.109 -0.0682224 -0.173385 -0.109 -0.0566812 -0.173385 -0.119 -0.0566812 -0.159406 -0.119 -0.0405481 -0.155732 -0.109 -0.0341625 0.155732 -0.109 -0.0341625 0.159406 -0.109 -0.0405481 0.233174 -0.114 0.0742366 0.211826 -0.119 0.0742366 0.211826 -0.114 0.0742366 0.191344 -0.114 0.0682224 0.191344 -0.119 0.0682224 0.173385 -0.119 0.0566812 0.173385 -0.114 0.0566812 0.150538 -0.114 0.0211299 0.150538 -0.119 0.0211299 0.150538 -0.114 -0.0211299 0.1475 -0.119 -7.27919e-17 0.150538 -0.119 -0.0211299 0.159406 -0.119 -0.0405481 0.271615 -0.119 0.0566812 0.253656 -0.114 0.0682224 0.155732 -0.114 -0.0341625 0.173385 -0.109 -0.0566812 0.211826 -0.109 -0.0742366 0.191344 -0.119 -0.0682224 0.211826 -0.119 -0.0742366 0.233174 -0.119 -0.0742366 0.233174 -0.109 -0.0742366 0.253656 -0.109 -0.0682224 0.253656 -0.119 -0.0682224 0.271615 -0.109 -0.0566812 0.285594 -0.109 -0.0405481 0.294462 -0.109 -0.0211299 0.294462 -0.119 -0.0211299 0.2975 -0.109 -4.52373e-17 0.2975 -0.119 -4.52373e-17 0.294462 -0.119 0.0211299 0.285594 -0.119 0.0405481 0.263201 -0.109 0.0629952 0.24 -0.109 0.139 0.271615 -0.109 0.0566812 -0.253656 -0.109 -0.0682224 -0.285594 -0.109 -0.0405481 -0.159406 -0.109 -0.0405481 0.2975 -0.109 -2.68676e-17 0.294462 -0.109 0.0211299 0.285594 -0.109 0.0405481 -0.285594 -0.109 0.0405481 -0.263201 -0.109 0.0629952 -0.24 -0.109 0.105608 0.191344 -0.109 -0.0682224 0.0508291 -0.109 0.03 0.24 -0.109 0.105608 0.24 -0.178 0.134 0.24 -0.178 0.139 -0.24 -0.109 0.139 -0.25 -0.2 0.139 0.25 -0.2 0.139 -0.195858 -0.2 0.134 -0.25 -0.2 0.134 0.25 -0.2 0.134 0.24 -0.114 0.134 0.271615 -0.119 -0.0566812 0.285594 -0.119 -0.0405481 0.2975 -0.119 -2.68676e-17 0.253656 -0.119 0.0682224 0.233174 -0.119 0.0742366 0.173385 -0.119 -0.0566812 0.159406 -0.119 0.0405481 -0.285594 -0.119 0.0405481 -0.294462 -0.119 0.0211299 -0.150538 -0.119 0.0211299 -0.159406 -0.119 0.0405481 -0.191344 -0.119 -0.0682224 -0.150538 -0.119 -0.0211299 -0.1475 -0.119 -2.88239e-18 -0.191344 -0.119 0.0682224 -0.271615 -0.119 -0.0566812 -0.285594 -0.119 -0.0405481 -0.2975 -0.119 -4.88066e-17 0.235858 -0.24 0.134 0.25 -0.24 0.134 0.235858 -0.24 0.139 0.25 -0.24 0.139 -0.25 -0.24 0.134 -0.235858 -0.24 0.139 -0.25 -0.24 0.139 -0.235858 -0.24 0.134 -0.195858 -0.2 0.139 -0.263201 -0.114 0.0629952 -0.24 -0.114 0.105608 0.263201 -0.114 0.0629952 0.24 -0.114 0.105608 -0.155732 -0.114 -0.0341625 -0.0508291 -0.109 0.03 0.325 -0.162 0.275 0.325 -0.205 0.275 0.325 -0.205 0.27 -0.325 0.205 0.275 -0.325 -0.205 0.275 0.325 0.205 0.275 -0.325 -0.205 0.27 0.325 0.205 0.27 0.417 0.158 0.27 0.325 0.158 0.27 0.325 -0.162 0.27 -0.325 0.205 0.27 0.417 -0.162 0.27 0.325 0.158 0.275 0.417 0.158 0.275 0.417 -0.162 0.275 0.3125 -0.114 0.123125 0.3125 0.114 0.091125 0.3155 -0.119 0.091125 0.2125 0.114 0.131125 0.2125 0.119 0.099125 0.2375 0.119 0.131125 0.2375 0.114 0.131125 0.2125 0.119 0.131125 0.290185 0.119 0.091125 0.290185 0.114 0.091125 0.2925 0.119 0.123125 0.2925 -0.114 0.123125 0.2925 -0.119 0.123125 0.3155 -0.119 0.123125 0.3125 0.114 0.123125 0.2925 0.114 0.123125 0.235185 0.119 0.099125 0.235185 0.114 0.099125 0.2125 0.114 0.099125 0.2125 -0.119 0.099125 0.3155 0.119 0.091125 0.3155 0.119 0.123125 0.2125 -0.114 0.131125 0.2125 -0.114 0.099125 0.2125 -0.119 0.131125 0.2375 -0.119 0.131125 0.3125 -0.114 0.091125 0.290185 -0.114 0.091125 0.2375 -0.114 0.131125 0.235185 -0.114 0.099125 0.290185 -0.119 0.091125 0.235185 -0.119 0.099125 -0.3125 0.114 0.091125 -0.3125 -0.114 0.091125 -0.290185 0.114 0.091125 -0.2125 0.114 0.099125 -0.2125 0.119 0.099125 -0.2125 0.119 0.131125 -0.2125 0.114 0.131125 -0.2375 0.114 0.131125 -0.3155 0.119 0.123125 -0.2925 0.119 0.123125 -0.2375 0.119 0.131125 -0.2925 0.114 0.123125 -0.3125 0.114 0.123125 -0.3125 -0.114 0.123125 -0.290185 0.119 0.091125 -0.235185 0.114 0.099125 -0.235185 0.119 0.099125 -0.3155 -0.119 0.123125 -0.2925 -0.119 0.123125 -0.3155 -0.119 0.091125 -0.2125 -0.119 0.099125 -0.3155 0.119 0.091125 -0.2125 -0.114 0.131125 -0.2375 -0.119 0.131125 -0.2125 -0.119 0.131125 -0.2125 -0.114 0.099125 -0.235185 -0.114 0.099125 -0.290185 -0.114 0.091125 -0.2375 -0.114 0.131125 -0.2925 -0.114 0.123125 -0.290185 -0.119 0.091125 -0.235185 -0.119 0.099125 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931436 0.363905 0 -0.931436 0.363905 0 -0.780869 -0.624695 0 -0.780869 -0.624695 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931435 -0.363909 0 0.931435 -0.363909 0 0.780866 0.624698 0 0.780866 0.624698 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931435 0.363907 0 -0.931436 0.363905 -1.45714e-07 -0.931436 0.363904 0 -0.780869 -0.624695 0 -0.780869 -0.624695 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931426 -0.36393 0 0.931435 -0.363909 -1.03772e-06 0.931436 -0.363905 0 0.780866 0.624698 0 0.780866 0.624698 0 0 0 1 0 0 1 0 0 1 0 0 1 -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931436 0.363905 0 -0.931436 0.363905 0 -0.780869 -0.624695 0 -0.780869 -0.624695 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931436 -0.363905 0 0.931436 -0.363905 0 0.780869 0.624695 0 0.780869 0.624695 0 0 0 1 0 0 1 0 0 1 0 0 1 -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931434 0.363909 0 -0.931436 0.363905 -2.25463e-07 -0.931437 0.363904 0 -0.780869 -0.624695 0 -0.780869 -0.624695 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931437 -0.363904 0 0.931436 -0.363905 4.62741e-08 0.931436 -0.363905 0 0.780869 0.624695 0 0.780869 0.624695 0 0 0 1 0 0 1 0 0 1 0 0 1 -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931435 0.363909 0 -0.931435 0.363909 0 -0.780866 -0.624698 0 -0.780866 -0.624698 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931436 -0.363905 0 0.931436 -0.363905 0 0.780869 0.624695 0 0.780869 0.624695 0 0 0 1 0 0 1 0 0 1 0 0 1 -0.150565 0.9886 0 -0.150565 0.9886 0 -0.931424 0.363935 0 -0.931435 0.363909 -1.30946e-06 -0.931436 0.363904 0 -0.780866 -0.624698 0 -0.780866 -0.624698 0 0.150565 -0.9886 0 0.150565 -0.9886 0 0.931437 -0.363902 0 0.931436 -0.363905 1.26023e-07 0.931436 -0.363905 0 0.780869 0.624695 0 0.780869 0.624695 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0.819152 0.573577 0 0.819152 0.573577 1.55221e-06 0.819152 0.573576 0 -0.573577 0.819152 0 -0.573577 0.819152 0 -0.819152 -0.573576 0 -0.819152 -0.573576 -9.44973e-07 -0.819152 -0.573576 0 -0.819152 -0.573577 0 -0.819152 -0.573577 1.55221e-06 -0.819152 -0.573576 0 0.573577 -0.819152 0 0.573577 -0.819152 0 0.819152 0.573576 0 0.819152 0.573576 -9.44973e-07 0.819152 0.573576 0 -0.00121763 -0.999999 0 0.309017 -0.951056 0 0.309017 -0.951056 0 0.66913 -0.743145 0 0.66913 -0.743145 0 0.906822 -0.421514 0 0.909962 -0.405141 0.0884879 0.974641 -0.223774 0 0.994522 -0.104529 0 0.954779 0 -0.297315 0.994522 0.104529 0 0.913545 0.406737 0 0.913545 0.406737 0 0.669131 0.743145 0 0.669131 0.743145 0 0.309017 0.951056 0 0.309017 0.951056 0 -0.104528 0.994522 0 -0.104528 0.994522 0 -0.5 0.866026 0 -0.5 0.866026 0 -0.809018 0.587784 0 -0.809018 0.587784 0 -0.978147 0.207913 0 -0.978147 0.207913 0 -0.978147 -0.207913 0 -0.978147 -0.207913 0 -0.809017 -0.587785 0 -0.809017 -0.587785 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.209102 -0.977894 0 -0.0998029 -0.949561 0.297277 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.978147 -0.207913 0 -0.809018 -0.587784 0 -0.809018 -0.587784 0 -0.5 -0.866026 0 -0.5 -0.866026 0 -0.104528 -0.994522 0 -0.104528 -0.994522 0 0.309017 -0.951056 0 0.309017 -0.951056 0 0.669131 -0.743145 0 0.669131 -0.743145 0 0.913545 -0.406737 0 0.913545 -0.406737 0 0.994522 -0.104529 0 0.954779 0 -0.297315 0.994522 0.104529 0 0.913545 0.406737 0 0.913545 0.406737 0 0.66913 0.743145 0 0.66913 0.743145 0 0.310175 0.95068 0 0.309009 0.951032 0.00717217 0.105752 0.994393 0 -0.104528 0.994522 0 -0.104528 0.994522 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.809017 0.587785 0 -0.809017 0.587785 0 -0.945914 0.324417 0 -0.935889 0.19893 0.290754 -0.992694 0.120662 0 -0.978147 -0.207913 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0.86679 0 -0.498673 -0.534451 0.150886 0.831622 0.909164 0.205222 -0.362359 -0.480315 0 0.877096 -0.281733 0 0.959493 -0.281733 0 0.959493 0 0 1 0 0 1 0.281733 0 0.959493 0.281733 0 0.959493 0.540641 0 0.841254 0.540641 0 0.841254 0.75575 0 0.65486 0.75575 0 0.65486 0.909632 0 0.415415 0.909632 0 0.415415 0.989821 0 0.142315 0.989821 0 0.142315 0.989821 0 -0.142315 0.989821 0 -0.142315 0.909632 0 -0.415415 0.86679 0 -0.498673 0.75575 0 -0.65486 0.75575 0 -0.65486 0.540641 0 -0.841254 0.540641 0 -0.841254 0.281733 0 -0.959493 0.281733 0 -0.959493 0 0 -1 0 0 -1 -0.281733 0 -0.959493 -0.281733 0 -0.959493 -0.540641 0 -0.841254 -0.540641 0 -0.841254 -0.75575 0 -0.654861 -0.75575 0 -0.654861 -0.909632 0 -0.415415 -0.909632 0 -0.415415 -0.989821 0 -0.142316 -0.989821 0 -0.142316 -0.989821 -2.6143e-16 0.142316 -0.989821 0 0.142316 -0.909632 0 0.415415 -0.909632 0 0.415415 -0.75575 0 0.654861 -0.75575 0 0.654861 -0.60025 0 0.799812 -0.60025 0 0.799812 0.60025 0 0.799812 0.460135 0.286821 0.840244 -0.989821 0 -0.142315 -0.989821 -5.22857e-16 0.142315 -0.989821 0 0.142315 -0.909632 0 0.415415 -0.909632 0 0.415415 -0.75575 0 0.65486 -0.75575 0 0.65486 -0.540641 0 0.841254 -0.540641 0 0.841254 -0.281733 0 0.959493 -0.281733 0 0.959493 0 0 1 0 0 1 0.281733 0 0.959493 0.281733 0 0.959493 0.540641 0 0.841254 -0.901116 0.136517 -0.411526 -0.928936 0 -0.370239 -0.989821 0 -0.142315 0.60025 0 0.799812 0.75575 0 0.654861 0.75575 0 0.654861 0.909632 0 0.415415 0.909632 0 0.415415 0.989821 0 0.142316 0.989821 0 0.142316 0.989821 0 -0.142316 0.989821 0 -0.142316 0.909632 0 -0.415415 0.909632 0 -0.415415 0.75575 0 -0.654861 0.75575 0 -0.654861 0.540641 0 -0.841254 0.540641 0 -0.841254 0.281733 0 -0.959493 0.281733 0 -0.959493 0 0 -1 0 0 -1 -0.281733 0 -0.959493 -0.281733 0 -0.959493 -0.540641 0 -0.841254 -0.540641 0 -0.841254 -0.75575 0 -0.65486 -0.75575 0 -0.65486 -0.86679 0 -0.498673 -0.86679 0 -0.498673 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 1 0 0 1 0 0 1 0 0 1 0 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.707107 0.707107 0 -0.707107 0.707107 0 0 1 0 0 1 0 1 0 0 1 0 0 0.910366 -0.413803 0 0.910366 -0.413803 0 0 1 0 0 1 0 -1 0 0 -1 0 0 -0.910366 -0.413803 0 -0.910366 -0.413803 0 0.707107 0.707107 0 0.707107 0.707107 0 -0.878261 0 0.478182 -0.878261 0 0.478182 0.878261 0 0.478182 0.878261 0 0.478182 0.521776 0 -0.853083 0.521776 0 -0.853083 -0.521776 0 -0.853083 -0.521776 0 -0.853083 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0.60025 0 0.799812 0.901116 -0.136516 -0.411526 -0.460135 -0.286821 0.840244 0.928936 0 -0.370239 0.989821 0 -0.142315 0.989821 0 -0.142315 0.989821 -5.22857e-16 0.142315 0.989821 0 0.142315 0.909632 0 0.415415 0.909632 0 0.415415 0.75575 0 0.65486 0.75575 0 0.65486 0.540641 0 0.841254 0.540641 0 0.841254 0.281733 0 0.959493 0.281733 0 0.959493 0 0 1 0 0 1 -0.281733 0 0.959493 -0.281733 0 0.959493 -0.540641 0 0.841254 -0.60025 0 0.799812 -0.75575 0 0.654861 -0.75575 0 0.654861 -0.909632 0 0.415415 -0.909632 0 0.415415 -0.989821 0 0.142316 -0.989821 0 0.142316 -0.989821 2.6143e-16 -0.142316 -0.989821 0 -0.142316 -0.909632 0 -0.415415 -0.909632 0 -0.415415 -0.75575 0 -0.654861 -0.75575 0 -0.654861 -0.540641 0 -0.841254 -0.540641 0 -0.841254 -0.281733 0 -0.959493 -0.281733 0 -0.959493 0 0 -1 0 0 -1 0.281733 0 -0.959493 0.281733 0 -0.959493 0.540641 0 -0.841254 0.540641 0 -0.841254 0.75575 0 -0.65486 0.75575 0 -0.65486 0.86679 0 -0.498673 0.86679 0 -0.498673 -0.86679 0 -0.498673 -0.909164 -0.205222 -0.362359 0.281733 0 0.959493 0 0 1 0 0 1 -0.281733 0 0.959493 -0.281733 0 0.959493 -0.540641 0 0.841254 -0.540641 0 0.841254 -0.75575 0 0.65486 -0.75575 0 0.65486 -0.909632 0 0.415415 -0.909632 0 0.415415 -0.989821 0 0.142315 -0.989821 0 0.142315 -0.989821 5.22857e-16 -0.142315 -0.989821 0 -0.142315 -0.909632 0 -0.415415 0.534451 -0.150886 0.831622 0.480315 0 0.877096 0.281733 0 0.959493 -0.86679 0 -0.498673 -0.75575 0 -0.65486 -0.75575 0 -0.65486 -0.540641 0 -0.841254 -0.540641 0 -0.841254 -0.281733 0 -0.959493 -0.281733 0 -0.959493 0 0 -1 0 0 -1 0.281733 0 -0.959493 0.281733 0 -0.959493 0.540641 0 -0.841254 0.540641 0 -0.841254 0.75575 0 -0.654861 0.75575 0 -0.654861 0.909632 0 -0.415415 0.909632 0 -0.415415 0.989821 0 -0.142316 0.989821 0 -0.142316 0.989821 0 0.142316 0.989821 0 0.142316 0.909632 0 0.415415 0.909632 0 0.415415 0.75575 0 0.654861 0.75575 0 0.654861 0.60025 0 0.799812 0.60025 0 0.799812 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 -1 0 0 -1 1 0 0 1 0 0 1 0 0 1 0 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0.707107 -0.707107 0 -0.707107 -0.707107 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0.910366 0.413803 0 0.910366 0.413803 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -0.910366 0.413803 0 -0.910366 0.413803 0 0.707107 -0.707107 0 0.707107 -0.707107 0 -0.878261 0 0.478182 -0.878261 0 0.478182 0.878261 0 0.478182 0.878261 0 0.478182 0.521776 0 -0.853083 0.521776 0 -0.853083 -0.521776 0 -0.853083 -0.521776 0 -0.853083 1 0 0 1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 1 0 0 1 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0.14394 0 0.989586 0.14394 0 0.989586 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.14394 0 -0.989586 -0.14394 0 -0.989586 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0.14394 0 0.989586 0.14394 0 0.989586 -0.14394 0 -0.989586 -0.14394 0 -0.989586 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 1 0 0 1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0.14394 0 0.989586 -0.14394 0 0.989586 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0.14394 0 -0.989586 0.14394 0 -0.989586 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 1 0 0 1 0 0 0 0 1 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.14394 0 0.989586 -0.14394 0 0.989586 0.14394 0 -0.989586 0.14394 0 -0.989586 0 0 -1 0 0 -1 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 |

1 0 5 0 2 0 2 1 5 1 41 1 8 2 1 2 0 2 0 3 1 3 2 3 3 4 8 4 39 4 39 5 8 5 0 5 7 6 3 6 4 6 4 7 3 7 39 7 6 8 7 8 38 8 38 9 7 9 4 9 5 10 6 10 41 10 41 11 6 11 38 11 3 12 7 12 8 12 8 13 7 13 6 13 8 14 6 14 1 14 1 15 6 15 5 15 113 16 116 16 117 16 20 17 92 17 18 17 18 18 92 18 9 18 9 19 92 19 90 19 9 20 90 20 10 20 90 21 91 21 10 21 10 22 91 22 85 22 10 23 85 23 11 23 11 24 85 24 87 24 11 25 87 25 12 25 87 26 83 26 12 26 12 27 83 27 84 27 12 28 84 28 19 28 24 29 13 29 14 29 14 30 13 30 15 30 14 31 15 31 9 31 9 32 15 32 16 32 9 33 16 33 18 33 18 34 16 34 17 34 17 35 69 35 18 35 18 36 69 36 19 36 18 37 19 37 20 37 20 38 19 38 84 38 71 39 22 39 21 39 21 40 22 40 19 40 21 41 19 41 23 41 23 42 19 42 69 42 48 43 22 43 24 43 24 44 22 44 71 44 24 45 71 45 13 45 48 46 25 46 22 46 22 47 25 47 46 47 22 48 46 48 26 48 26 49 46 49 55 49 26 50 55 50 43 50 43 51 55 51 27 51 43 52 27 52 42 52 42 53 27 53 53 53 42 54 53 54 14 54 14 55 53 55 54 55 14 56 54 56 24 56 24 57 54 57 50 57 24 58 50 58 48 58 113 59 40 59 28 59 28 60 40 60 36 60 28 61 36 61 115 61 115 62 36 62 29 62 115 63 29 63 123 63 123 64 29 64 30 64 34 65 80 65 29 65 29 66 80 66 31 66 29 67 31 67 30 67 30 68 31 68 32 68 30 69 32 69 10 69 10 70 32 70 33 70 10 71 33 71 9 71 9 72 33 72 76 72 9 73 76 73 29 73 29 74 76 74 34 74 37 75 60 75 36 75 36 76 60 76 59 76 36 77 59 77 29 77 29 78 59 78 58 78 29 79 58 79 9 79 9 80 58 80 35 80 9 81 35 81 14 81 14 82 35 82 63 82 14 83 63 83 36 83 36 84 63 84 37 84 40 85 42 85 38 85 38 86 4 86 40 86 40 87 4 87 39 87 40 88 39 88 36 88 36 89 39 89 0 89 36 90 0 90 14 90 14 91 0 91 2 91 14 92 2 92 42 92 42 93 2 93 41 93 42 94 41 94 38 94 113 95 117 95 40 95 40 96 117 96 118 96 40 97 118 97 42 97 42 98 118 98 44 98 42 99 44 99 43 99 43 100 44 100 120 100 43 101 120 101 26 101 49 102 45 102 25 102 25 103 45 103 46 103 50 104 47 104 48 104 48 105 47 105 49 105 48 106 49 106 25 106 52 107 47 107 54 107 54 108 47 108 50 108 51 109 52 109 53 109 53 110 52 110 54 110 55 111 56 111 27 111 27 112 56 112 51 112 27 113 51 113 53 113 45 114 56 114 46 114 46 115 56 115 55 115 52 116 51 116 47 116 47 117 51 117 56 117 47 118 56 118 49 118 49 119 56 119 45 119 66 120 57 120 35 120 35 121 57 121 63 121 65 122 66 122 58 122 58 123 66 123 35 123 61 124 65 124 59 124 59 125 65 125 58 125 62 126 61 126 60 126 60 127 61 127 59 127 64 128 62 128 37 128 37 129 62 129 60 129 57 130 64 130 63 130 63 131 64 131 37 131 61 132 62 132 65 132 65 133 62 133 64 133 65 134 64 134 66 134 66 135 64 135 57 135 68 136 67 136 23 136 23 137 67 137 21 137 17 138 74 138 69 138 69 139 74 139 68 139 69 140 68 140 23 140 73 141 74 141 16 141 16 142 74 142 17 142 70 143 73 143 15 143 15 144 73 144 16 144 71 145 72 145 13 145 13 146 72 146 70 146 13 147 70 147 15 147 67 148 72 148 21 148 21 149 72 149 71 149 73 150 70 150 74 150 74 151 70 151 72 151 74 152 72 152 68 152 68 153 72 153 67 153 82 154 75 154 33 154 33 155 75 155 76 155 77 156 82 156 32 156 32 157 82 157 33 157 79 158 77 158 31 158 31 159 77 159 32 159 78 160 79 160 80 160 80 161 79 161 31 161 81 162 78 162 34 162 34 163 78 163 80 163 75 164 81 164 76 164 76 165 81 165 34 165 79 166 78 166 77 166 77 167 78 167 81 167 77 168 81 168 82 168 82 169 81 169 75 169 88 170 95 170 83 170 83 171 95 171 84 171 85 172 86 172 87 172 87 173 86 173 88 173 87 174 88 174 83 174 89 175 86 175 91 175 91 176 86 176 85 176 93 177 89 177 90 177 90 178 89 178 91 178 20 179 94 179 92 179 92 180 94 180 93 180 92 181 93 181 90 181 95 182 94 182 84 182 84 183 94 183 20 183 89 184 93 184 86 184 86 185 93 185 94 185 86 186 94 186 88 186 88 187 94 187 95 187 96 188 176 188 132 188 132 189 176 189 175 189 132 190 175 190 190 190 96 191 132 191 178 191 178 192 132 192 112 192 178 193 112 193 130 193 97 194 122 194 133 194 133 195 122 195 134 195 134 196 122 196 98 196 98 197 122 197 121 197 98 198 121 198 125 198 99 199 106 199 100 199 100 200 106 200 101 200 102 201 99 201 105 201 105 202 99 202 100 202 103 203 102 203 104 203 104 204 102 204 105 204 106 205 103 205 101 205 101 206 103 206 104 206 104 207 105 207 101 207 101 208 105 208 100 208 129 209 103 209 124 209 124 210 103 210 106 210 124 211 106 211 114 211 114 212 106 212 107 212 107 213 106 213 99 213 107 214 99 214 110 214 102 215 108 215 99 215 99 216 108 216 109 216 99 217 109 217 110 217 129 218 126 218 103 218 103 219 126 219 111 219 103 220 111 220 102 220 102 221 111 221 119 221 102 222 119 222 108 222 114 223 107 223 112 223 115 224 123 224 114 224 112 225 116 225 113 225 112 226 113 226 114 226 114 227 113 227 28 227 114 228 28 228 115 228 132 229 108 229 119 229 116 230 112 230 117 230 117 231 112 231 132 231 117 232 132 232 118 232 118 233 132 233 44 233 44 234 132 234 119 234 44 235 119 235 120 235 26 236 120 236 119 236 119 237 111 237 121 237 121 238 12 238 19 238 121 239 19 239 119 239 119 240 19 240 22 240 119 241 22 241 26 241 12 242 121 242 11 242 11 243 121 243 122 243 11 244 122 244 10 244 114 245 123 245 30 245 10 246 122 246 30 246 30 247 122 247 124 247 30 248 124 248 114 248 121 249 111 249 125 249 125 250 111 250 126 250 125 251 126 251 127 251 128 252 127 252 129 252 129 253 127 253 126 253 128 254 129 254 97 254 97 255 129 255 124 255 97 256 124 256 122 256 112 257 107 257 130 257 130 258 107 258 110 258 130 259 110 259 191 259 131 260 191 260 109 260 109 261 191 261 110 261 131 262 109 262 190 262 190 263 109 263 108 263 190 264 108 264 132 264 97 265 133 265 154 265 154 266 133 266 134 266 154 267 134 267 152 267 152 268 134 268 98 268 152 269 98 269 151 269 151 270 98 270 125 270 151 271 125 271 150 271 150 272 125 272 158 272 150 273 158 273 135 273 135 274 158 274 136 274 135 275 136 275 137 275 137 276 136 276 159 276 137 277 159 277 138 277 138 278 159 278 160 278 138 279 160 279 139 279 139 280 160 280 161 280 139 281 161 281 157 281 157 282 161 282 140 282 157 283 140 283 142 283 142 284 140 284 141 284 142 285 141 285 156 285 156 286 141 286 143 286 156 287 143 287 155 287 155 288 143 288 144 288 155 289 144 289 145 289 145 290 144 290 162 290 145 291 162 291 146 291 146 292 162 292 147 292 146 293 147 293 153 293 153 294 147 294 148 294 153 295 148 295 149 295 149 296 148 296 97 296 149 297 97 297 154 297 135 298 145 298 150 298 150 299 145 299 146 299 150 300 146 300 151 300 151 301 146 301 153 301 151 302 153 302 152 302 152 303 153 303 149 303 152 304 149 304 154 304 135 305 137 305 145 305 145 306 137 306 138 306 145 307 138 307 139 307 155 308 145 308 156 308 156 309 145 309 139 309 156 310 139 310 142 310 142 311 139 311 157 311 158 312 125 312 127 312 160 313 159 313 136 313 136 314 158 314 160 314 160 315 158 315 127 315 160 316 127 316 161 316 97 317 148 317 128 317 128 318 148 318 147 318 140 319 161 319 141 319 141 320 161 320 127 320 141 321 127 321 143 321 143 322 127 322 128 322 143 323 128 323 144 323 144 324 128 324 147 324 144 325 147 325 162 325 163 326 188 326 179 326 179 327 188 327 164 327 179 328 164 328 181 328 181 329 164 329 165 329 181 330 165 330 167 330 167 331 165 331 166 331 167 332 166 332 183 332 183 333 166 333 168 333 183 334 168 334 182 334 182 335 168 335 189 335 182 336 189 336 169 336 169 337 189 337 192 337 169 338 192 338 180 338 180 339 192 339 170 339 170 340 192 340 194 340 170 341 194 341 171 341 171 342 194 342 173 342 171 343 173 343 172 343 172 344 173 344 193 344 172 345 193 345 186 345 186 346 193 346 190 346 186 347 190 347 174 347 190 348 175 348 174 348 174 349 175 349 176 349 174 350 176 350 177 350 177 351 176 351 96 351 177 352 96 352 185 352 185 353 96 353 178 353 185 354 178 354 184 354 184 355 178 355 130 355 184 356 130 356 163 356 163 357 130 357 187 357 163 358 187 358 188 358 170 359 163 359 180 359 180 360 163 360 179 360 180 361 179 361 169 361 169 362 179 362 181 362 169 363 181 363 182 363 182 364 181 364 167 364 182 365 167 365 183 365 170 366 171 366 163 366 163 367 171 367 172 367 163 368 172 368 186 368 184 369 163 369 185 369 185 370 163 370 186 370 185 371 186 371 177 371 177 372 186 372 174 372 187 373 130 373 191 373 187 374 165 374 188 374 188 375 165 375 164 375 191 376 189 376 168 376 131 377 190 377 193 377 187 378 191 378 165 378 165 379 191 379 168 379 165 380 168 380 166 380 189 381 191 381 192 381 192 382 191 382 131 382 192 383 131 383 194 383 194 384 131 384 193 384 194 385 193 385 173 385 195 386 322 386 290 386 290 387 322 387 266 387 290 388 266 388 318 388 318 389 266 389 320 389 282 390 196 390 287 390 287 391 196 391 288 391 203 392 279 392 202 392 279 393 203 393 290 393 290 394 203 394 204 394 290 395 204 395 195 395 235 396 198 396 203 396 321 397 195 397 197 397 197 398 195 398 209 398 240 399 202 399 233 399 233 400 202 400 245 400 198 401 199 401 203 401 203 402 199 402 243 402 203 403 243 403 200 403 240 404 238 404 202 404 202 405 238 405 201 405 202 406 201 406 203 406 203 407 201 407 236 407 203 408 236 408 235 408 204 409 217 409 208 409 208 410 216 410 204 410 204 411 216 411 214 411 204 412 214 412 212 412 212 413 205 413 204 413 204 414 205 414 211 414 204 415 211 415 195 415 195 416 211 416 206 416 195 417 206 417 209 417 217 418 207 418 218 418 310 419 232 419 321 419 208 420 217 420 303 420 321 421 197 421 310 421 310 422 197 422 209 422 310 423 209 423 307 423 307 424 209 424 206 424 307 425 206 425 210 425 210 426 206 426 211 426 210 427 211 427 302 427 302 428 211 428 205 428 302 429 205 429 304 429 304 430 205 430 212 430 304 431 212 431 305 431 305 432 212 432 214 432 305 433 214 433 213 433 213 434 214 434 216 434 213 435 216 435 215 435 215 436 216 436 208 436 215 437 208 437 306 437 306 438 208 438 303 438 217 439 218 439 303 439 303 440 218 440 276 440 303 441 276 441 301 441 301 442 276 442 219 442 301 443 219 443 220 443 220 444 219 444 275 444 220 445 275 445 221 445 221 446 275 446 222 446 221 447 222 447 223 447 223 448 222 448 224 448 223 449 224 449 300 449 300 450 224 450 225 450 300 451 225 451 308 451 308 452 225 452 274 452 308 453 274 453 226 453 226 454 274 454 273 454 226 455 273 455 309 455 309 456 273 456 228 456 309 457 228 457 227 457 227 458 228 458 229 458 227 459 229 459 230 459 230 460 229 460 272 460 230 461 272 461 311 461 311 462 272 462 231 462 311 463 231 463 232 463 232 464 231 464 271 464 232 465 271 465 321 465 245 466 268 466 267 466 233 467 245 467 246 467 244 468 199 468 299 468 299 469 199 469 198 469 299 470 198 470 234 470 234 471 198 471 235 471 234 472 235 472 295 472 295 473 235 473 236 473 295 474 236 474 237 474 237 475 236 475 201 475 237 476 201 476 293 476 293 477 201 477 238 477 293 478 238 478 239 478 239 479 238 479 240 479 239 480 240 480 241 480 241 481 240 481 233 481 241 482 233 482 292 482 292 483 233 483 246 483 242 484 200 484 244 484 244 485 200 485 243 485 244 486 243 486 199 486 245 487 267 487 246 487 246 488 267 488 248 488 246 489 248 489 247 489 247 490 248 490 249 490 247 491 249 491 250 491 250 492 249 492 251 492 250 493 251 493 294 493 294 494 251 494 277 494 294 495 277 495 252 495 252 496 277 496 253 496 252 497 253 497 254 497 254 498 253 498 256 498 254 499 256 499 255 499 255 500 256 500 257 500 255 501 257 501 258 501 258 502 257 502 260 502 258 503 260 503 259 503 259 504 260 504 261 504 259 505 261 505 296 505 296 506 261 506 262 506 296 507 262 507 297 507 297 508 262 508 263 508 297 509 263 509 298 509 298 510 263 510 270 510 298 511 270 511 242 511 242 512 270 512 264 512 242 513 264 513 200 513 278 514 265 514 280 514 276 515 218 515 225 515 225 516 218 516 274 516 280 517 266 517 278 517 278 518 266 518 322 518 278 519 322 519 207 519 249 520 248 520 270 520 270 521 248 521 267 521 270 522 267 522 264 522 264 523 267 523 268 523 264 524 268 524 265 524 265 525 268 525 269 525 265 526 269 526 280 526 249 527 270 527 251 527 251 528 270 528 263 528 251 529 263 529 262 529 322 530 271 530 207 530 207 531 271 531 231 531 207 532 231 532 218 532 218 533 231 533 272 533 218 534 272 534 229 534 225 535 224 535 222 535 229 536 228 536 218 536 218 537 228 537 273 537 218 538 273 538 274 538 222 539 275 539 225 539 225 540 275 540 219 540 225 541 219 541 276 541 262 542 261 542 251 542 251 543 261 543 260 543 251 544 260 544 257 544 257 545 256 545 251 545 251 546 256 546 253 546 251 547 253 547 277 547 204 548 203 548 278 548 278 549 203 549 265 549 289 550 286 550 279 550 279 551 286 551 280 551 279 552 280 552 202 552 202 553 280 553 269 553 280 554 196 554 266 554 266 555 196 555 282 555 266 556 282 556 320 556 320 557 282 557 281 557 281 558 282 558 283 558 281 559 283 559 284 559 313 560 285 560 315 560 315 561 285 561 196 561 315 562 196 562 286 562 286 563 196 563 280 563 290 564 287 564 279 564 279 565 287 565 288 565 279 566 288 566 289 566 316 567 317 567 319 567 319 568 317 568 287 568 319 569 287 569 318 569 318 570 287 570 290 570 289 571 288 571 314 571 314 572 288 572 312 572 314 573 312 573 291 573 292 574 246 574 295 574 293 575 239 575 241 575 255 576 258 576 295 576 295 577 258 577 259 577 295 578 259 578 296 578 292 579 295 579 241 579 241 580 295 580 237 580 241 581 237 581 293 581 246 582 247 582 295 582 295 583 247 583 250 583 295 584 250 584 294 584 294 585 252 585 295 585 295 586 252 586 254 586 295 587 254 587 255 587 296 588 297 588 295 588 295 589 297 589 298 589 295 590 298 590 242 590 242 591 244 591 295 591 295 592 244 592 299 592 295 593 299 593 234 593 300 594 308 594 310 594 220 595 210 595 301 595 301 596 210 596 302 596 301 597 302 597 303 597 303 598 302 598 304 598 303 599 304 599 306 599 306 600 304 600 305 600 306 601 305 601 215 601 215 602 305 602 213 602 223 603 300 603 221 603 221 604 300 604 310 604 221 605 310 605 220 605 220 606 310 606 307 606 220 607 307 607 210 607 308 608 226 608 310 608 310 609 226 609 309 609 310 610 309 610 227 610 227 611 230 611 310 611 310 612 230 612 311 612 310 613 311 613 232 613 196 614 285 614 288 614 288 615 285 615 312 615 285 616 313 616 312 616 312 617 313 617 291 617 313 618 315 618 291 618 291 619 315 619 314 619 315 620 286 620 314 620 314 621 286 621 289 621 316 622 284 622 317 622 317 623 284 623 283 623 319 624 281 624 316 624 316 625 281 625 284 625 318 626 320 626 319 626 319 627 320 627 281 627 317 628 283 628 287 628 287 629 283 629 282 629 195 630 321 630 322 630 322 631 321 631 271 631 269 632 268 632 202 632 202 633 268 633 245 633 217 634 204 634 207 634 207 635 204 635 278 635 203 636 200 636 265 636 265 637 200 637 264 637 323 638 324 638 327 638 327 639 324 639 414 639 327 640 414 640 449 640 449 641 414 641 408 641 325 642 447 642 326 642 326 643 447 643 417 643 333 644 327 644 449 644 338 645 328 645 333 645 327 646 333 646 420 646 420 647 333 647 329 647 420 648 329 648 451 648 451 649 329 649 368 649 451 650 368 650 367 650 367 651 365 651 451 651 451 652 365 652 379 652 451 653 379 653 450 653 328 654 336 654 333 654 333 655 336 655 335 655 333 656 335 656 452 656 380 657 374 657 329 657 329 658 374 658 330 658 329 659 330 659 372 659 343 660 449 660 345 660 345 661 449 661 448 661 372 662 331 662 329 662 329 663 331 663 371 663 329 664 371 664 368 664 343 665 342 665 449 665 449 666 342 666 332 666 449 667 332 667 333 667 333 668 332 668 339 668 333 669 339 669 338 669 448 670 407 670 334 670 433 671 361 671 452 671 345 672 448 672 347 672 452 673 335 673 433 673 433 674 335 674 336 674 433 675 336 675 337 675 337 676 336 676 328 676 337 677 328 677 430 677 430 678 328 678 338 678 430 679 338 679 431 679 431 680 338 680 339 680 431 681 339 681 340 681 340 682 339 682 332 682 340 683 332 683 435 683 435 684 332 684 342 684 435 685 342 685 341 685 341 686 342 686 343 686 341 687 343 687 344 687 344 688 343 688 345 688 344 689 345 689 346 689 346 690 345 690 347 690 448 691 334 691 347 691 347 692 334 692 406 692 347 693 406 693 428 693 428 694 406 694 348 694 428 695 348 695 429 695 429 696 348 696 349 696 429 697 349 697 438 697 438 698 349 698 350 698 438 699 350 699 351 699 351 700 350 700 401 700 351 701 401 701 437 701 437 702 401 702 352 702 437 703 352 703 436 703 436 704 352 704 400 704 436 705 400 705 353 705 353 706 400 706 354 706 353 707 354 707 355 707 355 708 354 708 357 708 355 709 357 709 356 709 356 710 357 710 358 710 356 711 358 711 432 711 432 712 358 712 359 712 432 713 359 713 360 713 360 714 359 714 402 714 360 715 402 715 361 715 361 716 402 716 362 716 361 717 362 717 452 717 380 718 363 718 364 718 374 719 380 719 377 719 424 720 365 720 425 720 425 721 365 721 367 721 425 722 367 722 366 722 366 723 367 723 368 723 366 724 368 724 369 724 369 725 368 725 371 725 369 726 371 726 370 726 370 727 371 727 331 727 370 728 331 728 427 728 427 729 331 729 372 729 427 730 372 730 373 730 373 731 372 731 330 731 373 732 330 732 375 732 375 733 330 733 374 733 375 734 374 734 376 734 376 735 374 735 377 735 378 736 450 736 424 736 424 737 450 737 379 737 424 738 379 738 365 738 380 739 364 739 377 739 377 740 364 740 381 740 377 741 381 741 426 741 426 742 381 742 409 742 426 743 409 743 383 743 383 744 409 744 382 744 383 745 382 745 384 745 384 746 382 746 386 746 384 747 386 747 385 747 385 748 386 748 387 748 385 749 387 749 388 749 388 750 387 750 389 750 388 751 389 751 421 751 421 752 389 752 390 752 421 753 390 753 422 753 422 754 390 754 391 754 422 755 391 755 392 755 392 756 391 756 393 756 392 757 393 757 394 757 394 758 393 758 404 758 394 759 404 759 395 759 395 760 404 760 405 760 395 761 405 761 396 761 396 762 405 762 399 762 396 763 399 763 378 763 378 764 399 764 397 764 378 765 397 765 450 765 381 766 364 766 389 766 410 767 453 767 414 767 405 768 364 768 399 768 399 769 364 769 363 769 414 770 398 770 410 770 410 771 398 771 411 771 410 772 411 772 363 772 363 773 411 773 397 773 363 774 397 774 399 774 389 775 364 775 390 775 400 776 352 776 354 776 354 777 352 777 401 777 354 778 401 778 357 778 357 779 401 779 350 779 357 780 350 780 358 780 358 781 350 781 349 781 358 782 349 782 359 782 359 783 349 783 348 783 359 784 348 784 402 784 391 785 390 785 403 785 403 786 390 786 364 786 403 787 364 787 404 787 404 788 364 788 405 788 348 789 406 789 402 789 402 790 406 790 334 790 402 791 334 791 362 791 362 792 334 792 407 792 362 793 407 793 453 793 453 794 407 794 408 794 453 795 408 795 414 795 389 796 387 796 386 796 386 797 382 797 389 797 389 798 382 798 409 798 389 799 409 799 381 799 329 800 333 800 410 800 410 801 333 801 453 801 451 802 411 802 420 802 420 803 411 803 398 803 420 804 398 804 412 804 412 805 398 805 413 805 414 806 447 806 398 806 398 807 447 807 325 807 398 808 325 808 413 808 445 809 444 809 415 809 415 810 444 810 447 810 415 811 447 811 324 811 324 812 447 812 414 812 413 813 325 813 416 813 416 814 325 814 441 814 416 815 441 815 442 815 420 816 326 816 327 816 327 817 326 817 417 817 327 818 417 818 323 818 323 819 417 819 418 819 418 820 417 820 446 820 418 821 446 821 443 821 440 822 439 822 419 822 419 823 439 823 326 823 419 824 326 824 412 824 412 825 326 825 420 825 424 826 421 826 378 826 378 827 421 827 422 827 378 828 422 828 396 828 396 829 422 829 392 829 396 830 392 830 395 830 395 831 392 831 423 831 370 832 421 832 369 832 369 833 421 833 424 833 369 834 424 834 366 834 366 835 424 835 425 835 426 836 373 836 377 836 377 837 373 837 375 837 377 838 375 838 376 838 426 839 383 839 373 839 373 840 383 840 384 840 373 841 384 841 385 841 370 842 427 842 421 842 421 843 427 843 373 843 421 844 373 844 388 844 388 845 373 845 385 845 347 846 428 846 429 846 430 847 431 847 353 847 346 848 347 848 344 848 344 849 347 849 429 849 344 850 429 850 341 850 341 851 429 851 435 851 433 852 355 852 356 852 356 853 432 853 433 853 433 854 432 854 360 854 433 855 360 855 361 855 355 856 433 856 353 856 353 857 433 857 434 857 353 858 434 858 430 858 431 859 340 859 353 859 353 860 340 860 435 860 353 861 435 861 436 861 436 862 435 862 429 862 436 863 429 863 437 863 437 864 429 864 438 864 437 865 438 865 351 865 441 866 325 866 439 866 439 867 325 867 326 867 442 868 441 868 440 868 440 869 441 869 439 869 416 870 442 870 419 870 419 871 442 871 440 871 413 872 416 872 412 872 412 873 416 873 419 873 445 874 443 874 444 874 444 875 443 875 446 875 415 876 418 876 445 876 445 877 418 877 443 877 324 878 323 878 415 878 415 879 323 879 418 879 444 880 446 880 447 880 447 881 446 881 417 881 448 882 449 882 407 882 407 883 449 883 408 883 397 884 411 884 450 884 450 885 411 885 451 885 333 886 452 886 453 886 453 887 452 887 362 887 380 888 329 888 363 888 363 889 329 889 410 889 461 890 459 890 463 890 463 891 459 891 467 891 455 892 458 892 456 892 456 893 458 893 460 893 454 894 455 894 464 894 464 895 455 895 456 895 457 896 459 896 465 896 465 897 459 897 461 897 458 898 457 898 460 898 460 899 457 899 465 899 458 900 455 900 457 900 457 901 455 901 454 901 459 902 457 902 467 902 467 903 457 903 454 903 467 904 454 904 468 904 468 905 454 905 469 905 464 906 456 906 460 906 465 907 461 907 463 907 462 908 466 908 463 908 463 909 466 909 464 909 463 910 464 910 465 910 465 911 464 911 460 911 469 912 454 912 466 912 466 913 454 913 464 913 467 914 468 914 463 914 463 915 468 915 462 915 468 916 469 916 462 916 462 917 469 917 466 917 484 918 471 918 470 918 470 919 471 919 496 919 479 920 478 920 471 920 471 921 478 921 490 921 471 922 490 922 496 922 496 923 490 923 472 923 496 924 472 924 497 924 497 925 472 925 500 925 488 926 473 926 474 926 474 927 473 927 477 927 475 928 477 928 476 928 476 929 477 929 473 929 491 930 490 930 480 930 480 931 490 931 478 931 480 932 478 932 475 932 475 933 478 933 486 933 475 934 486 934 477 934 477 935 486 935 474 935 473 936 488 936 476 936 476 937 488 937 487 937 476 938 487 938 485 938 485 939 487 939 479 939 485 940 479 940 484 940 484 941 479 941 471 941 475 942 476 942 480 942 480 943 476 943 485 943 481 944 482 944 470 944 470 945 482 945 483 945 470 946 483 946 484 946 484 947 483 947 491 947 484 948 491 948 485 948 485 949 491 949 480 949 478 950 479 950 486 950 486 951 479 951 487 951 486 952 487 952 474 952 474 953 487 953 488 953 472 954 483 954 500 954 500 955 483 955 482 955 500 956 482 956 501 956 501 957 482 957 495 957 501 958 495 958 489 958 489 959 495 959 494 959 490 960 491 960 472 960 472 961 491 961 483 961 492 962 493 962 494 962 494 963 493 963 489 963 494 964 495 964 492 964 492 965 495 965 498 965 470 966 496 966 481 966 481 967 496 967 497 967 481 968 497 968 498 968 498 969 497 969 499 969 498 970 499 970 492 970 492 971 499 971 493 971 498 972 495 972 481 972 481 973 495 973 482 973 497 974 500 974 499 974 499 975 500 975 501 975 499 976 501 976 493 976 493 977 501 977 489 977 502 978 514 978 503 978 503 979 514 979 515 979 529 980 532 980 503 980 503 981 532 981 521 981 503 982 521 982 502 982 502 983 521 983 523 983 502 984 523 984 504 984 504 985 523 985 516 985 508 986 505 986 507 986 507 987 505 987 506 987 507 988 512 988 508 988 508 989 512 989 509 989 523 990 510 990 516 990 516 991 510 991 511 991 516 992 511 992 518 992 518 993 511 993 512 993 518 994 512 994 506 994 506 995 512 995 507 995 514 996 502 996 513 996 513 997 502 997 504 997 513 998 504 998 509 998 509 999 504 999 517 999 509 1000 517 1000 508 1000 508 1001 517 1001 505 1001 509 1002 512 1002 513 1002 513 1003 512 1003 511 1003 513 1004 511 1004 514 1004 514 1005 511 1005 510 1005 514 1006 510 1006 515 1006 515 1007 510 1007 519 1007 515 1008 519 1008 531 1008 531 1009 519 1009 520 1009 504 1010 516 1010 517 1010 517 1011 516 1011 518 1011 517 1012 518 1012 505 1012 505 1013 518 1013 506 1013 519 1014 521 1014 520 1014 520 1015 521 1015 532 1015 520 1016 532 1016 525 1016 525 1017 532 1017 533 1017 525 1018 533 1018 526 1018 526 1019 533 1019 522 1019 510 1020 523 1020 519 1020 519 1021 523 1021 521 1021 527 1022 524 1022 522 1022 522 1023 524 1023 526 1023 525 1024 526 1024 530 1024 530 1025 526 1025 524 1025 524 1026 527 1026 530 1026 530 1027 527 1027 528 1027 530 1028 528 1028 531 1028 531 1029 528 1029 529 1029 531 1030 529 1030 515 1030 515 1031 529 1031 503 1031 525 1032 530 1032 520 1032 520 1033 530 1033 531 1033 532 1034 529 1034 533 1034 533 1035 529 1035 528 1035 533 1036 528 1036 522 1036 522 1037 528 1037 527 1037

45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/Coordinates: -------------------------------------------------------------------------------- 1 | =================================== 2 | Pioneer3AT Model Locations (x y z) 3 | =================================== 4 | 5 | Chassis : 0 0 0.177 6 | Top : 0.003 0 0.274 7 | 8 | Front Sonar : 0.193 0 0.25 9 | Back Sonar : -0.187 0 0.247 10 | 11 | FR Axle : 0.135 -0.156 0.111 12 | FR Hub : 0.135 -0.199 0.111 13 | FR wheel : 0.135 -0.199 0.111 14 | 15 | FL Axle : 0.135 0.156 0.111 16 | FL Hub : 0.135 0.199 0.111 17 | FL wheel : 0.135 0.199 0.111 18 | 19 | BR Axle : -0.134 -0.156 0.111 20 | BR Hub : -0.134 -0.199 0.111 21 | BR Wheel : -0.134 -0.199 0.111 22 | 23 | BL Axle : -0.134 0.156 0.111 24 | BL Hub : -0.134 0.199 0.111 25 | BL Wheel : -0.134 0.199 0.111 26 | -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/axle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/axle.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/back_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/back_sonar.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/chassis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/chassis.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/front_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/front_sonar.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/left_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/left_hubcap.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/right_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/right_hubcap.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/top.stl -------------------------------------------------------------------------------- /summit_description/meshes/p3at_meshes/wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blakcapple/DRL-PID/49762539b3d12651cc198811d77e15eb1f045f9d/summit_description/meshes/p3at_meshes/wheel.stl -------------------------------------------------------------------------------- /summit_description/msg/EnvInfo.msg: -------------------------------------------------------------------------------- 1 | float32[5] cx 2 | float32[5] cy 3 | float32 v_x 4 | float32 w_z 5 | float32 error_k 6 | float32 error_x 7 | float32 distance 8 | bool success_flag 9 | bool failed_flag 10 | 11 | -------------------------------------------------------------------------------- /summit_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | summit_description 4 | 0.0.0 5 | The summit_description package 6 | 7 | 8 | 9 | 10 | user 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | controller_manager 53 | gazebo_ros 54 | joint_state_publisher 55 | robot_state_publisher 56 | rospy 57 | rviz 58 | controller_manager 59 | gazebo_ros 60 | joint_state_publisher 61 | robot_state_publisher 62 | rospy 63 | rviz 64 | controller_manager 65 | gazebo_ros 66 | joint_state_publisher 67 | robot_state_publisher 68 | rospy 69 | rviz 70 | message_generation 71 | message_runtime 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /summit_description/robot/pioneer3at.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | 12 | 13 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | Gazebo/Black 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 1000000.0 334 | 100.0 335 | 1.0 336 | 1.0 337 | 341 | 342 | 1.0 343 | 0.00 344 | 345 | 346 | 347 | 348 | 1000000.0 349 | 100.0 350 | 1.0 351 | 1.0 352 | 356 | 357 | 1.0 358 | 0.00 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 384 | 385 | cmd_vel 386 | odom 387 | odom 388 | 50.0 389 | base_footprint 390 | true 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | -------------------------------------------------------------------------------- /summit_description/robot/summit.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /summit_description/script/PID/line_detect.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | import cv2 4 | from cv_bridge import CvBridge, CvBridgeError 5 | from sensor_msgs.msg import Image 6 | from summit_description.msg import LineInfo 7 | import numpy as np 8 | 9 | class LineDetect: 10 | 11 | def __init__(self): 12 | 13 | self.bridge = CvBridge() 14 | 15 | self.image_sub = rospy.Subscriber("iRobot/camera/image_raw", Image, 16 | self.camera_callback) 17 | 18 | self.detect_pub = rospy.Publisher("line_detect", LineInfo, queue_size=10) 19 | self.detect_msg = LineInfo() 20 | self.pub_rate = rospy.Rate(10) 21 | 22 | def camera_callback(self, data): 23 | 24 | try: 25 | cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8') 26 | except CvBridgeError as e : 27 | print(e) 28 | 29 | (height, width, channels) = cv_image.shape 30 | cv2.imshow("cv_image", cv_image) 31 | 32 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) 33 | 34 | upper_black = np.array([30,30,30]) 35 | lower_black = np.array([0,0,0]) 36 | mask = cv2.inRange(hsv, lower_black, upper_black) 37 | 38 | m = cv2.moments(mask, False) 39 | 40 | try: 41 | cx, cy = m['m10']/m['m00'], m['m10']/m['m00'] 42 | except ZeroDivisionError: 43 | cx, cy = height/2, width/2 44 | 45 | cv2.circle(mask, (int(cx), int(cy)), 10, (0,0,0), -1) 46 | cv2.imshow("mask", mask) 47 | 48 | cv2.waitKey(1) 49 | 50 | self.detect_msg.error_x = width/2 - cx 51 | self.detect_pub.publish(self.detect_msg) 52 | rospy.loginfo("error_x = " + str(self.detect_msg.error_x)) 53 | self.pub_rate.sleep() 54 | 55 | def main(): 56 | rospy.init_node("line_detect_node") 57 | image_object = LineDetect() 58 | rospy.spin() 59 | # ctrl_c = False 60 | # def shutdown(): 61 | # ctrl_c = True 62 | def shutdown(): 63 | print("showdown time") 64 | cv2.destroyAllWindows() 65 | rospy.on_shutdown(shutdown) 66 | # while not ctrl_c: 67 | # rate.sleep() 68 | 69 | if __name__ == '__main__': 70 | main() 71 | 72 | 73 | -------------------------------------------------------------------------------- /summit_description/script/PID/pid_tracker.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from move_car import MoveCar 6 | import numpy as np 7 | import math 8 | from summit_description.msg import LineInfo 9 | 10 | 11 | class LineFollower(object): 12 | 13 | def __init__(self): 14 | 15 | self.detect_sub = rospy.Subscriber("line_detect", LineInfo, self.move_callback) 16 | self.move = MoveCar() 17 | self.car_twist = Twist() 18 | self.error = 0 19 | 20 | def move_callback(self,data): 21 | 22 | self.error = data.error_x 23 | 24 | def _pid_calculate(self): 25 | 26 | self.car_twist.linear.x = 0.2 27 | self.car_twist.angular.z = self.error / 300 28 | 29 | if math.fabs(self.car_twist.angular.z) > 0.10 : 30 | if self.car_twist.angular.z > 0 : 31 | self.car_twist.angular.z = 0.10 32 | else: 33 | self.car_twist.angular.z = -0.10 34 | 35 | def control(self): 36 | 37 | self.move.move_robot(self.car_twist) 38 | self._pid_calculate() 39 | rospy.loginfo("ANGULAR VALUE SENT -->"+str(self.car_twist.angular.z)) 40 | rospy.loginfo("error -->"+str(self.error)) 41 | 42 | 43 | def stop(self): 44 | self.car_twist.linear.x = 0.0 45 | self.car_twist.angular.z = 0.0 46 | self.move.move_robot(self.car_twist) 47 | 48 | def main(): 49 | 50 | rospy.init_node("line_following_node", anonymous=True) 51 | line_follower= LineFollower() 52 | rate = rospy.Rate(10) 53 | 54 | def shutdown(): 55 | print("shutdown!") 56 | line_follower.stop() 57 | 58 | rospy.on_shutdown(shutdown) 59 | while not rospy.is_shutdown(): 60 | line_follower.control() 61 | rate.sleep() 62 | 63 | if __name__ == '__main__': 64 | main() 65 | -------------------------------------------------------------------------------- /summit_description/script/gotopoint/goToPoint.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # import ros stuff 4 | import rospy 5 | from sensor_msgs.msg import LaserScan 6 | from geometry_msgs.msg import Twist, Point 7 | from nav_msgs.msg import Odometry 8 | from tf import transformations 9 | from std_srvs.srv import * 10 | 11 | import math 12 | 13 | active_ = False 14 | 15 | # robot state variables 16 | position_ = Point() 17 | yaw_ = 0 18 | # machine state 19 | state_ = 0 20 | # goal 21 | desired_position_ = Point() 22 | desired_position_.x = rospy.get_param('des_pos_x') 23 | desired_position_.y = rospy.get_param('des_pos_y') 24 | desired_position_.z = 0 25 | # parameters 26 | yaw_precision_ = math.pi / 90 # +/- 2 degree allowed 27 | dist_precision_ = 0.3 28 | 29 | # publishers 30 | pub = None 31 | 32 | # service callbacks 33 | def go_to_point_switch(req): 34 | global active_ 35 | active_ = req.data 36 | res = SetBoolResponse() 37 | res.success = True 38 | res.message = 'Done!' 39 | return res 40 | 41 | # callbacks 42 | def clbk_odom(msg): 43 | global position_ 44 | global yaw_ 45 | 46 | # position 47 | position_ = msg.pose.pose.position 48 | 49 | # yaw 50 | quaternion = ( 51 | msg.pose.pose.orientation.x, 52 | msg.pose.pose.orientation.y, 53 | msg.pose.pose.orientation.z, 54 | msg.pose.pose.orientation.w) 55 | euler = transformations.euler_from_quaternion(quaternion) 56 | yaw_ = euler[2] 57 | 58 | def change_state(state): 59 | global state_ 60 | state_ = state 61 | print 'State changed to [%s]' % state_ 62 | 63 | def normalize_angle(angle): 64 | if(math.fabs(angle) > math.pi): 65 | angle = angle - (2 * math.pi * angle) / (math.fabs(angle)) 66 | return angle 67 | 68 | def fix_yaw(des_pos): 69 | global yaw_, pub, yaw_precision_, state_ 70 | desired_yaw = math.atan2(des_pos.y - position_.y, des_pos.x - position_.x) 71 | err_yaw = normalize_angle(desired_yaw - yaw_) 72 | 73 | #rospy.loginfo(err_yaw) 74 | 75 | twist_msg = Twist() 76 | if math.fabs(err_yaw) > yaw_precision_: 77 | twist_msg.angular.z = 2 if err_yaw > 0 else -2 78 | 79 | pub.publish(twist_msg) 80 | 81 | # state change conditions 82 | if math.fabs(err_yaw) <= yaw_precision_: 83 | print 'Yaw error: [%s]' % err_yaw 84 | change_state(1) 85 | 86 | def go_straight_ahead(des_pos): 87 | global yaw_, pub, yaw_precision_, state_ 88 | desired_yaw = math.atan2(des_pos.y - position_.y, des_pos.x - position_.x) 89 | err_yaw = desired_yaw - yaw_ 90 | err_pos = math.sqrt(pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2)) 91 | 92 | if err_pos > dist_precision_: 93 | twist_msg = Twist() 94 | twist_msg.linear.x = 0.6 95 | twist_msg.angular.z = 0.2 if err_yaw > 0 else -0.2 96 | pub.publish(twist_msg) 97 | else: 98 | print 'Position error: [%s]' % err_pos 99 | change_state(2) 100 | 101 | # state change conditions 102 | if math.fabs(err_yaw) > yaw_precision_: 103 | print 'Yaw error: [%s]' % err_yaw 104 | change_state(0) 105 | 106 | def done(): 107 | twist_msg = Twist() 108 | twist_msg.linear.x = 0 109 | twist_msg.angular.z = 0 110 | pub.publish(twist_msg) 111 | 112 | def main(): 113 | global pub, active_ 114 | 115 | rospy.init_node('go_to_point') 116 | 117 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 118 | 119 | sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom) 120 | 121 | srv = rospy.Service('go_to_point_switch', SetBool, go_to_point_switch) 122 | 123 | rate = rospy.Rate(20) 124 | 125 | while not rospy.is_shutdown(): 126 | if not active_: 127 | continue 128 | else: 129 | if state_ == 0: 130 | fix_yaw(desired_position_) 131 | elif state_ == 1: 132 | go_straight_ahead(desired_position_) 133 | elif state_ == 2: 134 | done() 135 | else: 136 | rospy.logerr('Unknown state!') 137 | 138 | rate.sleep() 139 | 140 | if __name__ == '__main__': 141 | main() -------------------------------------------------------------------------------- /summit_description/script/gotopoint/goToPoint_omni.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # import ros stuff 4 | import rospy 5 | from sensor_msgs.msg import LaserScan 6 | from geometry_msgs.msg import Twist, Point 7 | from nav_msgs.msg import Odometry 8 | from tf import transformations 9 | from std_srvs.srv import * 10 | 11 | import math 12 | import numpy as np 13 | 14 | active_ = False 15 | 16 | # robot state variables 17 | position_ = Point() 18 | yaw_ = 0 19 | # machine state 20 | state_ = 0 21 | # goal 22 | desired_position_ = Point() 23 | desired_position_.x = rospy.get_param('des_pos_x') 24 | desired_position_.y = rospy.get_param('des_pos_y') 25 | desired_position_.z = rospy.get_param('des_pos_yaw') 26 | # parameters 27 | yaw_precision_ = math.pi / 90 # +/- 2 degree allowed 28 | dist_precision_ = 0.3 29 | 30 | # publishers 31 | pub = None 32 | 33 | # service callbacks 34 | def go_to_point_switch(req): 35 | global active_ 36 | active_ = req.data 37 | res = SetBoolResponse() 38 | res.success = True 39 | res.message = 'Done!' 40 | return res 41 | 42 | # callbacks 43 | def clbk_odom(msg): 44 | global position_ 45 | global yaw_ 46 | 47 | # position 48 | position_ = msg.pose.pose.position 49 | 50 | # yaw 51 | quaternion = ( 52 | msg.pose.pose.orientation.x, 53 | msg.pose.pose.orientation.y, 54 | msg.pose.pose.orientation.z, 55 | msg.pose.pose.orientation.w) 56 | euler = transformations.euler_from_quaternion(quaternion) 57 | yaw_ = euler[2] 58 | 59 | def change_state(state): 60 | global state_ 61 | state_ = state 62 | print 'State changed to [%s]' % state_ 63 | 64 | def normalize_angle(angle): 65 | if(math.fabs(angle) > math.pi): 66 | angle = angle - (2 * math.pi * angle) / (math.fabs(angle)) 67 | return angle 68 | def go_to_point(des_pos): 69 | global yaw_, pub, yaw_precision_, state_ 70 | 71 | # Calculate the Errors 72 | err_yaw = normalize_angle(des_pos.z - yaw_) 73 | err_pos_y = des_pos.y - position_.y 74 | err_pos_x = des_pos.x - position_.x 75 | 76 | # Change coordinate frame: odom -> robot 77 | robot_err_x = np.cos(yaw_) * err_pos_x + np.sin(yaw_) * err_pos_y 78 | robot_err_y = - np.sin(yaw_) * err_pos_x + np.cos(yaw_) * err_pos_y 79 | 80 | twist_msg = Twist() 81 | 82 | # Flag to check whether we finish or not 83 | there_is_error = False 84 | 85 | if math.fabs(err_yaw) > yaw_precision_: 86 | twist_msg.angular.z = 0.7 if err_yaw > 0 else -0.7 87 | there_is_error = True 88 | 89 | if math.fabs(robot_err_x) > dist_precision_: 90 | twist_msg.linear.x = 0.7 if robot_err_x > 0 else -0.7 91 | there_is_error = True 92 | 93 | if math.fabs(robot_err_y) > dist_precision_: 94 | twist_msg.linear.y = 0.7 if robot_err_y > 0 else - 0.7 95 | there_is_error = True 96 | 97 | pub.publish(twist_msg) 98 | 99 | if not there_is_error: 100 | print 'Arrived' 101 | change_state(1) 102 | 103 | def done(): 104 | twist_msg = Twist() 105 | twist_msg.linear.x = 0 106 | twist_msg.angular.z = 0 107 | pub.publish(twist_msg) 108 | 109 | def main(): 110 | global pub, active_ 111 | 112 | rospy.init_node('go_to_point') 113 | 114 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 115 | 116 | sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom) 117 | 118 | srv = rospy.Service('go_to_point_switch', SetBool, go_to_point_switch) 119 | 120 | rate = rospy.Rate(20) 121 | 122 | while not rospy.is_shutdown(): 123 | if not active_: 124 | continue 125 | else: 126 | if state_ == 0: 127 | go_to_point(desired_position_) 128 | elif state_ == 1: 129 | done() 130 | else: 131 | rospy.logerr('Unknown state!') 132 | 133 | rate.sleep() 134 | 135 | if __name__ == '__main__': 136 | main() -------------------------------------------------------------------------------- /summit_description/srv/StartUp.srv: -------------------------------------------------------------------------------- 1 | bool active 2 | --- 3 | string result 4 | -------------------------------------------------------------------------------- /summit_description/urdf/base/summit_base.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | cmd_vel 8 | odom 9 | odom 10 | 50.0 11 | base_footprint 12 | true 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /summit_description/urdf/base/summit_base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /summit_description/urdf/camera/camera.urdf.xarco: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 30.0 41 | 42 | 1.3962634 43 | 44 | 300 45 | 300 46 | R8G8B8 47 | 48 | 49 | 0.02 50 | 300 51 | 52 | 53 | gaussian 54 | 0.0 55 | 0.007 56 | 57 | 58 | 59 | 1 60 | true 61 | 0 62 | iRobot/camera 63 | image_raw 64 | camera_info 65 | camera_link 66 | 0.07 67 | 0.0 68 | 0.0 69 | 0.0 70 | 0.0 71 | 0.0 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /summit_description/urdf/camera/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.05 0.05 0.05 0 0 0 6 | 7 | 0.1 8 | 9 | 10 | 11 | 12 | 0.1 0.1 0.1 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0.1 0.1 0.1 20 | 21 | 22 | 23 | 24 | 25 | 1.047 26 | 27 | 320 28 | 240 29 | 30 | 31 | 0.1 32 | 100 33 | 34 | 35 | 1 36 | 30 37 | true 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /summit_description/urdf/pioneer3at.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | Gazebo/Black 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 1000000.0 295 | 296 | 100.0 297 | 1.0 298 | 299 | 1.0 300 | 304 | 305 | 306 | 1.0 307 | 0.00 308 | 309 | 310 | 311 | 1000000.0 312 | 313 | 100.0 314 | 1.0 315 | 316 | 1.0 317 | 321 | 322 | 323 | 1.0 324 | 0.00 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 1000000.0 507 | 508 | 100.0 509 | 1.0 510 | 511 | 1.0 512 | 516 | 517 | 518 | 1.0 519 | 0.00 520 | 521 | 522 | 523 | 1000000.0 524 | 525 | 100.0 526 | 1.0 527 | 528 | 1.0 529 | 533 | 534 | 535 | 1.0 536 | 0.00 537 | 538 | 539 | 540 | 541 | 100.0 542 | sim_p3at 543 | p3at_front_left_wheel_joint 544 | p3at_front_right_wheel_joint 545 | p3at_back_left_wheel_joint 546 | p3at_back_right_wheel_joint 547 | 0.4 548 | 0.215 549 | base_link 550 | 5.0 551 | 200 552 | cmd_vel 553 | odom 554 | odom 555 | 1 556 | 557 | 558 | 559 | 560 | 561 | -------------------------------------------------------------------------------- /summit_description/urdf/wheels/omni_wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | transmission_interface/SimpleTransmission 62 | 63 | hardware_interface/VelocityJointInterface 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | false 77 | 78 | 79 | 80 | 81 | --------------------------------------------------------------------------------