[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47522, gap: 222.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47548, gap: 248.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47574, gap: 274.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47600, gap: 300.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47625, gap: 325.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47651, gap: 351.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47676, gap: 376.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47702, gap: 402.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47728, gap: 428.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47754, gap: 454.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47780, gap: 480.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47806, gap: 506.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47832, gap: 532.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47857, gap: 557.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47883, gap: 583.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47908, gap: 608.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47934, gap: 634.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47959, gap: 659.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 47985, gap: 685.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48011, gap: 711.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48037, gap: 737.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48063, gap: 763.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48089, gap: 789.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48115, gap: 815.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48141, gap: 841.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48167, gap: 867.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48193, gap: 893.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48219, gap: 919.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48245, gap: 945.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48270, gap: 970.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48296, gap: 996.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48322, gap: 1022.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48348, gap: 1048.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48374, gap: 1074.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48400, gap: 1100.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48426, gap: 1126.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48451, gap: 1151.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48477, gap: 1177.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48503, gap: 1203.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48528, gap: 1228.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48554, gap: 1254.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48580, gap: 1280.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48605, gap: 1305.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48631, gap: 1331.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48656, gap: 1356.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48683, gap: 1383.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48709, gap: 1409.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48735, gap: 1435.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48761, gap: 1461.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48787, gap: 1487.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48813, gap: 1513.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 48839, gap: 1539.
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[rp.applet.server.RpSlamServer] [INFO] {"category":"client.tcp.connection","data":{"ip":"127.0.0.1","port":35196},"isCustomerLog":true,"keyword":"received","usEpoch":"1663068683576412","usTick":"57382966"}
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (0.000000, 0.000000, -0.954009)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (0.000000, 0.000000, -0.954009)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (0.000000, 0.000000, -0.954009)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (0.000000, 0.000000, -0.954009)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (0.000000, 0.000000, -0.954009)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":399,"y":543},"mapPose":{"x":-1.924995660781860,"y":0.6750034689903259},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663068689453261","usTick":"63259815"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.51000 y:-1.05000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.34000 y:-4.90000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.86000 y:-4.17000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.88000 y: 0.50000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.47000 y:-5.35000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.82000 y:-0.62000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 1.14000 y:-1.97000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.98000 y:-3.80000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:005c12e7-dfd2-4c38-ba59-df9358945b5b, rectangle area count:7
[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 279.
[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 (13696 us).
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:638 ms, raw area:-19,-200,154,344
[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":"1145470","usEpoch":"1663068690598731","usTick":"64405285"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] transformExploredMapDataFromHiddmap
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :283 ms
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.motion.RandomRelocalizationAction] [INFO] Receive Relocalization task!!!
[rp.algorithm.slam.CSLAMBuilder] [INFO] startRelocalization
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationService#0] [INFO] merge size :1
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 176, icpInfo.goodness = 0.662046, ICP takes: 20 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.719135,  y =  3.500593, yaw = -0.032683, takes : 0.345000 s
[rp.algorithm.slam.RelocalizationService#0] [INFO] pose : (-3.719135, 3.500593, -0.032683)
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 246, icpInfo.goodness = 0.662175, ICP takes: 32 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.728087,  y =  3.495470, yaw = -0.031043, takes : 0.400000 s
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 401 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] both reloc success
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.728087, 3.495470, -1.778643)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.728087, 3.495470, -1.778643)
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 1, status 10, reason 
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.728087, 3.495470, -1.778643)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.728087, 3.495470, -1.778643)
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :403 ms
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:466 ms, raw area:-204,-257,282,488
[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 : 3 ms
[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: 10 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["with_yaw"],"mode":2,"yaw":0.05000000074505806},"points":[[-2.820,-0.620]],"yaw":0.05000000074505806}},"isCustomerLog":true,"usEpoch":"1663069530696154","usTick":"904508569"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-2.820000, -0.620000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663069530696417","usTick":"904508832"}
[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":"1663069530717441","usTick":"904529856"}
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.16000-> 0.00100 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":{"isCharging":false,"isOnDock":false},"isCustomerLog":true,"usEpoch":"1663069531149327","usTick":"904961741"}
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.15999-> 0.15999 omega: -0.00005->-0.00005.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.15996-> 0.15996 omega: -0.00023->-0.00023.
[rp.algorithm.motion.UnDockController] [INFO] move to landing points success
[rp.algorithm.motion.NavigationScheduleAction] [INFO] onReturnFromChild: [Undock] --> [MoveToMilestone].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] doInvokeChildMoveToMilestoneAction_ : (-2.820000, -0.620000)
[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.465667247772217,"y":3.494774818420410},"moveType":"VirtualTrack","robotPose":{"x":-3.465667228465818,"y":3.494774746896121,"yaw":-0.03264290567762096},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663069532775256","usTick":"906587670"}
[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.4657, 3.4948) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3000
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 93, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-3.458333, 3.458333) --> (-2.820000, -0.620000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [WaitPathFinderResult] --> [RecheckPath].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-3.46567, 3.49477) to (-2.82, -0.62), used 31 ms.
[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 = -2.820000, y = -0.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 (-3.4495, 3.4942) to (-2.0083, 0.4750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2887
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.44953, 3.49425) to (-2.00833, 0.475), used 47 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -1.10107::-1.13371 -> -0.03264.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.67500->-0.67500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36058-> 0.01800 omega:  0.42096->-0.22542.
[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.4244, 3.4880) to (-2.0083, 0.4750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2862
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-3.42442, 3.48799) to (-2.00833, 0.475), used 84 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2804, path size before filter:94, after filter:95
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.73085::-0.28187 -> -1.01272.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.51608-> 0.21980 omega:  0.85000-> 0.79588.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.53295-> 0.42180 omega: -0.26379->-0.15243.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.61030-> 0.57168 omega: -0.56908->-0.56908.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.55143-> 0.55143 omega: -0.14655->-0.23229.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43489-> 0.44456 omega: -0.12525->-0.16167.
[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.2882, 1.8726) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3783
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.28818, 1.87262) to (-2.82, -0.62), used 19 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2112, path size before filter:49, after filter:48
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.57260::-0.98614 -> -1.55874.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.53722-> 0.48627 omega:  0.80766-> 0.80500.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.56348-> 0.56348 omega: -0.66027->-0.61090.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.61761-> 0.61761 omega: -0.85000->-0.85000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.63791-> 0.64229 omega: -0.26379->-0.26379.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.37136-> 0.33308 omega:  0.02931->-0.10455.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.11038-> 0.10752 omega:  0.08793-> 0.04800.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] the milestone[-2.8200, -0.6200] is non precise arrived, robot pose[-2.7396, -0.5611] norm: 0.0997.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageRequestLocalPlan] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [AdjustYaw].
[rp.algorithm.motion.AdjustYawAction] [INFO] onStart, required yaw : 0.050000, timeout : 60000
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[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.05000 -> -0.04883
[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":"19941304","usEpoch":"1663069550637722","usTick":"924450136"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 2, status 10, reason 
[rp.applet.slamsdp.SDPController] [WARN] grab scan is not valid continue next loop !!!
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2015 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 3 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":[[-1.470,-5.350]],"yaw":0.0}},"isCustomerLog":true,"usEpoch":"1663069924428860","usTick":"1298241275"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-1.470000, -5.350000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663069924429661","usTick":"1298242076"}
[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.470000, -5.350000)
[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":-2.741481781005859,"y":-0.5705286264419556},"moveType":"VirtualTrack","robotPose":{"x":-2.741481751988828,"y":-0.5705286056185997,"yaw":-0.04154104396287383},"to":{"x":-1.470000028610229,"y":-5.349999904632568}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663069924456097","usTick":"1298268512"}
[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 (-2.7415, -0.5705) to (-1.4700, -5.3500).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1990
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-2.74148, -0.570529) to (-1.47, -5.35), used 18 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 111, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-2.725000, -0.575000) --> (-1.470000, -5.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 = -1.470000, y = -5.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 (-2.7415, -0.5705) to (-1.4250, -3.5750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1694
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.74148, -0.570529) to (-1.425, -3.575), used 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.05432::-0.09586 -> -0.04154.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.36000-> 0.00240 omega: -0.49828->-0.01800.
[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.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.51497-> 0.20280 omega: -0.02054->-0.13256.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.49141-> 0.40560 omega: -0.15195->-0.15195.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.50196-> 0.50196 omega: -0.14655->-0.14655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.51946-> 0.52131 omega: -0.26379->-0.24567.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.62334-> 0.55986 omega: -0.85000->-0.85000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.57495-> 0.54757 omega:  0.14655-> 0.14655.
[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 (-1.3715, -2.5426) to (-1.4700, -5.3500).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1618
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.37148, -2.54265) to (-1.47, -5.35), used 54 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1690, path size before filter:54, after filter:51
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.55653::-1.20066 -> -1.75719.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.61118-> 0.58849 omega:  0.79138-> 0.49293.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.52601-> 0.52601 omega:  0.02931-> 0.02931.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.52505-> 0.52505 omega: -0.08793->-0.08793.
[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 (-1.4014, -4.1358) to (-1.4700, -5.3500).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1191
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.40137, -4.13578) to (-1.47, -5.35), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.66144::-0.99543 -> -1.65687.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:542, path size before filter:26, after filter:25
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.66865::-0.98862 -> -1.65727.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.49974-> 0.49974 omega:  0.43966-> 0.44278.
[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 (-1.4238, -4.4841) to (-1.4700, -5.3500).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:324
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.42385, -4.48414) to (-1.47, -5.35), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.48988::-0.98492 -> -1.47480.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:217, path size before filter:14, after filter:21
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.48713::-0.98267 -> -1.46980.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] start far wait milestone free.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onPreSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitDelay].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.47132-> 0.44240 omega: -0.61552->-0.61552.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.15351-> 0.15351 omega: -0.19204->-0.19204.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.07736-> 0.07736 omega:  0.14655-> 0.14655.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] precise move end!, milestone is (-1.4700, -5.3500, 0.0000), pose is (-1.4498, -5.3388, -1.9218), error is (0.0231,110.1116)
[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.80000-> 0.21935.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[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.09679
[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":"17886236","usEpoch":"1663069942315898","usTick":"1316128312"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 3, status 10, reason 
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2010 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.05000000074505806},"points":[[-2.820,-0.620]],"yaw":0.05000000074505806}},"isCustomerLog":true,"usEpoch":"1663070002764923","usTick":"1376577338"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-2.820000, -0.620000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070002765159","usTick":"1376577574"}
[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_ : (-2.820000, -0.620000)
[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":-1.460759282112122,"y":-5.272088527679443},"moveType":"VirtualTrack","robotPose":{"x":-1.460759292327413,"y":-5.272088591459828,"yaw":-0.06180770847406824},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070002790498","usTick":"1376602913"}
[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 (-1.4608, -5.2721) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:594
[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] start path finding from (-1.4608, -5.2721) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3912
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.46076, -5.27209) to (-2.82, -0.62), used 15 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 136, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.458333, -5.258333) --> (-2.820000, -0.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 = -2.820000, y = -0.620000
[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 (-1.4608, -5.2721) to (-1.4250, -2.3250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:683
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.46076, -5.27209) to (-1.425, -2.325), used 1 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 :  1.57279:: 1.51098 -> -0.06181.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.07500-> 0.00300.
[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 : 3 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.78563-> 0.78563.
[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.38282).
[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 (-1.4483, -5.3071) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:561
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.4483, -5.30705) to (-1.425, -2.375), used 3 ms.
[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] 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:552
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30693) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28289).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:557
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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] path finding main loop finished, total cycle:547
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:551
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before 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":-1.448265790939331,"y":-5.306863784790039},"moveType":"VirtualTrack","robotPose":{"x":-1.448265792703557,"y":-5.306863798714870,"yaw":1.466274017478877},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070006179708","usTick":"1379992122"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:561
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 20 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:556
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:549
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:543
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:553
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:553
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:546
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070006812489","usTick":"1380624903"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:551
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 24 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:551
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:543
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 7 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:552
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 7 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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:551
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 11 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:542
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:544
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070007485706","usTick":"1381298120"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:551
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 23 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:552
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 6 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:552
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 7 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:541
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.algorithm.motion.MoveByWayPointsAction] [WARN] onSensorData: [RunningStageUpdateLocalPath] --> [RunningStageWaitLocalPathReady] cost: 14 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:524
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:513
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:504
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2006 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 8 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:504
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070008401594","usTick":"1382214008"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:490
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 13 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:506
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 13 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:510
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 6 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.28282).
[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.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] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:508
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:476
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:476
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:448
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:449
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070009052173","usTick":"1382864588"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:451
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 24 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:462
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:476
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:489
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:485
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070009696583","usTick":"1383508997"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 32 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:503
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070010620881","usTick":"1384433295"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:501
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 40 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:498
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 10 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070011554152","usTick":"1385366566"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 18 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:498
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:498
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:498
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:501
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070012216919","usTick":"1386029334"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 25 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070012823594","usTick":"1386636008"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 10 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070013496579","usTick":"1387308993"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 13 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[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] enable PathAverageSmoothFilter
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070014175470","usTick":"1387987884"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 12 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 21 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[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:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070014791663","usTick":"1388604077"}
[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] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before finding..
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 22 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070015453601","usTick":"1389266015"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 19 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070016177933","usTick":"1389990348"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 22 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:502
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070016835188","usTick":"1390647602"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 16 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070017661574","usTick":"1391473988"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 10 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070018491550","usTick":"1392303964"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 24 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070019606945","usTick":"1393419360"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 17 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[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.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] start path finding from (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070020266441","usTick":"1394078855"}
[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] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before finding..
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 20 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070021003857","usTick":"1394816271"}
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 27 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] just use explore map : 0
[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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070021594849","usTick":"1395407263"}
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onSensorData: [RequestPathFinder] --> [WaitPathFinderResult].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before finding..
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 17 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 3 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[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] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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.28282).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3069] to milestone[-2.8200, -0.6200] is: 4.8835, 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":-1.448265790939331,"y":-5.306864261627197},"moveType":"VirtualTrack","robotPose":{"x":-1.448265746523977,"y":-5.306864406762846,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070022306285","usTick":"1396118699"}
[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] [WARN] Searching local path status: Canceled, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 0 ms, canceled before finding..
[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 (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3069) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44827, -5.30686) to (-2.82, -0.62), used 11 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 7 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.06256:: 1.53083 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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.algorithm.motion.MoveByWayPointsAction] [WARN] local planner plan failed, retry[1].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] enable PathAverageSmoothFilter
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitCostMapUpdate].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] start path finding from (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 2 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28282).
[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 (-1.4483, -5.3069) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44827, -5.30686) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070023395002","usTick":"1397207416"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 18 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.06257:: 1.53084 ->  1.46827.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 7 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.28276).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:490
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070024433930","usTick":"1398246344"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 18 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:519
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 8 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:493
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:493
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:495
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:495
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 11 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070025668434","usTick":"1399480847"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:497
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 20 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:498
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:495
[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] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 25 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.28276).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:490
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, 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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070026356663","usTick":"1400169078"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 39 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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.06257:: 1.53084 ->  1.46827.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:500
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070027187357","usTick":"1400999771"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 9 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:487
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[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] path finding main loop finished, total cycle:494
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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] [WARN] checkNearByObstacle: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath] cost: 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] onSensorData: [RunningStageBackUpIfNearByOccupied] --> [RunningStageUpdateLocalPath] cost: 16 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:489
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, 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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070028018162","usTick":"1401830577"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:490
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 28 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:492
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.06257:: 1.53084 ->  1.46827.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:496
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:493
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:489
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.23296).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[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] path finding main loop finished, total cycle:508
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:508
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 6 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:520
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:520
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:521
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070029128624","usTick":"1402941039"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:520
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 86 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:523
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 6 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:532
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:544
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:558
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:563
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:572
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, 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.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [WARN] Searching local path status: Canceled, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070029831839","usTick":"1403644254"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:574
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 23 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:575
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:575
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:575
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.LocalCostMapBuilder#0] [INFO] depth frame timeout, current: 1404067, gap: 209.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:636
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.system.HealthUpdatingService#0] [WARN] LocalCostMapHealthProvider reported health status: 0x1040A00::depth camera disconnect
[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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:570
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 6 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:574
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:544
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, 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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070030583381","usTick":"1404395796"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:550
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 18 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:542
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:533
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[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:527
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 65 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:544
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:539
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:539
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:545
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:540
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:542
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:542
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:538
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070031683909","usTick":"1405496324"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:537
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 28 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:532
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:528
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:530
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:529
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:533
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070032921210","usTick":"1406733624"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:534
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 23 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:531
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 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.06257:: 1.53084 ->  1.46827.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:531
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 25 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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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:535
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:535
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:539
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:543
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:547
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:558
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:569
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 1 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [4].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:579
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 7 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [5].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:595
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [6].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] local goal is not in the boundary of local cost map.
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder search status fail times larger than preset try times
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageFailed].
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] RunningStageFailed, action failed.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] action stopped, distance from robot[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, acceptable distance:0.5000
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageFailed] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageFailed] --> [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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070034028496","usTick":"1407840910"}
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:652
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 25 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:666
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 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.06257:: 1.53084 ->  1.46827.
[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.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:683
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), 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.28276).
[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.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[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.4483, -5.3068) to (-1.4250, -2.3750).
[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] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:828
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.28276).
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitCostMapUpdate] --> [RunningStageBackUpIfNearByOccupied].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:838
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 4 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.28276).
[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.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.4483, -5.3068) to (-1.4250, -2.3750).
[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:871
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 13 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.28276).
[rp.slamware.motionplan.algorithm.local_planner.dwa_local_planner] [WARN] cost function[goal_cost] prepare error.
[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[-1.4483,-5.3068] to milestone[-2.8200, -0.6200] is: 4.8834, 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":-1.448259234428406,"y":-5.306800842285156},"moveType":"VirtualTrack","robotPose":{"x":-1.448259206717281,"y":-5.306800842300994,"yaw":1.468272419571129},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070034687080","usTick":"1408499495"}
[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] [WARN] Searching local path status: Canceled, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 0 ms, canceled before finding..
[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 (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:899
[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] start path finding from (-1.4483, -5.3068) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3914
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-1.44826, -5.3068) to (-2.82, -0.62), used 22 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 137, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-1.425000, -5.308334) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:884
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 2 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.06257:: 1.53084 ->  1.46827.
[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 (-1.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:904
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 3 ms.
[rp.slamware.motionplan.algorithm.local_planner.MapGridCostFunction] [WARN] failed because path occupied and obstacle too close( 0.28276).
[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.4483, -5.3068) to (-1.4250, -2.3750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1151
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.44826, -5.3068) to (-1.425, -2.375), used 5 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.22345-> 0.00900 omega:  0.02931-> 0.02931.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:1246, path size before filter:83, after filter:84
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.61028:: 0.85799 ->  1.46827.
[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.52560-> 0.21280 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.58445-> 0.41500 omega:  0.38103-> 0.35191.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.55850-> 0.55850 omega:  0.00000-> 0.00000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.55598-> 0.55598 omega:  0.14655-> 0.14655.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.53280-> 0.53280 omega:  0.00000-> 0.06855.
[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 (-1.3262, -2.9829) to (-1.4250, -0.0250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1814
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.32616, -2.98294) to (-1.425, -0.025), used 31 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2348, path size before filter:78, after filter:70
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.12827:: 1.53291 ->  1.66118.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.60345-> 0.42078 omega: -0.61552->-0.33957.
[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 (-1.2954, -2.5341) to (-2.3750, -0.2750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1811
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.29543, -2.53407) to (-2.375, -0.275), used 3 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.2901, -2.5114) to (-2.3750, -0.2750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1829
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.29014, -2.51136) to (-2.375, -0.275), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.2811, -2.4838) to (-2.4250, -0.2917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:1808
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-1.28107, -2.48379) to (-2.425, -0.291667), used 9 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [3].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.06436 omega:  0.00000-> 0.00000.
[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.2761, -2.4695) to (-2.4250, -0.2917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2133
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.27608, -2.46945) to (-2.425, -0.291667), used 19 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.48411:: 2.70104 ->  1.21693.
[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.18792:: 1.40485 ->  1.21693.
[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 (-1.2730, -2.4610) to (-2.4250, -0.2917).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2260
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.27302, -2.46096) to (-2.425, -0.291667), used 15 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.54606:: 2.75800 ->  1.21194.
[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.20002:: 1.41247 ->  1.21244.
[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 (-1.2639, -2.4319) to (-2.4750, -0.3250).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2243
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.26393, -2.43195) to (-2.475, -0.325), used 18 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.44787:: 2.66651 ->  1.21864.
[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.19098:: 1.41041 ->  1.21944.
[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 (-1.2662, -2.4381) to (-2.4750, -0.3250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2312
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.26621, -2.43814) to (-2.475, -0.325), used 14 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.24695:: 2.46768 ->  1.22073.
[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.18888:: 1.40931 ->  1.22044.
[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 (-1.2672, -2.4407) to (-2.4750, -0.3250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2265
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.26716, -2.44073) to (-2.475, -0.325), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  1.37240:: 2.58904 ->  1.21664.
[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.19428:: 1.41032 ->  1.21604.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.43600-> 0.17760 omega:  0.26379-> 0.12000.
[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 (-1.2046, -2.2944) to (-2.5250, -0.3250).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2282
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-1.20462, -2.29437) to (-2.525, -0.325), used 16 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] update path yaw diff :  0.16827:: 1.43582 ->  1.26755.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2052, path size before filter:50, after filter:49
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.16771:: 1.43676 ->  1.26905.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.55517-> 0.37980 omega:  0.85000-> 0.85000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.56512-> 0.55666 omega:  0.29725-> 0.31951.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.60214-> 0.54643 omega:  0.08793-> 0.12405.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.53352-> 0.52776 omega:  0.79138-> 0.24543.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.30360-> 0.30360 omega:  0.48459-> 0.48459.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.13569-> 0.13569 omega:  0.20517-> 0.20517.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] the milestone[-2.8200, -0.6200] is non precise arrived, robot pose[-2.7277, -0.5827] norm: 0.0995.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitLocalPlanResult] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [AdjustYaw].
[rp.algorithm.motion.AdjustYawAction] [INFO] onStart, required yaw : 0.050000, timeout : 60000
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.17453-> 0.47450.
[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.05000 -> -0.04837
[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":"51765964","usEpoch":"1663070054531124","usTick":"1428343538"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 4, status 10, reason 
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2010 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":{"acceptable_precision":0.50,"flags":["precise","with_yaw"],"mode":2,"yaw":0.0},"points":[[-1.470,-5.350]],"yaw":0.0}},"isCustomerLog":true,"usEpoch":"1663070063772960","usTick":"1437585375"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-1.470000, -5.350000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070063773131","usTick":"1437585545"}
[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.470000, -5.350000)
[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":-2.742850065231323,"y":-0.5933951735496521},"moveType":"VirtualTrack","robotPose":{"x":-2.742850063079331,"y":-0.5933951455891217,"yaw":-0.03188204166597552},"to":{"x":-1.470000028610229,"y":-5.349999904632568}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070063804696","usTick":"1437617110"}
[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 (-2.7429, -0.5934) to (-1.4700, -5.3500).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:8641
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-2.74285, -0.593395) to (-1.47, -5.35), used 181 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 148, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-2.725000, -0.575000) --> (-1.470000, -5.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 = -1.470000, y = -5.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 (-2.7429, -0.5934) to (-1.4250, -3.5750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3697
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.74285, -0.593395) to (-1.425, -3.575), used 25 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  0.69891:: 0.66702 -> -0.03188.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.04050-> 0.00450.
[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 : 2 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.68553-> 0.68553.
[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.7381, -0.6140) to (-1.4250, -3.6250).
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2639
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-2.73806, -0.613987) to (-1.425, -3.625), used 13 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPlanResult] --> [RunningStageWaitLocalPathReady].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [1].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.7228, -0.5885) to (-1.4250, -3.5750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:2703
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [ERROR] Searching local path status: Failed, Basic, (-2.72284, -0.588502) to (-1.425, -3.575), used 18 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] ensure last pose of global path is milestone
[rp.algorithm.motion.MoveByWayPointsAction] [WARN] local pathfinder failed, try [2].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [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.7228, -0.5885) to (-1.4250, -3.5750).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:3585
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.72279, -0.588465) to (-1.425, -3.575), used 28 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] filter path time cost:2038, path size before filter:94, after filter:90
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff : -0.95303::-0.34257 ->  0.61046.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitRotate] --> [RunningStageUnknown].
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"code":0,"id":0,"status":-2},"isCustomerLog":true,"keyword":"end","usElapsed":"1797437","usEpoch":"1663070065570568","usTick":"1439382982"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 5, status -2, reason aborted
[rp.slamware.server.SlamwareServerAgent] [INFO] {"category":"request.move","data":{"param":{"options":{"acceptable_precision":0.50,"flags":["with_yaw"],"mode":2,"yaw":0.05000000074505806},"points":[[-2.820,-0.620]],"yaw":0.05000000074505806}},"isCustomerLog":true,"usEpoch":"1663070067619368","usTick":"1441431782"}
[rp.algorithm.motion.NavigationScheduleAction] [INFO] pushMilestone, (-2.820000, -0.620000).
[rp.algorithm.motion.NavigationScheduleAction] [INFO] {"category":"action.move","data":{"id":0,"moveType":"VirtualTrack"},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070067619870","usTick":"1441432284"}
[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_ : (-2.820000, -0.620000)
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] thresholdOfFindNearestGoal: 0.070711.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] distance of alternative near goal: 0.500000.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] {"category":"action.move.milestone","data":{"from":{"x":-2.733427762985229,"y":-0.5756823420524597},"moveType":"VirtualTrack","robotPose":{"x":-2.733427807425009,"y":-0.5756823157836501,"yaw":0.6158590689621216},"to":{"x":-2.819999933242798,"y":-0.6200000047683716}},"isCustomerLog":true,"keyword":"requestGlobalPathFind","usEpoch":"1663070067642919","usTick":"1441455334"}
[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 (-2.7334, -0.5757) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:11
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching global path status: Successed, VirtualTrack, (-2.73343, -0.575682) to (-2.82, -0.62), used 0 ms.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] global path find succeed, size: 4, mode: assign.
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] (-2.725000, -0.575000) --> (-2.820000, -0.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 = -2.820000, y = -0.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 (-2.7334, -0.5757) to (-2.8200, -0.6200).
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] path finding main loop finished, total cycle:10
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] Searching local path status: Successed, Basic, (-2.73343, -0.575682) to (-2.82, -0.62), used 0 ms.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageWaitLocalPathReady] --> [RunningStageRotateToPathDirection].
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] yaw diff :  2.99887::-2.66846 ->  0.61586.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] onSensorData: [RunningStageRotateToPathDirection] --> [RunningStageWaitRotate].
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.00450-> 0.00450.
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2009 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 7 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 : 4 ms
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega:  0.80000-> 0.80000.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] the milestone[-2.8200, -0.6200] is non precise arrived, robot pose[-2.7438, -0.6077] norm: 0.0772.
[rp.algorithm.motion.MoveByWayPointsAction] [INFO] reset: [RunningStageWaitRotate] --> [RunningStageUnknown].
[rp.algorithm.motion.MoveToMilestoneAction] [INFO] onReturnFromChild: [MoveByWayPoints] --> [AdjustYaw].
[rp.algorithm.motion.AdjustYawAction] [INFO] onStart, required yaw : 0.050000, timeout : 60000
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.80000-> 0.41000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.80000->-0.80000.
[rp.algorithm.motion.SDPMotionPlanner] [INFO] submitVelocityControl: vx:  0.00000-> 0.00000 omega: -0.80000->-0.80000.
[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.05000 ->  0.14770
[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":"5879709","usEpoch":"1663070073499579","usTick":"1447311993"}
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 6, status 10, reason 
[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: 6 ms
[rp.slamware.server.SlamwareServerAgent] [INFO] add rectangle area, usage:10, count:1
[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.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":"1663070212441377","usTick":"1586253791"}
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.715671, 3.488955, -0.309722)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.715671, 3.488955, -0.309722)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.715671, 3.488955, -0.309722)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.715671, 3.488955, -0.309722)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.715671, 3.488955, -0.309722)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":399,"y":543},"mapPose":{"x":-1.924995660781860,"y":0.6750034689903259},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070213382137","usTick":"1587194552"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.51000 y:-1.05000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.34000 y:-4.90000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.86000 y:-4.17000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.88000 y: 0.50000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.47000 y:-5.35000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.82000 y:-0.62000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 1.14000 y:-1.97000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.98000 y:-3.80000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:005c12e7-dfd2-4c38-ba59-df9358945b5b, rectangle area count:7
[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 140.
[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":"772360","usEpoch":"1663070214154498","usTick":"1587966912"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.623331, 6.254166, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.623331, 6.254166, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.623331, 6.254166, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.623331, 6.254166, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.623331, 6.254166, 0.773916)
[rp.algorithm.motion.SensorMonitor] [INFO] robot is isStationaryOrRotateStable(2008 ms).
[rp.algorithm.motion.SensorMonitor] [INFO] StageWaitStationayOrPureRotate change to StageHibernateSensor
[rp.algorithm.motion.SensorMonitor] [INFO] StageHibernateSensor change to StageCheckSensorStatus, duration: 7 ms
[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 41.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 40.
[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 38.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 43.
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:777 ms, raw area:-204,-257,282,488
[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 (4891 us).
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.623076, 6.254169, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.623076, 6.254169, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.623076, 6.254169, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.623076, 6.254169, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.623076, 6.254169, 0.773916)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":398,"y":498},"mapPose":{"x":-1.949995636940002,"y":-0.8499962091445923},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070260195647","usTick":"1634008061"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.51000 y:-1.05000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.34000 y:-4.90000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.86000 y:-4.17000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.88000 y: 0.50000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.47000 y:-5.35000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.82000 y:-0.62000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 1.14000 y:-1.97000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.98000 y:-3.80000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:95a75ee5-9f0d-48f5-b832-a21d0038f0d8, rectangle area count:7
[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 96.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 196.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 70.
[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":"932796","usEpoch":"1663070261128443","usTick":"1634940857"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[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:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 155, icpInfo.goodness = 0.720445, ICP takes: 64 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.708373,  y =  3.496324, yaw = -0.013913, takes : 0.998000 s
[rp.algorithm.slam.RelocalizationService#0] [INFO] pose : (-3.708373, 3.496324, -0.013913)
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 233, icpInfo.goodness = 0.674612, ICP takes: 49 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.718904,  y =  3.502663, yaw = -0.012700, takes : 1.297000 s
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 1303 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] both reloc success
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :1306 ms
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 7, status 10, reason 
[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 (25924 us).
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.718904, 3.502663, -0.727665)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":398,"y":498},"mapPose":{"x":-1.949995636940002,"y":-0.8499962091445923},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070262958559","usTick":"1636770973"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.51000 y:-1.05000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.34000 y:-4.90000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.86000 y:-4.17000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.88000 y: 0.50000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.47000 y:-5.35000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.82000 y:-0.62000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 1.14000 y:-1.97000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.98000 y:-3.80000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:95a75ee5-9f0d-48f5-b832-a21d0038f0d8, rectangle area count:7
[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 311.
[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":"1180761","usEpoch":"1663070264139319","usTick":"1637951734"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[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:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 166, icpInfo.goodness = 0.361697, ICP takes: 35 ms
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.server.SlamwareServerAgent] [WARN] Relocalizationing, please wait for a moment
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 238, icpInfo.goodness = 0.664439, ICP takes: 43 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.717169,  y =  3.507226, yaw = -0.012243, takes : 0.983000 s
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 987 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] only proxy reloc success
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.717169, 3.507226, -0.701494)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.717169, 3.507226, -0.701494)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.717169, 3.507226, -0.701494)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.717169, 3.507226, -0.701494)
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :988 ms
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 8, status 10, reason 
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homedock with id:f7454deb-1d07-4f98-a219-df6034d2cc9f
[rp.slamware.artifact.HomeDockManagerService#0] [INFO] set homepose to existing dock:b97b25ef-20e3-4cf4-ad0a-52bfee32a393
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.717725, 3.491471, -0.673854)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.717725, 3.491471, -0.673854)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.717725, 3.491471, -0.673854)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.717725, 3.491471, -0.673854)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.717725, 3.491471, -0.673854)
[rp.algorithm.slam.SDPSLAMBuilder] [INFO] {"category":"map.set","data":{"param":{"dimension":{"x":398,"y":498},"mapPose":{"x":-1.949995636940002,"y":-0.8499962091445923},"partially":false,"resolution":0.05000000074505806}},"isCustomerLog":true,"keyword":"begin","usEpoch":"1663070266452261","usTick":"1640264675"}
[rp.slamware.map.PoiService#0] [INFO] clear all pois
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.51000 y:-1.05000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.34000 y:-4.90000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.86000 y:-4.17000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.88000 y: 0.50000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-1.47000 y:-5.35000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x:-2.82000 y:-0.62000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 1.14000 y:-1.97000
[rp.slamware.server.SlamwareServerAgent] [INFO] set poi in position x: 0.98000 y:-3.80000
[rp.slamware.server.SlamwareServerAgent] [INFO] set rectangle area map, id:95a75ee5-9f0d-48f5-b832-a21d0038f0d8, rectangle area count:7
[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 127.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 110.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 52.
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:1344 ms, raw area:-204,-257,282,488
[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":"1117607","usEpoch":"1663070267569868","usTick":"1641382282"}
[rp.algorithm.slam.CSLAMBuilder] [INFO] setRobotPose in (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.933293, 3.348628, 0.773916)
[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:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, yaw min:-180.000000, yaw max:180.000000
[rp.algorithm.slam.MonteCarlo] [INFO] resetParticle x min:-5.933293, x max:-1.933293, y min:1.348628, y max:5.348628, 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 (33071 us).
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 176, icpInfo.goodness = 0.684975, ICP takes: 46 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.716594,  y =  3.491455, yaw = -0.013649, takes : 0.628000 s
[rp.algorithm.slam.RelocalizationService#0] [INFO] pose : (-3.716594, 3.491455, -0.013649)
[rp.algorithm.slam.RelocalizationProxy] [INFO] localPoints = 232, icpInfo.goodness = 0.670633, ICP takes: 54 ms
[rp.algorithm.slam.RelocalizationProxy] [WARN] Global relocalization Succeed, finalRobotPos x = -3.721493,  y =  3.510080, yaw = -0.012823, takes : 0.783000 s
[rp.algorithm.slam.RelocalizationProxy] [INFO] parallel reloc takes: 787 ms
[rp.algorithm.slam.RelocalizationProxy] [INFO] both reloc success
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.721493, 3.510080, -0.734688)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.721493, 3.510080, -0.734688)
[rp.algorithm.slam.AMCLProxy] [INFO] setPose (-3.721493, 3.510080, -0.734688)
[rp.algorithm.slam.AMCLProxy] [INFO] EKF is reset after set pose!
[rp.algorithm.slam.MonteCarlo] [INFO] setPose (-3.721493, 3.510080, -0.734688)
[rp.algorithm.slam.CSLAMBuilder] [INFO] processObservation takes :788 ms
[rp.algorithm.motion.ActionHistory] [INFO] Creating gravestone of action 9, status 10, reason 
[rp.applet.slamsdp.SLAMApplication] [INFO] save new private config:[
   {
      "config" : {
         "lidar" : {
            "installation_pose" : {
               "yaw" : -0.02921090522252767
            }
         }
      },
      "id" : "E580EC95C1EA9ED0B2E29FF9665A467D",
      "type" : 1
   },
   {
      "config" : {
         "base" : {
            "robot_angular_speed" : 0.850,
            "robot_line_speed" : 0.70
         },
         "mapping" : {
            "amcl" : {
               "online_amcl_icp_confidence_goodness_threshold" : 0.60,
               "online_amcl_icp_goodness_threshold" : 0.450
            }
         },
         "motion_plan" : {
            "actions" : {
               "move_by_way_points" : {
                  "enable_emergency_brake_when_action_faild" : false,
                  "forward_local_map_dis" : 1.0,
                  "local_path_update_frequency" : -1,
                  "rotate_target_radius_scale" : 2.0,
                  "rotate_to_path_angle_begin" : 0.8850,
                  "rotate_to_path_angle_end" : 0.20,
                  "rotation_angular_speed" : 0.80,
                  "wait_path_free_time_begin" : -1,
                  "wait_path_free_time_fixed" : -1,
                  "wait_path_free_time_size" : -1
               }
            },
            "angular_acceleration" : 1.50,
            "brake_deceleration" : 2.0,
            "dwa_local_planner" : {
               "goal_checkobs_dist" : 0.60,
               "inflate_radius" : 0.80,
               "motion_cost_min_trans_vel" : 0.30,
               "motion_obstacle_dist" : 2.0,
               "motion_obstacle_min_trans_vel" : 0.30,
               "occdist_scale" : 50,
               "path_scale" : 50,
               "slope_vel_lim_x" : 0.50,
               "vx_samples" : 30
            },
            "global_path_finder" : {
               "max_cost_distance" : 0.80,
               "neutral_move_cost" : 15
            },
            "linear_acceleration" : 0.20,
            "linear_deceleration" : 0.60,
            "local_costmap" : {
               "local_costmap_depth_obstacle_initial_cost" : 0
            }
         }
      },
      "id" : "steady",
      "type" : 2
   }
]

[rpos.system.config] [INFO] default_path_finder_impl_type: "global_path_finder".
[rp.applet.slamsdp.SLAMApplication] [INFO] overwriting config by private config update request, ok.
[rp.applet.slamsdp.SDPController] [INFO] local app config updated, update target config in driver.
[rp.slamware.motionplan.algorithm.ScanDataOutPusher] [INFO] scan data out pusher, strategy: none, step try ratio: 0.500000, eyeshot map resolution: 0.050000, try step: 0.025000, cost map resolution: 0.050000, occupy radius: 0.275000
[rp.algorithm.slam.ObservationMapBuilder] [INFO] on config updated, before resizeGrid, maxSenseDistanceToRobotCenter: 0.000000, area: [(-6.350000, 0.900000), (-1.550000, 5.700000)], anchor: (-3.950000, 3.300000).
[rp.algorithm.slam.ObservationMapBuilder] [INFO] on config updated, after resizeGrid, maxSenseDistanceToRobotCenter: 0.000000, area: [(-6.350000, 0.900000), (-1.550000, 5.700000)], anchor: (-3.950000, 3.300000).
[rp.applet.slamsdp.RPSLAMSDPDrv] [INFO] create depth camera, id 100, messaging.
[rp.applet.slamsdp.RPSLAMSDPDrv] [INFO] driving model: 0.
[rp.applet.slamsdp.SDPController] [INFO] call updateTargetConfig() elpased 10 ms.
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] update app config in global cost map builder, current update interval is 2000 ms.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 31.
[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 31.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 37.
[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 39.
[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 41.
[rp.slamware.motionplan.algorithm.local_planner.LocalCostMapBuilder#0] [WARN] time cost of merge rectangle cost map is 33.
[rp.slamware.motionplan.algorithm.home_detect.RpHomeDetectService#0] [INFO] update local app config.
[rp.slamware.motionplan.algorithm.path_finding.CostMapBuilder#0] [INFO] cost map rebuild time:958 ms, raw area:-204,-257,282,488
[rp.slamware.motionplan.algorithm.path_finding.GlobalPathFinderService#0] [INFO] update local app config.
