[rp.slamware.system.BaseMonitorLogSupplier] [INFO] {"category":"base.status","data":{"isCharging":false},"isCustomerLog":true,"usEpoch":"1663401364507063","usTick":"56939175"}
[rp.applet.server.RpSlamServer] [INFO] {"category":"client.tcp.connection","data":{"ip":"127.0.0.1","port":51282},"isCustomerLog":true,"keyword":"received","usEpoch":"1663401370514983","usTick":"62947096"}
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:d36cce36-da92-43da-9af5-670dbab2ee95
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:ff23f0d4-1c38-43bb-8af3-67769b71c6fe
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:d36cce36-da92-43da-9af5-670dbab2ee95
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (0.000000, 0.000000, 25.328194)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (0.000000, 0.000000, 25.328194)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (0.000000, 0.000000, 25.328194)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (0.000000, 0.000000, 25.328194)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (0.000000, 0.000000, 25.328194)
[rp.algorithm.slam.CSLAMBuilder] [WARN] force to update map takes : 30 ms
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":505,"y":1255},"mapPose":{"x":-2.224996328353882,"y":13.27500343322754},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401382134323","usTick":"74566436"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-4.50000 y: 3.28000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.12000 y: 3.08000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.84000 y: 3.24000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-0.10000 y: 3.19000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-0.29000 y: 3.12000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.97000 y: 3.29000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.26000 y:11.11000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.28000 y: 3.15000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.58000 y:21.09000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 5.25000 y:-3.53000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.92000 y: 3.13000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.11000 y: 3.02000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:30a6b53e-d2e9-4740-b499-b79e28f996fa, rectangle area count:2
[rp.algorithm.slam.CSLAMBuilder] [INFO] transformHiddmapDataFromExploredMap
[rp.slamware.motion.ElevatorAreaService#0] [INFO] clear all elevator areas
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 123.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 41.
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :249 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] Switched to AMCL Mapping!
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"result":true},"isCustomerLog":true,"keyword":"end","usElapsed":"1220502","usEpoch":"1663401383354826","usTick":"75786938"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.motion.RandomRelocalizationAction] [INFO] Receive Relocalization task!!!
[rp.algorithm.slam.CSLAMBuilder] [INFO] startRelocalization
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start rebuild virtual track assist map.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] virtual track assist rebuilt (4377 us).
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.507850, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.407907, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 298 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :298 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.396859, ICP takes: 29 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.387182, ICP takes: 58 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 69 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.475784, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.382548, ICP takes: 31 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 48 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.267713, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.260553, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 334 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :334 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.333898, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.290880, ICP takes: 29 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 38 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 137, icpInfo.goodness = 0.267289, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.258014, ICP takes: 40 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 44 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.257574, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.512100, ICP takes: 45 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 210 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :210 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.287550, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.505516, ICP takes: 44 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 59 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.501560, ICP takes: 33 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.266330, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 44 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.511633, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.509915, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 187 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.489779, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 193, icpInfo.goodness = 0.509874, ICP takes: 72 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 94 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.495927, ICP takes: 64 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.494843, ICP takes: 84 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 99 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.275636, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.240852, ICP takes: 56 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 234 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :234 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.207949, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.323050, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 35 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.399412, ICP takes: 26 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.503141, ICP takes: 100 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 111 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.259093, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.216162, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 234 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :235 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.245970, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.265049, ICP takes: 37 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 48 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.264380, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.224304, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 27 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 33.
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.512271, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.257787, ICP takes: 6 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 260 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :261 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.525545, ICP takes: 41 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.490294, ICP takes: 43 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 55 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.499365, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.472174, ICP takes: 46 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 53 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:1063 ms, raw area:-297,-362,506,1255
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.405306, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.497074, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 277 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :277 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.505713, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.502957, ICP takes: 26 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 30 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.484348, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.504009, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 46 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.322182, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.227925, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 219 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :220 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 183, icpInfo.goodness = 0.301324, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.232349, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 14 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.417908, ICP takes: 40 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.297904, ICP takes: 45 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 60 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.479437, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.332708, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 165 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.531763, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.353534, ICP takes: 43 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 52 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.502852, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.332351, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 26 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.192219, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 184, icpInfo.goodness = 0.504629, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 245 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :245 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 149, icpInfo.goodness = 0.252715, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.510970, ICP takes: 55 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 92 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.261510, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.505005, ICP takes: 30 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 33 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.512008, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.519715, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 145 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.510407, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.386021, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 25 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.496224, ICP takes: 133 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.406108, ICP takes: 177 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 192 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.488159, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.304107, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 192 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.330054, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 138, icpInfo.goodness = 0.509742, ICP takes: 39 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 44 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 137, icpInfo.goodness = 0.489675, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.298702, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 25 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 138, icpInfo.goodness = 0.490738, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 184, icpInfo.goodness = 0.276011, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 141 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.306213, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.479855, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 28 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.492705, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.299405, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 19 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.195596, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.328309, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 240 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :243 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.327146, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.417412, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 30 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.415935, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.312463, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 41 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.242709, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.300901, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 136 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.499872, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.510737, ICP takes: 38 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 47 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.514136, ICP takes: 71 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 149, icpInfo.goodness = 0.502696, ICP takes: 81 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 88 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.278596, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.263690, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 190 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 149, icpInfo.goodness = 0.283482, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.251729, ICP takes: 73 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 97 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.287804, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.517251, ICP takes: 41 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 52 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.329540, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.286487, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 211 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :212 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 153, icpInfo.goodness = 0.296794, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.311502, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 19 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.307405, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.299985, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 29 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.252514, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.465992, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 132 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 137, icpInfo.goodness = 0.499158, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.503107, ICP takes: 48 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 58 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.517353, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.514815, ICP takes: 45 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 50 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.308848, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.278278, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 179 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.297931, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.262433, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 19 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.309217, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.243645, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 18 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.527529, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.260965, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 138 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.230487, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.483853, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 26 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.502678, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.286780, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 32 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.399315, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.234157, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 139 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.381136, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.275503, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 26 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.382247, ICP takes: 6 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.239135, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 27 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.307530, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.318986, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 139 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.278052, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.255767, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 20 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.266079, ICP takes: 7 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.224135, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 16 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.368062, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.242183, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 131 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.511849, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.316527, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 23 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.494174, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.317992, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 27 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.293246, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.264203, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 199 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.234820, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.511644, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 31 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.285054, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.517987, ICP takes: 30 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 38 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 138, icpInfo.goodness = 0.288867, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.456302, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 165 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 137, icpInfo.goodness = 0.318330, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.440730, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 48 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.298249, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.426180, ICP takes: 28 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 35 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 138, icpInfo.goodness = 0.491063, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.273189, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 151 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.492410, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.275258, ICP takes: 43 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 47 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.489904, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.279090, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 21 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.299760, ICP takes: 7 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 191, icpInfo.goodness = 0.250039, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 141 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.286318, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.281380, ICP takes: 54 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 64 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.289315, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.195233, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 28 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.278027, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.519197, ICP takes: 33 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 160 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.277621, ICP takes: 28 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.503352, ICP takes: 31 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 39 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.270747, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.528622, ICP takes: 37 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 41 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.334914, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 149, icpInfo.goodness = 0.354165, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 170 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.351270, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 192, icpInfo.goodness = 0.336233, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 23 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.314917, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 192, icpInfo.goodness = 0.324456, ICP takes: 26 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 34 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.184816, ICP takes: 7 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.513854, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 139 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.519314, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.261350, ICP takes: 71 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 80 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.287406, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.504876, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 28 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.365159, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.266578, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 127 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.367601, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.429869, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 32 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.344817, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.404613, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 38 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.371617, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.302285, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 128 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.521766, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.278469, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 22 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.522903, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.302850, ICP takes: 29 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 36 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.511354, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.520767, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 204 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :204 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 150, icpInfo.goodness = 0.484459, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.530783, ICP takes: 44 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 57 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.506740, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.525305, ICP takes: 37 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 43 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.246934, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 192, icpInfo.goodness = 0.402469, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 122 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.273347, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.392923, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 29 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.287527, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.416863, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 47 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.266654, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 192, icpInfo.goodness = 0.161718, ICP takes: 21 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 209 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :209 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 184, icpInfo.goodness = 0.220174, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.250754, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 43 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.260995, ICP takes: 31 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.519930, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 47 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 148, icpInfo.goodness = 0.496948, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.289227, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 156 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.498874, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.303559, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 19 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.491792, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.291178, ICP takes: 37 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 46 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.464485, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.318625, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 204 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :206 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.235700, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.522203, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 28 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.504480, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.257457, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 21 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.292977, ICP takes: 10 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 184, icpInfo.goodness = 0.514638, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 133 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.301040, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.512286, ICP takes: 18 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 42 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 136, icpInfo.goodness = 0.289239, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 185, icpInfo.goodness = 0.517271, ICP takes: 41 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 50 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 147, icpInfo.goodness = 0.252810, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.502379, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 224 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :225 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.506464, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 137, icpInfo.goodness = 0.287251, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 30 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.236615, ICP takes: 28 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.507482, ICP takes: 35 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 48 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.256792, ICP takes: 7 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 184, icpInfo.goodness = 0.486872, ICP takes: 25 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 213 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :214 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 138, icpInfo.goodness = 0.257099, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.524794, ICP takes: 27 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 35 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.245862, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 193, icpInfo.goodness = 0.480413, ICP takes: 36 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 45 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 145, icpInfo.goodness = 0.280929, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.305077, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 219 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :220 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 140, icpInfo.goodness = 0.273140, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.317183, ICP takes: 23 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 31 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.306998, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.266544, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 29 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.slamware.server.SlamwareServerAgent] [WARN] Relocalizationing, please wait for a moment
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.515569, ICP takes: 19 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.358804, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 216 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :216 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 144, icpInfo.goodness = 0.386616, ICP takes: 9 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 192, icpInfo.goodness = 0.500449, ICP takes: 40 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 49 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.390124, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.515245, ICP takes: 34 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 45 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.243144, ICP takes: 13 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.338124, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 147 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.509992, ICP takes: 12 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.320582, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 20 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 139, icpInfo.goodness = 0.504084, ICP takes: 16 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 187, icpInfo.goodness = 0.318329, ICP takes: 22 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 26 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 141, icpInfo.goodness = 0.240248, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 190, icpInfo.goodness = 0.412744, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 127 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 188, icpInfo.goodness = 0.292290, ICP takes: 11 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 146, icpInfo.goodness = 0.323173, ICP takes: 17 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 21 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 143, icpInfo.goodness = 0.391368, ICP takes: 8 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 186, icpInfo.goodness = 0.230830, ICP takes: 14 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] Max convergence time is arrived, reset!!
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-0.465945, x max:3.534055, y min:-1.570052, y max:2.429948, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 18 ms
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.motion.RandomRelocalizationAction] [WARN] Global relocalization Failed in time out > 15000 ms, excution time is 15006 ms.
[rp.algorithm.slam.CSLAMBuilder] [INFO] stopRelocalization
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.534055, 0.429948, -179.133994)
[rp.algorithm.slam.RelocalizationProxy] [INFO] stopRelocalization
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 1, status -1, reason failed
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 142, icpInfo.goodness = 0.257539, ICP takes: 15 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 189, icpInfo.goodness = 0.507478, ICP takes: 24 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 193 ms
[rp.algorithm.slam.AMCLProxy] [INFO] onLocalizationRecovery: low localization to normal
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"flags":["milestone","precise"],"mode":0},"points":[[-5.710000038146973,3.349999904632568]]}},"isCustomerLog":true,"usEpoch":"1663401400418949","usTick":"92851061"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-5.710000, 3.350000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"Basic"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401400428721","usTick":"92860833"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move.invoke","isCustomerLog":true,"keyword":"undock","usEpoch":"1663401400439370","usTick":"92871483"}
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.12000-> 0.01380 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 6 ms
[rp.slamware.system.BaseMonitorLogSupplier] [INFO] {"category":"base.status","data":{"isOnDock":false},"isCustomerLog":true,"usEpoch":"1663401400811096","usTick":"93243209"}
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.12000-> 0.12000 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.11993-> 0.11993 omega: -0.00034->-0.00034.
[rp.algorithm.motion.UnDockController] [INFO] move to landing points success
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (-5.710000, 3.350000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":1.110360383987427,"y":0.5165002942085266},"moveType":"Basic","robotPose":{"x":1.110360421659763,"y":0.5165003129108218,"yaw":-3.135854307456502},"to":{"x":-5.710000038146973,"y":3.349999904632568}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401402844212","usTick":"95276324"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (1.1104, 0.5165) to (-5.7100, 3.3500).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:5767
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, Basic, (1.11036, 0.5165) to (-5.71, 3.35), used 14 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 182, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (1.125000, 0.525000) --> (-5.710000, 3.350000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -5.710000, y = 3.350000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (1.1027, 0.5165) to (0.4761, 2.5956).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1319
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (1.10267, 0.516457) to (0.476062, 2.59558), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.84806:: 2.29927 -> -3.13585.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39655-> 0.16080 omega: -0.53116->-0.53116.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44092-> 0.44092 omega: -0.42586->-0.39638.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44482-> 0.44482 omega: -0.56034->-0.56034.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44324-> 0.44324 omega: -0.15690->-0.22821.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42089-> 0.41641 omega:  0.65000-> 0.41443.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.24138-> 0.23958 omega:  0.07132-> 0.54284.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39655-> 0.25894 omega:  0.43262-> 0.43262.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.0872, 2.5469) to (-2.1157, 2.7990).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2233
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.0871716, 2.54687) to (-2.11567, 2.79902), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:970, path size before filter:68, after filter:63
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.27091:: 2.98582 ->  2.71491.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.33058-> 0.33512 omega:  0.38103-> 0.36252.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38019-> 0.38019 omega: -0.15690->-0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37918-> 0.37918 omega:  0.47069-> 0.47069.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38082-> 0.38000 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38318-> 0.38318 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.34483-> 0.35849 omega: -0.15690->-0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.25862-> 0.25862 omega: -0.11207->-0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.18966-> 0.18966 omega: -0.02241->-0.02241.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.99977).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.3787, 2.7214) to (-4.6097, 2.9719).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1065
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.37873, 2.72144) to (-4.60969, 2.97194), used 6 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.27764).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.3958, 2.7233) to (-4.6097, 2.9719).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1058
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.39579, 2.72335) to (-4.60969, 2.97194), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.25930).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.4095, 2.7248) to (-4.7094, 2.9789).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1111
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.40948, 2.72485) to (-4.70945, 2.97886), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.24800).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.24353).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.4152, 2.7255) to (-4.7094, 2.9789).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1125
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.41522, 2.72547) to (-4.70945, 2.97886), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.4166, 2.7256) to (-4.7094, 2.9789).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1131
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.41655, 2.72561) to (-4.70945, 2.97886), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2297, path size before filter:69, after filter:64
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.00071:: 3.03428 ->  3.03357.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37736-> 0.18720 omega:  0.15690-> 0.13710.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36756-> 0.36756 omega: -0.24655->-0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38039-> 0.38039 omega:  0.20172-> 0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38759-> 0.38759 omega:  0.42586-> 0.42586.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38903-> 0.38903 omega:  0.47069-> 0.47069.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39655-> 0.39655 omega:  0.02241-> 0.07907.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44155-> 0.44155 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45417-> 0.45060 omega: -0.36443->-0.36443.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.25862-> 0.25862 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.11604-> 0.11604 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.05027-> 0.05027 omega: -0.65000->-0.54286.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] precise move end!, milestone is (-5.7100, 3.3500, 0.0000), pose is (-5.7091, 3.3238, 2.2935), error is (0.0262,131.4106)
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] milestones is arrived, finish.
[rp.algorithm.motion.NavigationScheduleAction] [INFO] all milestone is arrived, markAsFinished
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":10},"isCustomerLog":true,"keyword":"end","usElapsed":"28488656","usEpoch":"1663401428917376","usTick":"121349489"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 2, status 10, reason 
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2028 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 6 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"flags":["milestone","precise"],"mode":0},"points":[[0.9800000190734863,1.620000004768372]]}},"isCustomerLog":true,"usEpoch":"1663401443184930","usTick":"135617043"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (0.980000, 1.620000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"Basic"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401443185188","usTick":"135617301"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (0.980000, 1.620000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":-5.722261428833008,"y":3.315706253051758},"moveType":"Basic","robotPose":{"x":-5.722261269670718,"y":3.315706308527604,"yaw":2.265276413206770},"to":{"x":0.9800000190734863,"y":1.620000004768372}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401443234119","usTick":"135666232"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-5.7223, 3.3157) to (0.9800, 1.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3993
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, Basic, (-5.72226, 3.31571) to (0.98, 1.62), used 19 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 151, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-5.725000, 3.325000) --> (0.980000, 1.620000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = 0.980000, y = 1.620000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-5.7223, 3.3157) to (-3.5428, 3.0243).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-5.72226, 3.31571) to (-3.54275, 3.02431), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -2.92121::-0.65594 ->  2.26528.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.03000->-0.03000.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 5 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40610-> 0.18180 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37302-> 0.37302 omega:  0.06724-> 0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.27586-> 0.27586 omega:  0.15690-> 0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.18966-> 0.18966 omega:  0.33621-> 0.33621.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-4.7197, 3.1166) to (-2.4802, 2.9135).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1773
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-4.7197, 3.11662) to (-2.48019, 2.91348), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1663, path size before filter:62, after filter:59
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.29659:: 0.24557 -> -0.05102.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44828-> 0.34716 omega:  0.43323-> 0.45200.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40110-> 0.40110 omega: -0.50425->-0.50425.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.25862-> 0.25862 omega: -0.29138->-0.20507.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.99315).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-3.7292, 3.0496) to (-1.4845, 2.8206).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1665
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.72923, 3.04957) to (-1.48452, 2.82056), used 25 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.16218::-0.16162 -> -0.32380.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:772, path size before filter:62, after filter:59
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] intend to rotate to path direction, but current pose is occupancy, go out, lines = 480.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageGoOutOccupiedSpace].
[rp.algorithm.motion.BreakOutAction] [INFO] simple move from:(-3.71830,  3.04610) to:(-3.37500,  3.07500).
[rp.algorithm.motion.BreakOutAction] [INFO] current pose is absolutely safe, break out success.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] go out occupied space success, start local plan.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageGoOutOccupiedSpace] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-3.7365, 3.1086) to (-1.4791, 2.7421).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1606
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.73648, 3.10864) to (-1.47906, 2.74209), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.16094::-0.20231 -> -0.04137.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00685 omega:  0.00000-> 0.00000.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-3.7308, 3.1083) to (-1.4791, 2.7421).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1850
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.73085, 3.10834) to (-1.47906, 2.74209), used 77 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.18935::-0.19781 -> -0.00845.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:986, path size before filter:68, after filter:64
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.18935::-0.19781 -> -0.00845.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] current pose is occupancy, go out.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRequestLocalPlan] --> [RunningStageGoOutOccupiedSpace].
[rp.algorithm.motion.BreakOutAction] [INFO] simple move from:(-3.47506,  3.03278) to:(-3.22500,  3.12500).
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.17408-> 0.17376 omega:  0.03805-> 0.03805.
[rp.algorithm.motion.BreakOutAction] [INFO] current pose is absolutely safe, break out success.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] go out occupied space success, start local plan.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageGoOutOccupiedSpace] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-3.4707, 3.0997) to (-1.2250, 2.7285).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1563
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.47068, 3.09969) to (-1.22501, 2.72854), used 15 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.39880::-0.38871 ->  0.01009.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.18512 omega: -0.47069->-0.27419.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-3.2666, 3.0177) to (-1.0750, 2.7295).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1334
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.26662, 3.01771) to (-1.07501, 2.7295), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.30734::-0.66225 -> -0.35491.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2557, path size before filter:70, after filter:65
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.28765::-0.67236 -> -0.38471.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.32342 omega: -0.29138->-0.29138.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36744-> 0.36744 omega: -0.02241->-0.02241.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-2.6081, 2.6669) to (-0.4463, 2.7416).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:699
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.60805, 2.66686) to (-0.446277, 2.7416), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.37401:: 0.07494 -> -0.29906.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2159, path size before filter:68, after filter:62
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.40201:: 0.10624 -> -0.29576.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38610-> 0.34331 omega:  0.65000-> 0.47600.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40295-> 0.40295 omega:  0.20172-> 0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40148-> 0.40148 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40648-> 0.40648 omega: -0.60517->-0.31572.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38724-> 0.38724 omega: -0.33621->-0.33621.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-0.8589, 2.8455) to (0.9800, 1.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:587
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-0.858856, 2.84547) to (0.98, 1.62), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.02813::-0.34225 -> -0.31412.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1100, path size before filter:43, after filter:39
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.02842::-0.34254 -> -0.31412.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37944-> 0.37944 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38297-> 0.38297 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38535-> 0.38535 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37224-> 0.37152 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] start far wait milestone free.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitDelay].
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2005 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 1 ms
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] the milestone[0.9800, 1.6200] is occupied wait timeout and stop nearby, robot pose[0.4454, 1.9622] norm: 0.6347.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitDelay] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitDelay] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] milestones is arrived, finish.
[rp.algorithm.motion.NavigationScheduleAction] [INFO] all milestone is arrived, markAsFinished
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":10},"isCustomerLog":true,"keyword":"end","usElapsed":"29779340","usEpoch":"1663401472964528","usTick":"165396641"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 3, status 10, reason 
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["precise","with_yaw"],"mode":2,"yaw":-0.3499999940395355},"points":[[5.250,-3.530]],"yaw":-0.3499999940395355}},"isCustomerLog":true,"usEpoch":"1663401661601990","usTick":"354034103"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (5.250000, -3.530000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401661602257","usTick":"354034370"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (5.250000, -3.530000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":0.4454069137573242,"y":1.962156414985657},"moveType":"VirtualTrack","robotPose":{"x":0.4454069171316724,"y":1.962156417057899,"yaw":-0.7389929293486723},"to":{"x":5.250,"y":-3.529999971389771}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401661639857","usTick":"354071970"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4454, 1.9622) to (5.2500, -3.5300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:7003
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (0.445407, 1.96216) to (5.25, -3.53), used 36 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 232, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (0.425000, 1.975000) --> (5.250000, -3.530000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = 5.250000, y = -3.530000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4454, 1.9622) to (2.6250, 3.1250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2135
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.445407, 1.96216) to (2.625, 3.125), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2741, path size before filter:69, after filter:67
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] utilize found local path, cost: 55::67 -6.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  1.80225:: 1.06326 -> -0.73899.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.02400-> 0.02400.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 8 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39480-> 0.14040 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43632-> 0.43632 omega:  0.20172-> 0.26721.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40058-> 0.40058 omega: -0.20172->-0.16924.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40122-> 0.40116 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39264-> 0.39264 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38463-> 0.38463 omega:  0.24655-> 0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40418-> 0.40418 omega: -0.51552->-0.51552.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43990-> 0.43358 omega: -0.33621->-0.33621.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45829-> 0.45829 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42883-> 0.42883 omega: -0.38956->-0.38956.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42357-> 0.42357 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42426-> 0.42426 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42238-> 0.42238 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42716-> 0.42716 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42238-> 0.41555 omega: -0.11207->-0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42420-> 0.42420 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42571-> 0.42571 omega:  0.06724-> 0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42301-> 0.42301 omega: -0.11207->-0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42515-> 0.42515 omega:  0.02241-> 0.02241.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 10.16 m, remove it
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 10.16 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42576-> 0.42576 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42223-> 0.42223 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.37931 omega:  0.14240-> 0.14240.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.4577, -2.6889) to (5.2500, -3.5300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2609
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.45772, -2.68891) to (5.25, -3.53), used 33 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1685, path size before filter:45, after filter:44
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.07200::-1.30478 -> -1.37679.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37024-> 0.13751 omega:  0.65000-> 0.40524.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] current pose is occupancy, go out.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRequestLocalPlan] --> [RunningStageGoOutOccupiedSpace].
[rp.algorithm.motion.BreakOutAction] [INFO] simple move from:( 3.50430, -2.94970) to:( 3.72500, -3.27500).
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.13801-> 0.13801 omega:  0.02414-> 0.02414.
[rp.algorithm.motion.BreakOutAction] [INFO] current pose is absolutely safe, break out success.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] go out occupied space success, start local plan.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageGoOutOccupiedSpace] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.5294, -3.1009) to (5.2500, -3.5300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2300
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.52935, -3.10093) to (5.25, -3.53), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.92012::-0.40750 -> -1.32762.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.30466-> 0.17512 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.22414-> 0.17107 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.15474-> 0.11452 omega:  0.60517-> 0.60517.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] current pose is occupancy, go out.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRequestLocalPlan] --> [RunningStageGoOutOccupiedSpace].
[rp.algorithm.motion.BreakOutAction] [INFO] simple move from:( 3.95003, -3.35228) to:( 4.22500, -3.17500).
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.06097-> 0.06028 omega:  0.37170-> 0.27852.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.24428-> 0.15106 omega:  0.03290-> 0.03290.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.11189-> 0.11189 omega: -0.36682->-0.36682.
[rp.algorithm.motion.BreakOutAction] [INFO] current pose is absolutely safe, break out success.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] go out occupied space success, start local plan.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageGoOutOccupiedSpace] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (4.2166, -3.2334) to (5.2500, -3.5300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:355
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (4.21664, -3.23342) to (5.25, -3.53), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.24556::-0.20170 ->  0.04386.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36776-> 0.16288 omega: -0.56034->-0.56917.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37442-> 0.37442 omega: -0.11207->-0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.34105-> 0.32246 omega:  0.24655-> 0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.12069-> 0.11455 omega: -0.15690->-0.15690.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] precise move end!, milestone is (5.2500, -3.5300, -0.3500), pose is (5.2294, -3.5172, -0.3016), error is (0.0242,2.7725)
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [AdjustYaw].
[rp.algorithm.motion.AdjustYawAction] [INFO] onStart, required yaw : -0.350000, timeout : 60000
[rp.algorithm.motion.AdjustYawAction] [INFO] StageRotate -> StageRecheck
[rp.algorithm.motion.AdjustYawAction] [INFO] adjust yaw success. yaw: -0.35000 -> -0.30691
[rp.algorithm.motion.AdjustYawAction] [INFO] onStop
[rp.algorithm.motion.NavigationScheduleAction] [INFO] all milestone is arrived, markAsFinished
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":10},"isCustomerLog":true,"keyword":"end","usElapsed":"36421655","usEpoch":"1663401698023912","usTick":"390456025"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 4, status 10, reason 
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.00000->-0.12740.
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2032 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 1 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["with_yaw"],"mode":2,"yaw":0.0},"points":[[-1.920,3.130]],"yaw":0.0}},"isCustomerLog":true,"usEpoch":"1663401743902471","usTick":"436334584"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-1.920000, 3.130000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401743902856","usTick":"436334969"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":5.244780063629150,"y":-3.511097192764282},"moveType":"VirtualTrack","robotPose":{"x":5.244779931098140,"y":-3.511097101195225,"yaw":-0.3568214550592792},"to":{"x":-1.919999957084656,"y":3.130000114440918}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401743930142","usTick":"436362254"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (5.2448, -3.5111) to (-1.9200, 3.1300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:5427
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (5.24478, -3.5111) to (-1.92, 3.13), used 79 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 267, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (5.208333, -3.508333) --> (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -1.920000, y = 3.130000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (5.2448, -3.5111) to (3.3750, -1.3250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:977
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (5.24478, -3.5111) to (3.375, -1.325), used 18 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:3105, path size before filter:82, after filter:81
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] utilize found local path, cost: 71::81 -9.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -3.09829:: 2.82807 -> -0.35682.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.01200->-0.01200.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 7 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.19652-> 0.09240 omega: -0.65000->-0.65000.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.94606).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (5.2499, -3.5212) to (3.3615, -1.3419).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1056
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (5.2499, -3.52117) to (3.36152, -1.34191), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (5.2234, -3.5295) to (3.3615, -1.3419).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1080
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (5.2234, -3.52948) to (3.36152, -1.34191), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.79933:: 2.76287 -> -2.72098.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:3682, path size before filter:88, after filter:83
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.79447:: 2.76093 -> -2.72778.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39655-> 0.27970 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40259-> 0.40259 omega:  0.15690-> 0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36207-> 0.36207 omega: -0.29138->-0.29138.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.07724-> 0.21127 omega:  0.65000-> 0.57200.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] current pose is occupancy, go out.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRequestLocalPlan] --> [RunningStageGoOutOccupiedSpace].
[rp.algorithm.motion.BreakOutAction] [INFO] simple move from:( 4.05771, -3.17149) to:( 3.77500, -3.02500).
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.12451-> 0.12451 omega: -0.13346->-0.13346.
[rp.algorithm.motion.BreakOutAction] [INFO] current pose is absolutely safe, break out success.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] go out occupied space success, start local plan.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageGoOutOccupiedSpace] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.9496, -3.1664) to (3.3528, -0.9279).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:764
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.94964, -3.16642) to (3.35277, -0.927936), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.62815:: 2.40231 ->  3.03046.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.31645 omega: -0.53782->-0.53782.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39395-> 0.38891 omega: -0.38103->-0.34255.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40965-> 0.40965 omega: -0.51552->-0.51552.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40602-> 0.40602 omega: -0.15690->-0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45503-> 0.45503 omega: -0.11207->-0.08541.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44575-> 0.44575 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44074-> 0.44074 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42383-> 0.42383 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42568-> 0.42568 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42004-> 0.42004 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42705-> 0.42705 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41918-> 0.41918 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40854-> 0.40854 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38535-> 0.38535 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3647, 2.4160) to (1.0750, 3.1250).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.70736).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2417
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36467, 2.41601) to (1.075, 3.125), used 19 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.68243).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3635, 2.4312) to (1.0750, 3.1250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2106
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36354, 2.43124) to (1.075, 3.125), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3605, 2.4650) to (1.0750, 3.1250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2106
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36054, 2.465) to (1.075, 3.125), used 10 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2366, path size before filter:69, after filter:68
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.07857 omega:  0.00000-> 0.20600.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.91761:: 2.62970 ->  1.71209.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3302, 2.4968) to (1.1250, 3.0750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2344
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.33022, 2.49676) to (1.125, 3.075), used 55 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.04375:: 2.93109 ->  1.88734.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not reach path update freq time and less than rotate angle, ignore this.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.79701:: 2.68764 ->  1.89063.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3272, 2.5124) to (1.1250, 3.0750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.49943).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2319
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.32718, 2.51245) to (1.125, 3.075), used 33 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.49333).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3252, 2.5179) to (1.1250, 3.0750).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2364
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.32522, 2.51787) to (1.125, 3.075), used 56 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.48583).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3211, 2.5278) to (1.1250, 3.0750).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2439
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.3211, 2.52781) to (1.125, 3.075), used 33 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3179, 2.5128) to (1.1250, 3.0750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2420
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.31786, 2.51281) to (1.125, 3.075), used 12 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.68797:: 2.79333 ->  2.10536.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1815, path size before filter:66, after filter:65
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.76158:: 2.87203 ->  2.11045.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43363-> 0.12990 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":-2},"isCustomerLog":true,"keyword":"end","usElapsed":"26059284","usEpoch":"1663401769962140","usTick":"462394253"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 5, status -2, reason aborted
[rp.slamware.server.SlamwareServerAgent] [INFO] Action #5 is under status error
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2004 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 7 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"flags":["milestone","precise"],"mode":0},"points":[[-6.570000171661377,3.180000066757202]]}},"isCustomerLog":true,"usEpoch":"1663401938729076","usTick":"631161188"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-6.570000, 3.180000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"Basic"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663401938730012","usTick":"631162125"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (-6.570000, 3.180000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":3.175937175750732,"y":2.649059057235718},"moveType":"Basic","robotPose":{"x":3.175937136299026,"y":2.649059146260112,"yaw":2.638774820812419},"to":{"x":-6.570000171661377,"y":3.180000066757202}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401938742969","usTick":"631175082"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.1759, 2.6491) to (-6.5700, 3.1800).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:11547
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, Basic, (3.17594, 2.64906) to (-6.57, 3.18), used 33 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 209, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (3.158334, 2.641667) --> (-6.570000, 3.180000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -6.570000, y = 3.180000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.1759, 2.6491) to (0.9252, 2.8395).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2058
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.17594, 2.64906) to (0.92521, 2.83949), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.24567:: 2.39311 ->  2.63877.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36000-> 0.00780 omega: -0.29138->-0.07800.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 5 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44828-> 0.31290 omega:  0.33621-> 0.28290.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44063-> 0.44063 omega:  0.24655-> 0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40923-> 0.40566 omega:  0.06724-> 0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.32759-> 0.32759 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.24138-> 0.24138 omega: -0.06724->-0.06724.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.99483).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (1.4618, 2.8427) to (-0.8241, 2.8902).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1657
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (1.46185, 2.84274) to (-0.824055, 2.89019), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2327, path size before filter:69, after filter:67
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.16004:: 2.95212 ->  3.11216.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.07027 omega:  0.65000-> 0.51000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.37177 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40510-> 0.40510 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5019, 2.5147) to (-1.6228, 2.9090).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1922
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.501942, 2.51467) to (-1.62276, 2.90904), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.37376:: 2.98555 -> -2.92388.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2152, path size before filter:66, after filter:62
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.28410:: 3.07271 -> -2.92638.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4729, 2.5082) to (-1.8345, 2.9461).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.29797).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2096
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.472919, 2.50824) to (-1.83452, 2.9461), used 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.27689).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4485, 2.5032) to (-1.8345, 2.9461).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2665
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.448503, 2.50324) to (-1.83452, 2.9461), used 17 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.05480 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4576, 2.5068) to (-1.8345, 2.9461).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2489
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.457568, 2.5068) to (-1.83452, 2.9461), used 29 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4665, 2.5084) to (-1.8345, 2.9461).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2479
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.466529, 2.50838) to (-1.83452, 2.9461), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4754, 2.5100) to (-1.8345, 2.9461).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2519
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.475422, 2.50997) to (-1.83452, 2.9461), used 14 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4816, 2.5111) to (-1.8345, 2.9461).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2520
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[0.4829,2.5113] to milestone[-6.5700, 3.1800] is: 7.0845, acceptable distance:0.1000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] Move out last failed location, clear failureCounter_
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (0.481599, 2.51109) to (-1.83452, 2.9461), used 15 ms, canceled when filtering path..
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":0.4846229553222656,"y":2.511652708053589},"moveType":"Basic","robotPose":{"x":0.4846229503164091,"y":2.511652683625694,"yaw":-2.958182372889579},"to":{"x":-6.570000171661377,"y":3.180000066757202}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401948576113","usTick":"641008226"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4846, 2.5117) to (-6.5700, 3.1800).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:8690
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, Basic, (0.484623, 2.51165) to (-6.57, 3.18), used 49 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 171, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (0.475000, 2.525000) --> (-6.570000, 3.180000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -6.570000, y = 3.180000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4890, 2.5125) to (-1.7724, 3.0697).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2567
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.489031, 2.51248) to (-1.77244, 3.06968), used 21 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -2.50108:: 0.83082 -> -2.95129.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.47400->-0.43800.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.31145-> 0.14700 omega: -0.47069->-0.17148.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.32759-> 0.32759 omega: -0.02241->-0.02241.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] reduce path failed, because, path begin occupied.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5884, 2.9412) to (-1.6729, 3.0789).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2101
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.58842, 2.94124) to (-1.67287, 3.0789), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2735, path size before filter:71, after filter:65
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  2.58317::-2.15689 ->  1.54312.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.27600-> 0.15300.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.62294).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5086, 2.9888) to (-1.7688, 3.0702).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2212
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.508639, 2.98877) to (-1.76879, 3.07016), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.00000-> 0.05700.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied stable, angle:  0.25000, dist:  0.25000, move:  0.05500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx: -0.15000->-0.04140 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.6134, 2.9941) to (-1.6194, 3.0841).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2111
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.613405, 2.99412) to (-1.61944, 3.08409), used 33 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.88449::-2.16749 -> -3.05197.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2328, path size before filter:66, after filter:59
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.88210::-2.17047 -> -3.05257.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.11846-> 0.08550 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.4928, 2.9736) to (-1.7742, 3.0561).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2120
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.492766, 2.97356) to (-1.77416, 3.05614), used 21 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.45216::-2.13921 -> -2.59136.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2084, path size before filter:65, after filter:61
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.44687::-2.13830 -> -2.58516.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5103, 2.9644) to (-1.7573, 3.0717).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2262
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.510321, 2.96438) to (-1.75734, 3.07175), used 12 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.36899::-2.10834 -> -2.47733.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:3109, path size before filter:72, after filter:68
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.27267::-2.20909 -> -2.48176.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStageRequestLocalPlan] --> [RunningStageBackUpIfContacted].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] is contacted, go backward distance:0.050000
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx: -0.15000->-0.06992 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageBackUpIfContacted] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5473, 2.9936) to (-1.7750, 3.0283).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1596
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.547308, 2.99364) to (-1.77498, 3.02825), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.23742::-2.11828 -> -2.35570.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.5558, 3.0021) to (-1.7750, 3.0283).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.12869).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2475
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.555792, 3.00209) to (-1.77498, 3.02825), used 12 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied stable, angle:  0.25000, dist:  0.25000, move:  0.05500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx: -0.15000->-0.01230 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.6290, 3.0422) to (-1.6250, 3.0263).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1981
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.629043, 3.04217) to (-1.62499, 3.0263), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000->-0.04500 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied stable, angle:  0.25000, dist:  0.25000, move:  0.05500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx: -0.15000->-0.09780 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onReturnFromChild: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.6838, 3.1233) to (-1.5750, 3.0257).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1909
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.68385, 3.12326) to (-1.575, 3.02565), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7051, 3.1445) to (-1.5250, 3.0250).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2088
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.705149, 3.14454) to (-1.525, 3.025), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7087, 3.1481) to (-1.5250, 3.0250).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] find free path failed, none free cost:128.
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2102
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.708683, 3.14808) to (-1.525, 3.025), used 10 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[0.7094,3.1488] to milestone[-6.5700, 3.1800] is: 7.2795, acceptable distance:0.1000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] Move out last failed location, clear failureCounter_
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":0.7099519968032837,"y":3.149345874786377},"moveType":"Basic","robotPose":{"x":0.7099519836202289,"y":3.149345937164457,"yaw":-2.356434345245361},"to":{"x":-6.570000171661377,"y":3.180000066757202}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401964537020","usTick":"656969133"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7100, 3.1493) to (-6.5700, 3.1800).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:16932
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] retry only use global cost map when search path Failed
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 1
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7100, 3.1493) to (-6.5700, 3.1800).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:6466
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, Basic, (0.709952, 3.14935) to (-6.57, 3.18), used 14 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 160, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (0.725000, 3.125000) --> (-6.570000, 3.180000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -6.570000, y = 3.180000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] nearby occupied, stop and recheck many times.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7127, 3.1521) to (-1.5749, 3.0530).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2066
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.712672, 3.15206) to (-1.57487, 3.05299), used 17 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local pathfinder failed, need move to obstacle continue.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.59634::-2.95278 -> -2.35643.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7124, 3.1517) to (-1.5749, 3.0530).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2063
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.712355, 3.15175) to (-1.57487, 3.05299), used 7 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18287).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18305).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7124, 3.1518) to (-1.5749, 3.0530).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2209
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.7124, 3.15179) to (-1.57487, 3.05299), used 47 ms.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7126, 3.1520) to (-1.5749, 3.0530).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18328).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2048
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.712582, 3.15197) to (-1.57487, 3.05299), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18322).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7126, 3.1520) to (-1.5749, 3.0530).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2075
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.712627, 3.15202) to (-1.57487, 3.05299), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7125, 3.1519) to (-1.5749, 3.0530).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18311).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2051
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (0.712536, 3.15193) to (-1.57487, 3.05299), used 24 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7125, 3.1519) to (-1.5749, 3.0530).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.18317).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2275
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[0.7125,3.1519] to milestone[-6.5700, 3.1800] is: 7.2826, acceptable distance:0.1000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":0.7125364542007446,"y":3.151928901672363},"moveType":"Basic","robotPose":{"x":0.7125364421943245,"y":3.151929014857451,"yaw":-2.356434345245361},"to":{"x":-6.570000171661377,"y":3.180000066757202}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663401965349773","usTick":"657781886"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (0.712536, 3.15193) to (-1.57487, 3.05299), used 25 ms, canceled when filtering path..
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7125, 3.1519) to (-6.5700, 3.1800).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching global path status: Canceled, Basic, (0.712536, 3.15193) to (-6.57, 3.18), used 9 ms, canceled when finding..
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":-2},"isCustomerLog":true,"keyword":"end","usElapsed":"26645611","usEpoch":"1663401965375623","usTick":"657807736"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 6, status -2, reason aborted
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx: -0.50000->-0.00090 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 7, status 10, reason 
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2001 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 10 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["precise","with_yaw"],"mode":2,"yaw":0.0},"points":[[3.580,21.090]],"yaw":0.0}},"isCustomerLog":true,"usEpoch":"1663402479552310","usTick":"1171984423"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (3.580000, 21.090000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663402479552624","usTick":"1171984737"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (3.580000, 21.090000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":0.7230516672134399,"y":3.162438631057739},"moveType":"VirtualTrack","robotPose":{"x":0.7230516611632803,"y":3.162438625229306,"yaw":-2.356434345245361},"to":{"x":3.579999923706055,"y":21.09000015258789}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663402479614297","usTick":"1172046410"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7231, 3.1624) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:10633
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (0.723052, 3.16244) to (3.58, 21.09), used 40 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 418, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (0.725000, 3.175000) --> (3.580000, 21.090000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = 3.580000, y = 21.090000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (0.7231, 3.1624) to (2.9250, 3.1250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1116
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (0.723052, 3.16244) to (2.925, 3.125), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  2.38422:: 0.02779 -> -2.35643.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.08100-> 0.00600.
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 4 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39240-> 0.14790 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42419-> 0.42419 omega:  0.38103-> 0.38103.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41914-> 0.41914 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38720-> 0.38720 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.34483-> 0.34483 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38448-> 0.38448 omega:  0.65000-> 0.37676.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44029-> 0.43316 omega:  0.51552-> 0.50386.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43996-> 0.43996 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42367-> 0.42367 omega:  0.29138-> 0.29138.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42505-> 0.42505 omega:  0.00000-> 0.02224.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42477-> 0.42477 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41892-> 0.41892 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43440-> 0.42820 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42554-> 0.42123 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42864-> 0.42864 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42234-> 0.42234 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41002-> 0.41002 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41965-> 0.41965 omega:  0.00000-> 0.00000.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 10.94 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42452-> 0.42452 omega:  0.06724-> 0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42550-> 0.42550 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42172-> 0.42172 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42550-> 0.42550 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41955-> 0.41955 omega:  0.02241-> 0.02241.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 12.38 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42399-> 0.42399 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43423-> 0.43423 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43951-> 0.43951 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43318-> 0.43318 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44182-> 0.44182 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43207-> 0.40607 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43210-> 0.43210 omega: -0.15690->-0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42837-> 0.42837 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43800-> 0.41670 omega: -0.02241->-0.02241.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 11.08 m, remove it
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 11.08 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44013-> 0.44013 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43856-> 0.43856 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40367-> 0.40367 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42438-> 0.42438 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43407-> 0.43407 omega:  0.00000-> 0.00000.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 13.04 m, remove it
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 12.48 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43599-> 0.43599 omega: -0.06724->-0.04941.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43804-> 0.43804 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43855-> 0.43855 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43916-> 0.43916 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42894-> 0.42894 omega:  0.02241-> 0.02241.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 15.02 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38592-> 0.38223 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38352-> 0.38352 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.2439, 19.2640) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1197
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.24387, 19.264) to (3.58, 21.09), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:861, path size before filter:41, after filter:37
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.76980:: 0.77809 ->  1.54789.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41821-> 0.38014 omega: -0.65000->-0.38507.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.2735, 19.5432) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1631
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.27351, 19.5432) to (3.58, 21.09), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff : -0.43801:: 0.73212 ->  1.17013.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:629, path size before filter:28, after filter:27
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.43485:: 0.73008 ->  1.16493.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43103-> 0.30528 omega: -0.51867->-0.55734.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3524, 19.7031) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1473
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.35241, 19.7031) to (3.58, 21.09), used 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.34748:: 2.18507 ->  0.83758.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not reach path update freq time and less than rotate angle, ignore this.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.19819:: 0.61799 ->  0.81618.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.51528).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3729, 19.7257) to (3.5800, 21.0900).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1381
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.37293, 19.7257) to (3.58, 21.09), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3776, 19.7238) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.49118).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1366
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.3776, 19.7238) to (3.58, 21.09), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3913, 19.7373) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.47366).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1361
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.3913, 19.7373) to (3.58, 21.09), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3983, 19.7443) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.46808).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1336
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.39831, 19.7443) to (3.58, 21.09), used 10 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.4003, 19.7462) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1312
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.40026, 19.7462) to (3.58, 21.09), used 7 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.46687).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.4009, 19.7468) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.46636).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1284
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.40086, 19.7468) to (3.58, 21.09), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[3.4008,19.7467] to milestone[3.5800, 21.0900] is: 1.3552, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] Move out last failed location, clear failureCounter_
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":3.400538682937622,"y":19.74648475646973},"moveType":"VirtualTrack","robotPose":{"x":3.400538664319948,"y":19.74648501528818,"yaw":0.7699612864073813},"to":{"x":3.579999923706055,"y":21.09000015258789}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663402528935650","usTick":"1221367763"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.4005, 19.7465) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:685
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (3.40054, 19.7465) to (3.58, 21.09), used 4 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 37, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (3.425000, 19.725000) --> (3.580000, 21.090000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = 3.580000, y = 21.090000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.4000, 19.7459) to (3.5800, 21.0900).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1266
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.39999, 19.7459) to (3.58, 21.09), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:680, path size before filter:29, after filter:28
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] utilize found local path, cost: 37::28 -8.00000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  1.57671:: 2.34956 ->  0.77286.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.60000-> 0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37316-> 0.07710 omega:  0.65000-> 0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.46552-> 0.37770 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45684-> 0.44987 omega: -0.60744->-0.57229.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 16.94 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.27586-> 0.27586 omega: -0.24655->-0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.12069-> 0.12069 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.07546-> 0.07546 omega: -0.11038->-0.18943.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] precise move end!, milestone is (3.5800, 21.0900, 0.0000), pose is (3.5636, 21.0720, 0.5857), error is (0.0244,33.5561)
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStagePreciseMove] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [AdjustYaw].
[rp.algorithm.motion.AdjustYawAction] [INFO] onStart, required yaw : 0.000000, timeout : 60000
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.17453->-0.45500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.17453->-0.17453.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.17453->-0.17453.
[rp.algorithm.motion.AdjustYawAction] [INFO] StageRotate -> StageRecheck
[rp.algorithm.motion.AdjustYawAction] [INFO] adjust yaw success. yaw:  0.00000 ->  0.09858
[rp.algorithm.motion.AdjustYawAction] [INFO] onStop
[rp.algorithm.motion.NavigationScheduleAction] [INFO] all milestone is arrived, markAsFinished
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":10},"isCustomerLog":true,"keyword":"end","usElapsed":"58960179","usEpoch":"1663402538512803","usTick":"1230944916"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 8, status 10, reason 
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["with_yaw"],"mode":2,"yaw":0.0},"points":[[-1.920,3.130]],"yaw":0.0}},"isCustomerLog":true,"usEpoch":"1663402540169673","usTick":"1232601786"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-1.920000, 3.130000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663402540169824","usTick":"1232601937"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] schedulerService doesn't create
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [MapAlignment] --> [Undock].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onSensorData: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":3.576287508010864,"y":21.08784866333008},"moveType":"VirtualTrack","robotPose":{"x":3.576287536718038,"y":21.08784801896350,"yaw":0.07759132633156750},"to":{"x":-1.919999957084656,"y":3.130000114440918}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663402540189764","usTick":"1232621876"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.5763, 21.0878) to (-1.9200, 3.1300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:10221
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (3.57629, 21.0878) to (-1.92, 3.13), used 62 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 467, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (3.575000, 21.075001) --> (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -1.920000, y = 3.130000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.5763, 21.0878) to (3.2750, 18.8250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1059
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.57629, 21.0878) to (3.275, 18.825), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -2.41496::-2.33737 ->  0.07759.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.00600->-0.00600.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.60000->-0.60000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43071-> 0.14040 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.48325-> 0.44280 omega:  0.15690-> 0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45964-> 0.45964 omega:  0.24655-> 0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45700-> 0.44976 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44964-> 0.44964 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.45074-> 0.45074 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42110-> 0.42110 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42511-> 0.42511 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42660-> 0.42660 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42714-> 0.42714 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42505-> 0.42505 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42463-> 0.42463 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42446-> 0.42446 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42598-> 0.42598 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43477-> 0.43477 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44051-> 0.43945 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42705-> 0.42705 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43954-> 0.43954 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42835-> 0.42835 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43923-> 0.43923 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44010-> 0.44010 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.24138-> 0.22744 omega: -0.02241->-0.03424.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.99453).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.2652, 12.1017) to (3.3250, 9.8750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1778
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.26515, 12.1017) to (3.325, 9.875), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2736, path size before filter:68, after filter:66
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.02896::-1.66542 -> -1.63646.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42280-> 0.16440 omega:  0.31159-> 0.34417.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.20690-> 0.20690 omega:  0.06724-> 0.06724.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.81197).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3291, 11.9650) to (3.3378, 9.7417).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1037
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.32912, 11.965) to (3.33776, 9.7417), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.79007).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3411, 11.9344) to (3.3370, 9.6917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1036
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.34113, 11.9344) to (3.33698, 9.6917), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.77699).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3469, 11.9200) to (3.3370, 9.6917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1029
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.34694, 11.92) to (3.33698, 9.6917), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3485, 11.9162) to (3.3370, 9.6917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1044
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.34848, 11.9162) to (3.33698, 9.6917), used 5 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.77520).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.77794).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3475, 11.9186) to (3.3370, 9.6917).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[3.3474,11.9189] to milestone[-1.9200, 3.1300] is: 10.2465, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] Move out last failed location, clear failureCounter_
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (3.34752, 11.9186) to (3.33698, 9.6917), used 5 ms, canceled when finding..
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":3.347476243972778,"y":11.91872978210449},"moveType":"VirtualTrack","robotPose":{"x":3.347476172454177,"y":11.91872933550576,"yaw":-1.186049469472836},"to":{"x":-1.919999957084656,"y":3.130000114440918}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663402567175627","usTick":"1259607740"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3475, 11.9187) to (-1.9200, 3.1300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:7042
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (3.34748, 11.9187) to (-1.92, 3.13), used 33 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 283, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (3.325000, 11.925000) --> (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -1.920000, y = 3.130000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3481, 11.9173) to (3.3250, 9.6750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1044
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.34805, 11.9173) to (3.325, 9.675), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.54878::-1.73433 -> -1.18555.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36000-> 0.08760 omega: -0.65000->-0.65000.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3577, 11.8790) to (3.3250, 9.6250).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.67407).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1182
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.35772, 11.879) to (3.325, 9.625), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.98381).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3639, 11.7510) to (3.3250, 9.5250).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1236
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36391, 11.751) to (3.325, 9.525), used 14 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.97549).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3643, 11.7421) to (3.3250, 9.4750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1548
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36432, 11.7421) to (3.325, 9.475), used 12 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3636, 11.7205) to (3.3250, 9.4750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1550
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.95964).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36361, 11.7205) to (3.325, 9.475), used 22 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.95926).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3636, 11.7187) to (3.3250, 9.4750).
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1551
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36355, 11.7187) to (3.325, 9.475), used 10 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3636, 11.7205) to (3.3250, 9.4750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1644
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.96151).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36364, 11.7205) to (3.325, 9.475), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] action failed because plan_hotmap map time after time hit.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[3.3637,11.7214] to milestone[-1.9200, 3.1300] is: 10.0861, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitCostMapUpdate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [RequestPathFinder].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.325000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":3.363735437393188,"y":11.72278404235840},"moveType":"VirtualTrack","robotPose":{"x":3.363735524452535,"y":11.72278404917485,"yaw":-1.611407955744957},"to":{"x":-1.919999957084656,"y":3.130000114440918}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663402568362330","usTick":"1260794443"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3637, 11.7228) to (-1.9200, 3.1300).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:9644
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (3.36374, 11.7228) to (-1.92, 3.13), used 38 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 279, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (3.375000, 11.725000) --> (-1.920000, 3.130000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RecheckPath] --> [MoveByWayPoints].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] setWayPoints: [RunningStageUnknown] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] not support static object obstacle service.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] invokeMoveByWayPointsAction milestoneLocation x = -1.920000, y = 3.130000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.3639, 11.7285) to (3.3250, 9.4750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1637
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.36388, 11.7285) to (3.325, 9.475), used 8 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.54344::-2.09976 -> -1.55632.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.29059-> 0.04590 omega: -0.65000->-0.45900.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43103-> 0.36120 omega: -0.38630->-0.38630.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41379-> 0.32129 omega:  0.02241-> 0.12390.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.97558).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.0714, 11.2816) to (3.3250, 9.0250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1482
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.07142, 11.2816) to (3.325, 9.025), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2657, path size before filter:63, after filter:60
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.27241::-1.95297 -> -2.22538.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] check path occupied in front, update local path.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (3.0652, 11.2707) to (3.2616, 9.0352).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1458
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (3.06522, 11.2707) to (3.26159, 9.03522), used 10 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.03996::-2.17803 -> -2.21799.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2096, path size before filter:67, after filter:66
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.03972::-2.17807 -> -2.21779.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40249-> 0.13320 omega:  0.29138-> 0.29138.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.30960 omega:  0.38103-> 0.38103.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36207-> 0.31608 omega:  0.51552-> 0.42638.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.37931 omega: -0.02241->-0.02241.
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 10.26 m, remove it
[rp.algorithm.slamsdp.LidarObservationFilter] [INFO] distance from first scan in angle queue is 10.26 m, remove it
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38624-> 0.38624 omega:  0.24655-> 0.24655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40259-> 0.40259 omega:  0.11207-> 0.11207.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42152-> 0.42152 omega: -0.29138->-0.29138.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.44215-> 0.44215 omega: -0.15690->-0.15690.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42894-> 0.42894 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42399-> 0.42079 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42604-> 0.42604 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42326-> 0.42326 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42415-> 0.42415 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42664-> 0.42207 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42144-> 0.42144 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.41790-> 0.41790 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40623-> 0.40623 omega: -0.02241->-0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38704-> 0.38704 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.37931 omega: -0.06724->-0.06724.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.39272-> 0.39272 omega: -0.65000->-0.65000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43357-> 0.42537 omega: -0.42586->-0.42021.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.42536-> 0.42536 omega: -0.47361->-0.58400.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.40311-> 0.40311 omega: -0.20172->-0.20172.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.38350-> 0.38350 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.29310-> 0.29310 omega:  0.02241-> 0.02241.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.20690-> 0.20690 omega: -0.11207->-0.11207.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.99993).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (1.5653, 3.0853) to (-0.6750, 3.1250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2307
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (1.56529, 3.0853) to (-0.675, 3.125), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:867, path size before filter:65, after filter:63
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.39451::-2.78495 ->  3.10373.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37931-> 0.24270 omega:  0.43589-> 0.43589.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":-2},"isCustomerLog":true,"keyword":"end","usElapsed":"58244302","usEpoch":"1663402598414126","usTick":"1290846239"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 9, status -2, reason aborted
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2025 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 1 ms
[rp.algorithm.motion.SensorMonitor] [INFO] isMoving
[rp.algorithm.motion.SensorMonitor] [INFO] StageCheckSensorStatus change to StageWakeUpSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageWakeUpSensor change to StageWaitStationayOrPureRotate, duration : 5 ms
[rp.slamware.system.HealthUpdatingService#0] [WARN] BaseDriverHealthProvider reported health status: 0x2010700::motor brake released
[rp.slamware.system.BaseMonitorLogSupplier] [INFO] {"category":"base.status","data":{"isCharging":true,"isOnDock":true},"isCustomerLog":true,"usEpoch":"1663402627270105","usTick":"1319702218"}
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:d36cce36-da92-43da-9af5-670dbab2ee95
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:ff23f0d4-1c38-43bb-8af3-67769b71c6fe
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:d36cce36-da92-43da-9af5-670dbab2ee95
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (1.358242, 0.576026, 179.965769)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.358242, 0.576026, 179.965769)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.358242, 0.576026, 179.965769)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.358242, 0.576026, 179.965769)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.358242, 0.576026, 179.965769)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":505,"y":1255},"mapPose":{"x":-2.224996328353882,"y":13.27500343322754},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663402628041489","usTick":"1320473602"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-4.50000 y: 3.28000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.12000 y: 3.08000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.84000 y: 3.24000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-0.10000 y: 3.19000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-0.29000 y: 3.12000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-3.97000 y: 3.29000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.26000 y:11.11000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.28000 y: 3.15000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.58000 y:21.09000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 5.25000 y:-3.53000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.92000 y: 3.13000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 3.11000 y: 3.02000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:30a6b53e-d2e9-4740-b499-b79e28f996fa, rectangle area count:2
[rp.slamware.motion.ElevatorAreaService#0] [INFO] clear all elevator areas
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 162.
[rp.algorithm.slam.CSLAMBuilder] [INFO] Switched to AMCL Mapping!
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"result":true},"isCustomerLog":true,"keyword":"end","usElapsed":"769630","usEpoch":"1663402628811119","usTick":"1321243232"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (1.207480, 0.440591, -178.716667)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.207480, 0.440591, -178.716667)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.207480, 0.440591, -178.716667)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (1.207480, 0.440591, -178.716667)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (1.207480, 0.440591, -178.716667)
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2003 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 9 ms
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 44.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 34.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 54.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 57.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 53.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 54.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 47.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 49.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 47.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 46.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 42.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 45.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 46.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 33.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 54.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 49.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 33.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start rebuild virtual track assist map.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] virtual track assist rebuilt (5767 us).
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:1576 ms, raw area:-297,-362,506,1255
[rp.slamware.system.BaseMonitorLogSupplier] [INFO] {"category":"base.status","data":{"isCharging":false},"isCustomerLog":true,"usEpoch":"1663402744208958","usTick":"1436641071"}
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] check directory(/root)
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] output statistics info to file(/root/statistics.info)
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] output statistics info to file(/root/statistics.info.bk)
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] check directory(/root)
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] output statistics info to file(/root/statistics.info)
[rp.algorithm.slamsdp.RpStatisticsLoader] [INFO] output statistics info to file(/root/statistics.info.bk)
