dwa区域避障路径规划算法的优化

dwa区域避障路径规划算法一直在不断的优化和测试,最近又在算法中添加了距离目标的成本。在所有的成本消耗计算中,包括和添加了


航向得分 、速度得分 、障碍物距离得分,目标距离得分。程序中将所有的得分都归一化,也就是都是小于1的小数,这样累加求和比较容易。
因为这套程序要用到rovi 车中,所以对算法和避障要求非常严格。下图事模拟小车在行驶过程中突然遇到障碍物做的处理,以及直行的处理。


dwa2.jpg

dwa2.jpg 两张图中看到当障碍物的距离不同时旋转的弧度有很大的区别。

dwa2.jpg



 for (float v=dw.min_v_; v<=dw.max_v_; v+=config.v_reso){
        for (float w=dw.min_w_; w<=dw.max_w_; w+=config.yawrate_reso){

            Traj traj = calc_trajectory(x, v, w, config);//模拟计算出 轨迹来

            float to_goal_cost = calc_to_goal_cost(traj, goal, config);
            float speed_cost = config.speed_cost_gain * (config.max_speed - traj.back().v_);
            float ob_cost = calc_obstacle_cost(traj, ob, config);
			float dis_cost = calc_to_goalDist_cost(traj, goal, config);
			// by lide 增加比重
		    // 航向得分的比重、速度得分的比重、障碍物距离,目标距离得分的比重 
           
            float final_cost =  to_goal_cost +  speed_cost + ob_cost + dis_cost;
            //计算总的损耗  损耗越小path better
            if (final_cost  <= min_cost){
                min_cost = final_cost;
                min_u = Control{v, w};
                best_traj = traj;
            }
            traj_cnt ++;
        }
    }



dwa的线速度和角速度有一个动态的范围,目前经过尝试,角速度w 固定在-0.2至+0.2时能够比较好的找到路径,动态范围如果一直为正代表只能往左转,

这样的后果就是导致一直在往左转。为-0.2-0.0时只能往右转,这样也会导致一个一直往右转的圆圈。两者都不可取。在实验中得到的数据和经验。


打印执行log如下:


 sudo ./dwa_control
/dev/ttyUSB0: No such file or directory
/dev/ttyUSB0 open failed
初始状态时v只能取值0.0和0.1 w角速度可取值-0.2,-0.1,0.0,0.1.0.2,一共10次路径计算结果。
v=-0.0,w scale:-0.2,0.2
ob cost:0.200,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.864
ob cost:0.200,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.864
ob cost:0.200,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.864
ob cost:0.200,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.864
ob cost:0.200,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.864
v=0.1,w scale:-0.2,0.2
ob cost:0.240,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.760
ob cost:0.242,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.693
ob cost:0.224,goalcost:0.3,dis_cost:0.8,speed_cost:0.4 --> finanl cost:1.635
ob cost:0.242,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.674
ob cost:0.240,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.739
---> 以上10次得出最佳路径。
Time to generate:  2.7 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(10.0,1.0,1.6),90.0
(20210108 14:50:02.420)            dwa_main[ 107]<DEBUG>:  distance goal :4.0
////////////////////////////////////////////////////////////////////
v=0.0,w scale:-0.2,0.2
ob cost:0.201,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.858
ob cost:0.201,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.858
ob cost:0.201,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.858
ob cost:0.201,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.858
ob cost:0.201,goalcost:0.4,dis_cost:0.8,speed_cost:0.5
finanl cost:1.858
v=0.1,w scale:-0.2,0.2
ob cost:0.242,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.755
ob cost:0.243,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.687
ob cost:0.225,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:1.628
ob cost:0.243,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.668
ob cost:0.242,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:1.733
v=0.2,w scale:-0.2,0.2
ob cost:0.303,goalcost:0.3,dis_cost:0.8,speed_cost:0.3
finanl cost:1.683
ob cost:0.303,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:1.535
ob cost:0.243,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:1.363
ob cost:0.303,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:1.481
ob cost:0.303,goalcost:0.2,dis_cost:0.8,speed_cost:0.3
finanl cost:1.627
Time to generate:  4.7 ms
control (v,w) (0.2,0.0)
 start theta:1.6
car state:(10.0,1.1,1.6),90.0
(20210108 14:50:02.431)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
add objec in map !!!!!!!
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:4.429,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:5.928
ob cost:7.327,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:8.756
ob cost:340282346638528859811704183484516925440.000,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:7.330,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:8.738
ob cost:4.429,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:5.905
Time to generate:  1.3 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,1.7),95.8
(20210108 14:50:02.439)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:5.284,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:6.772
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:340282346638528859811704183484516925440.000,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:5.650,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:7.066
ob cost:3.789,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:5.277
Time to generate:  2.7 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,1.8),101.5
(20210108 14:50:02.447)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:6.414,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:7.891
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:340282346638528859811704183484516925440.000,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:4.669,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:6.095
ob cost:3.423,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.923
Time to generate:  2.4 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,1.9),107.2
(20210108 14:50:02.455)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:8.076,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:9.543
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:6.693,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:8.082
ob cost:3.952,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:5.388
ob cost:3.131,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.643
Time to generate:  2.5 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.0),113.0
(20210108 14:50:02.463)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:5.034,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:6.427
ob cost:3.417,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.865
ob cost:2.874,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.399
Time to generate:  2.0 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.1),118.7
(20210108 14:50:02.471)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:8.946,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:10.342
ob cost:4.118,goalcost:0.2,dis_cost:0.8,speed_cost:0.4
finanl cost:5.518
ob cost:3.072,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.532
ob cost:2.651,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.189
Time to generate:  8.4 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.2),124.4
(20210108 14:50:02.479)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:6.211,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:7.605
ob cost:3.539,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.946
ob cost:2.820,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.293
ob cost:2.513,goalcost:0.4,dis_cost:0.8,speed_cost:0.4
finanl cost:4.064
Time to generate:  8.4 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.3),130.2
(20210108 14:50:02.488)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:340282346638528859811704183484516925440.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:340282346638528859811704183484516925440.000
ob cost:4.765,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:6.159
ob cost:3.100,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.516
ob cost:2.602,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.089
ob cost:2.398,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:13.158
Time to generate:  2.9 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.4),135.9
(20210108 14:50:02.496)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:9.737,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:11.162
ob cost:3.873,goalcost:0.2,dis_cost:0.7,speed_cost:0.4
finanl cost:5.269
ob cost:2.763,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.190
ob cost:2.428,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.929
ob cost:2.291,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:13.060
Time to generate:  3.1 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.5),141.6
(20210108 14:50:02.505)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:6.779,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:8.199
ob cost:3.269,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:4.668
ob cost:2.552,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.991
ob cost:2.317,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.834
ob cost:2.191,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.969
Time to generate:  3.7 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.6),147.4
(20210108 14:50:02.515)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:5.100,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:6.517
ob cost:2.840,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.244
ob cost:2.371,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.822
ob cost:2.215,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.746
ob cost:2.136,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.923
Time to generate:  1.9 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.7),153.1
(20210108 14:50:02.522)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:4.085,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:5.500
ob cost:2.555,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.966
ob cost:2.240,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.705
ob cost:2.125,goalcost:0.3,dis_cost:10.0,speed_cost:0.4
finanl cost:12.870
ob cost:2.092,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.887
Time to generate:  0.6 ms
control (v,w) (-0.0,0.2)
 start theta:1.6
car state:(10.0,1.1,2.8),158.8
(20210108 14:50:02.529)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=-0.0,w scale:-0.2,0.2
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
ob cost:2.000,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.643
v=0.1,w scale:-0.2,0.2
ob cost:3.429,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.844
ob cost:2.330,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.749
ob cost:2.144,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.623
ob cost:2.081,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.838
ob cost:2.049,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.852
Time to generate:  1.7 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(10.0,1.2,2.8),158.8
(20210108 14:50:02.536)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=0.0,w scale:-0.2,0.2
ob cost:2.066,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.705
ob cost:2.066,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.705
ob cost:2.066,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.705
ob cost:2.066,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.705
ob cost:2.066,goalcost:0.3,dis_cost:0.8,speed_cost:0.5
finanl cost:3.705
v=0.1,w scale:-0.2,0.2
ob cost:2.929,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:4.340
ob cost:2.247,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.663
ob cost:2.141,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.617
ob cost:2.107,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.861
ob cost:2.075,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.875
v=0.2,w scale:-0.2,0.2
ob cost:2.267,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:3.429
ob cost:2.192,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:3.375
ob cost:2.123,goalcost:0.2,dis_cost:0.8,speed_cost:0.3
finanl cost:3.435
ob cost:2.066,goalcost:0.4,dis_cost:10.0,speed_cost:0.3
finanl cost:12.728
ob cost:2.066,goalcost:0.5,dis_cost:10.0,speed_cost:0.3
finanl cost:12.828
Time to generate:  1.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(9.9,1.2,2.7),156.0
(20210108 14:50:02.544)            dwa_main[ 107]<DEBUG>:  distance goal :3.8
v=0.1,w scale:-0.2,0.2
ob cost:2.432,goalcost:0.3,dis_cost:0.7,speed_cost:0.4
finanl cost:3.835
ob cost:2.204,goalcost:0.2,dis_cost:0.8,speed_cost:0.4
finanl cost:3.608
ob cost:2.169,goalcost:0.3,dis_cost:0.8,speed_cost:0.4
finanl cost:3.630
ob cost:2.164,goalcost:0.3,dis_cost:10.0,speed_cost:0.4
finanl cost:12.907
ob cost:2.164,goalcost:0.4,dis_cost:10.0,speed_cost:0.4
finanl cost:12.956
v=0.2,w scale:-0.2,0.2
ob cost:2.211,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:3.364
ob cost:2.164,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:3.330
ob cost:2.164,goalcost:0.2,dis_cost:0.8,speed_cost:0.3
finanl cost:3.454
ob cost:2.164,goalcost:0.3,dis_cost:10.0,speed_cost:0.3
finanl cost:12.808
ob cost:2.164,goalcost:0.4,dis_cost:10.0,speed_cost:0.3
finanl cost:12.914
v=0.3,w scale:-0.2,0.2
ob cost:2.164,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:3.027
ob cost:2.164,goalcost:0.0,dis_cost:0.7,speed_cost:0.2
finanl cost:3.103
ob cost:2.164,goalcost:0.1,dis_cost:0.8,speed_cost:0.2
finanl cost:3.279
ob cost:2.164,goalcost:0.3,dis_cost:10.0,speed_cost:0.2
finanl cost:12.709
ob cost:2.164,goalcost:0.5,dis_cost:10.0,speed_cost:0.2
finanl cost:12.882
Time to generate:  2.3 ms
control (v,w) (0.3,-0.2)
 start theta:1.6
car state:(9.7,1.3,2.6),150.2
(20210108 14:50:02.552)            dwa_main[ 107]<DEBUG>:  distance goal :3.7
v=0.2,w scale:-0.2,0.2
ob cost:2.203,goalcost:0.2,dis_cost:0.7,speed_cost:0.3
finanl cost:3.341
ob cost:2.203,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:3.338
ob cost:2.203,goalcost:0.2,dis_cost:0.8,speed_cost:0.3
finanl cost:3.452
ob cost:2.203,goalcost:0.3,dis_cost:10.0,speed_cost:0.3
finanl cost:12.809
ob cost:2.203,goalcost:0.4,dis_cost:10.0,speed_cost:0.3
finanl cost:12.926
v=0.3,w scale:-0.2,0.2
ob cost:2.203,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:3.041
ob cost:2.203,goalcost:0.0,dis_cost:0.7,speed_cost:0.2
finanl cost:3.103
ob cost:2.203,goalcost:0.1,dis_cost:0.8,speed_cost:0.2
finanl cost:3.264
ob cost:2.203,goalcost:0.3,dis_cost:10.0,speed_cost:0.2
finanl cost:12.691
ob cost:2.203,goalcost:0.5,dis_cost:10.0,speed_cost:0.2
finanl cost:12.885
v=0.4,w scale:-0.2,0.2
ob cost:2.203,goalcost:0.0,dis_cost:0.3,speed_cost:0.1
finanl cost:2.625
ob cost:2.203,goalcost:0.1,dis_cost:0.7,speed_cost:0.1
finanl cost:3.081
ob cost:2.203,goalcost:0.0,dis_cost:0.8,speed_cost:0.1
finanl cost:3.150
ob cost:2.203,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:12.566
ob cost:2.203,goalcost:0.6,dis_cost:10.0,speed_cost:0.1
finanl cost:12.857
Time to generate:  1.9 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.6,1.4,2.5),144.5
(20210108 14:50:02.559)            dwa_main[ 107]<DEBUG>:  distance goal :3.6
v=0.3,w scale:-0.2,0.2
ob cost:2.008,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:2.816
ob cost:2.008,goalcost:0.0,dis_cost:0.7,speed_cost:0.2
finanl cost:2.865
ob cost:2.008,goalcost:0.0,dis_cost:0.8,speed_cost:0.2
finanl cost:3.013
ob cost:2.008,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:12.431
ob cost:2.008,goalcost:0.4,dis_cost:10.0,speed_cost:0.2
finanl cost:12.644
v=0.4,w scale:-0.2,0.2
ob cost:2.008,goalcost:0.0,dis_cost:0.2,speed_cost:0.1
finanl cost:2.336
ob cost:2.008,goalcost:0.1,dis_cost:0.7,speed_cost:0.1
finanl cost:2.879
ob cost:2.395,goalcost:0.1,dis_cost:0.8,speed_cost:0.1
finanl cost:3.385
ob cost:2.008,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:12.278
ob cost:2.008,goalcost:0.5,dis_cost:10.0,speed_cost:0.1
finanl cost:12.603
v=0.5,w scale:-0.2,0.2
ob cost:2.008,goalcost:0.0,dis_cost:0.4,speed_cost:0.0
finanl cost:2.423
ob cost:2.008,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:2.929
ob cost:340282346638528859811704183484516925440.000,goalcost:0.2,dis_cost:10.0,speed_cost:0.0
finanl cost:340282346638528859811704183484516925440.000
ob cost:2.008,goalcost:0.1,dis_cost:10.0,speed_cost:0.0
finanl cost:12.105
ob cost:2.008,goalcost:0.6,dis_cost:10.0,speed_cost:0.0
finanl cost:12.576
Time to generate:  2.2 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.4,1.5,2.4),138.8
(20210108 14:50:02.567)            dwa_main[ 107]<DEBUG>:  distance goal :3.5
v=0.3,w scale:-0.2,0.2
ob cost:1.687,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:2.467
ob cost:1.687,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:2.526
ob cost:1.687,goalcost:0.0,dis_cost:0.8,speed_cost:0.2
finanl cost:2.659
ob cost:1.687,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:12.044
ob cost:1.687,goalcost:0.4,dis_cost:10.0,speed_cost:0.2
finanl cost:12.270
v=0.4,w scale:-0.2,0.2
ob cost:1.687,goalcost:0.0,dis_cost:0.2,speed_cost:0.1
finanl cost:1.966
ob cost:1.687,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:2.545
ob cost:2.230,goalcost:0.1,dis_cost:0.8,speed_cost:0.1
finanl cost:3.255
ob cost:1.687,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:11.866
ob cost:1.687,goalcost:0.4,dis_cost:10.0,speed_cost:0.1
finanl cost:12.211
v=0.5,w scale:-0.2,0.2
ob cost:1.687,goalcost:0.0,dis_cost:0.4,speed_cost:0.0
finanl cost:2.137
ob cost:1.687,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:2.599
ob cost:2.655,goalcost:0.3,dis_cost:10.0,speed_cost:0.0
finanl cost:12.931
ob cost:1.687,goalcost:0.0,dis_cost:10.0,speed_cost:0.0
finanl cost:11.712
ob cost:1.687,goalcost:0.5,dis_cost:10.0,speed_cost:0.0
finanl cost:12.164
Time to generate:  4.8 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.3,1.7,2.3),133.0
(20210108 14:50:02.574)            dwa_main[ 107]<DEBUG>:  distance goal :3.4
v=0.3,w scale:-0.2,0.2
ob cost:1.393,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:2.148
ob cost:1.393,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:2.211
ob cost:1.393,goalcost:0.0,dis_cost:0.7,speed_cost:0.2
finanl cost:2.388
ob cost:1.393,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.686
ob cost:1.393,goalcost:0.3,dis_cost:10.0,speed_cost:0.2
finanl cost:11.916
v=0.4,w scale:-0.2,0.2
ob cost:1.393,goalcost:0.0,dis_cost:0.2,speed_cost:0.1
finanl cost:1.745
ob cost:1.393,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:2.233
ob cost:1.466,goalcost:0.2,dis_cost:0.8,speed_cost:0.1
finanl cost:2.515
ob cost:1.393,goalcost:0.0,dis_cost:10.0,speed_cost:0.1
finanl cost:11.499
ob cost:1.393,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:11.836
v=0.5,w scale:-0.2,0.2
ob cost:1.393,goalcost:0.0,dis_cost:0.5,speed_cost:0.0
finanl cost:1.887
ob cost:1.393,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:2.293
ob cost:4.598,goalcost:0.3,dis_cost:10.0,speed_cost:0.0
finanl cost:14.903
ob cost:1.393,goalcost:0.1,dis_cost:10.0,speed_cost:0.0
finanl cost:11.525
ob cost:1.393,goalcost:0.4,dis_cost:10.0,speed_cost:0.0
finanl cost:11.763
Time to generate:  2.3 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.2,1.8,2.2),127.3
(20210108 14:50:02.582)            dwa_main[ 107]<DEBUG>:  distance goal :3.3
v=0.3,w scale:-0.2,0.2
ob cost:1.164,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.901
ob cost:1.164,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.952
ob cost:1.164,goalcost:0.1,dis_cost:0.7,speed_cost:0.2
finanl cost:2.174
ob cost:1.164,goalcost:0.0,dis_cost:10.0,speed_cost:0.2
finanl cost:11.396
ob cost:1.164,goalcost:0.3,dis_cost:10.0,speed_cost:0.2
finanl cost:11.623
v=0.4,w scale:-0.2,0.2
ob cost:1.164,goalcost:0.0,dis_cost:0.3,speed_cost:0.1
finanl cost:1.615
ob cost:1.164,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.982
ob cost:1.349,goalcost:0.2,dis_cost:0.8,speed_cost:0.1
finanl cost:2.414
ob cost:1.510,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:11.692
ob cost:1.164,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:11.519
v=0.5,w scale:-0.2,0.2
ob cost:1.164,goalcost:0.0,dis_cost:0.5,speed_cost:0.0
finanl cost:1.698
ob cost:1.164,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:2.050
ob cost:1.647,goalcost:0.3,dis_cost:10.0,speed_cost:0.0
finanl cost:11.971
ob cost:2.459,goalcost:0.2,dis_cost:10.0,speed_cost:0.0
finanl cost:12.678
ob cost:1.164,goalcost:0.2,dis_cost:10.0,speed_cost:0.0
finanl cost:11.414
Time to generate:  2.3 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.1,2.0,2.1),121.6
(20210108 14:50:02.590)            dwa_main[ 107]<DEBUG>:  distance goal :3.1
v=0.3,w scale:-0.2,0.2
ob cost:0.992,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.721
ob cost:0.992,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.739
ob cost:0.992,goalcost:0.1,dis_cost:0.7,speed_cost:0.2
finanl cost:2.011
ob cost:1.225,goalcost:0.0,dis_cost:10.0,speed_cost:0.2
finanl cost:11.448
ob cost:0.992,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.383
v=0.4,w scale:-0.2,0.2
ob cost:0.992,goalcost:0.0,dis_cost:0.4,speed_cost:0.1
finanl cost:1.529
ob cost:0.992,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.787
ob cost:0.992,goalcost:0.2,dis_cost:0.8,speed_cost:0.1
finanl cost:2.065
ob cost:4.927,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:15.172
ob cost:0.992,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:11.255
v=0.5,w scale:-0.2,0.2
ob cost:0.992,goalcost:0.0,dis_cost:0.6,speed_cost:0.0
finanl cost:1.557
ob cost:0.992,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:1.864
ob cost:0.992,goalcost:0.3,dis_cost:0.8,speed_cost:0.0
finanl cost:2.117
ob cost:3.918,goalcost:0.3,dis_cost:10.0,speed_cost:0.0
finanl cost:14.205
ob cost:0.992,goalcost:0.1,dis_cost:10.0,speed_cost:0.0
finanl cost:11.117
Time to generate:  2.3 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(9.0,2.2,2.0),115.8
(20210108 14:50:02.597)            dwa_main[ 107]<DEBUG>:  distance goal :3.0
v=0.3,w scale:-0.2,0.2
ob cost:0.861,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.593
ob cost:0.861,goalcost:0.0,dis_cost:0.4,speed_cost:0.2
finanl cost:1.556
ob cost:0.861,goalcost:0.1,dis_cost:0.7,speed_cost:0.2
finanl cost:1.881
ob cost:1.610,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.881
ob cost:0.861,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.186
v=0.4,w scale:-0.2,0.2
ob cost:0.861,goalcost:0.0,dis_cost:0.5,speed_cost:0.1
finanl cost:1.470
ob cost:0.861,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.637
ob cost:0.861,goalcost:0.2,dis_cost:0.7,speed_cost:0.1
finanl cost:1.935
ob cost:2.931,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:13.228
ob cost:0.957,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:11.131
v=0.5,w scale:-0.2,0.2
ob cost:0.861,goalcost:0.0,dis_cost:0.6,speed_cost:0.0
finanl cost:1.472
ob cost:0.861,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:1.721
ob cost:0.861,goalcost:0.3,dis_cost:0.8,speed_cost:0.0
finanl cost:1.983
ob cost:3.166,goalcost:0.3,dis_cost:10.0,speed_cost:0.0
finanl cost:13.503
ob cost:1.551,goalcost:0.0,dis_cost:10.0,speed_cost:0.0
finanl cost:11.557
Time to generate:  2.4 ms
control (v,w) (0.4,-0.2)
 start theta:1.6
car state:(8.9,2.4,1.9),110.1
(20210108 14:50:02.605)            dwa_main[ 107]<DEBUG>:  distance goal :2.8
v=0.3,w scale:-0.2,0.2
ob cost:0.760,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.507
ob cost:0.760,goalcost:0.0,dis_cost:0.4,speed_cost:0.2
finanl cost:1.394
ob cost:0.760,goalcost:0.1,dis_cost:0.7,speed_cost:0.2
finanl cost:1.776
ob cost:1.374,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.686
ob cost:0.847,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.108
v=0.4,w scale:-0.2,0.2
ob cost:0.760,goalcost:0.1,dis_cost:0.5,speed_cost:0.1
finanl cost:1.429
ob cost:0.760,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.524
ob cost:0.760,goalcost:0.2,dis_cost:0.7,speed_cost:0.1
finanl cost:1.830
ob cost:5.866,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:16.204
ob cost:1.598,goalcost:0.0,dis_cost:10.0,speed_cost:0.1
finanl cost:11.707
v=0.5,w scale:-0.2,0.2
ob cost:0.760,goalcost:0.0,dis_cost:0.6,speed_cost:0.0
finanl cost:1.418
ob cost:0.760,goalcost:0.2,dis_cost:0.7,speed_cost:0.0
finanl cost:1.609
ob cost:0.760,goalcost:0.3,dis_cost:0.8,speed_cost:0.0
finanl cost:1.873
ob cost:4.213,goalcost:0.4,dis_cost:10.0,speed_cost:0.0
finanl cost:14.584
ob cost:6.064,goalcost:0.1,dis_cost:10.0,speed_cost:0.0
finanl cost:16.164
Time to generate:  2.3 ms
control (v,w) (0.3,-0.1)
 start theta:1.6
car state:(8.9,2.5,1.9),107.2
(20210108 14:50:02.612)            dwa_main[ 107]<DEBUG>:  distance goal :2.7
v=0.2,w scale:-0.2,0.2
ob cost:0.696,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.662
ob cost:0.696,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.500
ob cost:0.696,goalcost:0.0,dis_cost:0.6,speed_cost:0.3
finanl cost:1.681
ob cost:0.696,goalcost:0.0,dis_cost:0.7,speed_cost:0.3
finanl cost:1.752
ob cost:0.696,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.085
v=0.3,w scale:-0.2,0.2
ob cost:0.696,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.438
ob cost:0.696,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.321
ob cost:0.696,goalcost:0.1,dis_cost:0.7,speed_cost:0.2
finanl cost:1.716
ob cost:1.135,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.470
ob cost:1.032,goalcost:0.0,dis_cost:10.0,speed_cost:0.2
finanl cost:11.256
v=0.4,w scale:-0.2,0.2
ob cost:0.696,goalcost:0.1,dis_cost:0.5,speed_cost:0.1
finanl cost:1.385
ob cost:0.696,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.473
ob cost:0.696,goalcost:0.2,dis_cost:0.7,speed_cost:0.1
finanl cost:1.770
ob cost:7.776,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:18.137
ob cost:2.573,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:12.728
Time to generate:  2.2 ms
control (v,w) (0.3,-0.1)
 start theta:1.6
car state:(8.8,2.7,1.8),104.4
(20210108 14:50:02.619)            dwa_main[ 107]<DEBUG>:  distance goal :2.6
v=0.2,w scale:-0.2,0.2
ob cost:0.643,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.584
ob cost:0.643,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.404
ob cost:0.643,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.627
ob cost:0.676,goalcost:0.0,dis_cost:0.7,speed_cost:0.3
finanl cost:1.745
ob cost:0.671,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.031
v=0.3,w scale:-0.2,0.2
ob cost:0.643,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.381
ob cost:0.643,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.268
ob cost:0.643,goalcost:0.2,dis_cost:0.7,speed_cost:0.2
finanl cost:1.664
ob cost:1.170,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.527
ob cost:1.308,goalcost:0.0,dis_cost:10.0,speed_cost:0.2
finanl cost:11.520
v=0.4,w scale:-0.2,0.2
ob cost:0.643,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.350
ob cost:0.643,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.432
ob cost:0.643,goalcost:0.2,dis_cost:0.7,speed_cost:0.1
finanl cost:1.719
ob cost:2.339,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:12.718
ob cost:4.472,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:14.671
Time to generate:  2.3 ms
control (v,w) (0.3,-0.1)
 start theta:1.6
car state:(8.8,2.8,1.8),101.5
(20210108 14:50:02.626)            dwa_main[ 107]<DEBUG>:  distance goal :2.5
v=0.2,w scale:-0.2,0.2
ob cost:0.596,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.513
ob cost:0.596,goalcost:0.0,dis_cost:0.4,speed_cost:0.3
finanl cost:1.313
ob cost:0.596,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.578
ob cost:0.650,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.731
ob cost:0.734,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:11.067
v=0.3,w scale:-0.2,0.2
ob cost:0.596,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.335
ob cost:0.596,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.233
ob cost:0.596,goalcost:0.2,dis_cost:0.7,speed_cost:0.2
finanl cost:1.620
ob cost:1.132,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.508
ob cost:1.800,goalcost:0.0,dis_cost:10.0,speed_cost:0.2
finanl cost:12.046
v=0.4,w scale:-0.2,0.2
ob cost:0.596,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.321
ob cost:0.596,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.398
ob cost:0.596,goalcost:0.3,dis_cost:0.7,speed_cost:0.1
finanl cost:1.675
ob cost:1.377,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:11.773
ob cost:8.606,goalcost:0.1,dis_cost:10.0,speed_cost:0.1
finanl cost:18.844
Time to generate:  2.2 ms
control (v,w) (0.3,-0.1)
 start theta:1.6
car state:(8.8,3.0,1.7),98.6
(20210108 14:50:02.634)            dwa_main[ 107]<DEBUG>:  distance goal :2.4
v=0.2,w scale:-0.2,0.2
ob cost:0.556,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.447
ob cost:0.556,goalcost:0.0,dis_cost:0.4,speed_cost:0.3
finanl cost:1.229
ob cost:0.556,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.535
ob cost:0.612,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.703
ob cost:0.785,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:11.091
v=0.3,w scale:-0.2,0.2
ob cost:0.556,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.298
ob cost:0.556,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.213
ob cost:0.556,goalcost:0.2,dis_cost:0.7,speed_cost:0.2
finanl cost:1.582
ob cost:1.008,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.401
ob cost:2.311,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:12.589
v=0.4,w scale:-0.2,0.2
ob cost:0.556,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.299
ob cost:0.556,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.371
ob cost:0.556,goalcost:0.3,dis_cost:0.7,speed_cost:0.1
finanl cost:1.636
ob cost:0.976,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:11.385
ob cost:2.612,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:12.886
Time to generate:  1.7 ms
control (v,w) (0.3,-0.1)
 start theta:1.6
car state:(8.8,3.1,1.7),95.8
(20210108 14:50:02.641)            dwa_main[ 107]<DEBUG>:  distance goal :2.3
v=0.2,w scale:-0.2,0.2
ob cost:0.521,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.386
ob cost:0.521,goalcost:0.0,dis_cost:0.3,speed_cost:0.3
finanl cost:1.153
ob cost:0.521,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.496
ob cost:0.601,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.700
ob cost:0.811,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:11.130
v=0.3,w scale:-0.2,0.2
ob cost:0.521,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.270
ob cost:0.521,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.204
ob cost:0.521,goalcost:0.2,dis_cost:0.7,speed_cost:0.2
finanl cost:1.549
ob cost:0.864,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.271
ob cost:2.129,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:12.435
v=0.4,w scale:-0.2,0.2
ob cost:0.521,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.280
ob cost:0.521,goalcost:0.1,dis_cost:0.6,speed_cost:0.1
finanl cost:1.348
ob cost:0.521,goalcost:0.3,dis_cost:0.7,speed_cost:0.1
finanl cost:1.603
ob cost:0.756,goalcost:0.3,dis_cost:10.0,speed_cost:0.1
finanl cost:11.176
ob cost:3.006,goalcost:0.2,dis_cost:10.0,speed_cost:0.1
finanl cost:13.312
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.7,3.2,1.6),92.9
(20210108 14:50:02.648)            dwa_main[ 107]<DEBUG>:  distance goal :2.2
v=0.1,w scale:-0.2,0.2
ob cost:0.500,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.588
ob cost:0.500,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.501
ob cost:0.500,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.512
ob cost:0.500,goalcost:0.0,dis_cost:0.7,speed_cost:0.4
finanl cost:1.575
ob cost:0.500,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.943
v=0.2,w scale:-0.2,0.2
ob cost:0.500,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.358
ob cost:0.500,goalcost:0.0,dis_cost:0.3,speed_cost:0.3
finanl cost:1.087
ob cost:0.500,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.465
ob cost:0.583,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.685
ob cost:0.801,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:11.138
v=0.3,w scale:-0.2,0.2
ob cost:0.500,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.268
ob cost:0.500,goalcost:0.1,dis_cost:0.4,speed_cost:0.2
finanl cost:1.193
ob cost:0.500,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.522
ob cost:0.748,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.163
ob cost:1.637,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.964
Time to generate:  2.3 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.7,3.3,1.6),90.0
(20210108 14:50:02.655)            dwa_main[ 107]<DEBUG>:  distance goal :2.1
v=0.1,w scale:-0.2,0.2
ob cost:0.481,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.553
ob cost:0.481,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.459
ob cost:0.481,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.471
ob cost:0.481,goalcost:0.0,dis_cost:0.7,speed_cost:0.4
finanl cost:1.545
ob cost:0.491,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.920
v=0.2,w scale:-0.2,0.2
ob cost:0.481,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.334
ob cost:0.481,goalcost:0.0,dis_cost:0.2,speed_cost:0.3
finanl cost:1.017
ob cost:0.481,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.434
ob cost:0.560,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.662
ob cost:0.772,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.125
v=0.3,w scale:-0.2,0.2
ob cost:0.481,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.268
ob cost:0.481,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.186
ob cost:0.481,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.498
ob cost:0.653,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.075
ob cost:1.415,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:11.761
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.8,3.4,1.5),87.2
(20210108 14:50:02.663)            dwa_main[ 107]<DEBUG>:  distance goal :2.0
v=0.1,w scale:-0.2,0.2
ob cost:0.464,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.518
ob cost:0.464,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.418
ob cost:0.464,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.441
ob cost:0.464,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.526
ob cost:0.488,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.904
v=0.2,w scale:-0.2,0.2
ob cost:0.464,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.312
ob cost:0.464,goalcost:0.0,dis_cost:0.2,speed_cost:0.3
finanl cost:0.943
ob cost:0.464,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.405
ob cost:0.532,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.634
ob cost:0.728,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.097
v=0.3,w scale:-0.2,0.2
ob cost:0.464,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.269
ob cost:0.464,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.182
ob cost:0.464,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.475
ob cost:0.577,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.005
ob cost:1.498,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.861
Time to generate:  2.3 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.8,3.5,1.5),84.3
(20210108 14:50:02.670)            dwa_main[ 107]<DEBUG>:  distance goal :1.9
v=0.1,w scale:-0.2,0.2
ob cost:0.448,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.485
ob cost:0.448,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.376
ob cost:0.448,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.412
ob cost:0.448,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.507
ob cost:0.481,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.885
v=0.2,w scale:-0.2,0.2
ob cost:0.448,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.294
ob cost:0.448,goalcost:0.0,dis_cost:0.1,speed_cost:0.3
finanl cost:0.873
ob cost:0.448,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.377
ob cost:0.503,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.603
ob cost:0.677,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.061
v=0.3,w scale:-0.2,0.2
ob cost:0.448,goalcost:0.1,dis_cost:0.5,speed_cost:0.2
finanl cost:1.271
ob cost:0.448,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.181
ob cost:0.448,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.454
ob cost:0.516,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.947
ob cost:1.410,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.788
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.8,3.6,1.4),81.4
(20210108 14:50:02.677)            dwa_main[ 107]<DEBUG>:  distance goal :1.9
v=0.1,w scale:-0.2,0.2
ob cost:0.434,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.453
ob cost:0.433,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.335
ob cost:0.433,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.383
ob cost:0.433,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.488
ob cost:0.471,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.879
v=0.2,w scale:-0.2,0.2
ob cost:0.433,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.279
ob cost:0.433,goalcost:0.0,dis_cost:0.1,speed_cost:0.3
finanl cost:0.839
ob cost:0.433,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.351
ob cost:0.473,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.571
ob cost:0.667,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.063
v=0.3,w scale:-0.2,0.2
ob cost:0.433,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.273
ob cost:0.433,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.181
ob cost:0.433,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.435
ob cost:0.465,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.899
ob cost:1.221,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.613
Time to generate:  2.5 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.8,3.7,1.4),78.6
(20210108 14:50:02.684)            dwa_main[ 107]<DEBUG>:  distance goal :1.8
v=0.1,w scale:-0.2,0.2
ob cost:0.425,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.425
ob cost:0.420,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.293
ob cost:0.420,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.353
ob cost:0.420,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.468
ob cost:0.458,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.877
v=0.2,w scale:-0.2,0.2
ob cost:0.420,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.267
ob cost:0.420,goalcost:0.0,dis_cost:0.1,speed_cost:0.3
finanl cost:0.863
ob cost:0.420,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.326
ob cost:0.445,goalcost:0.1,dis_cost:0.7,speed_cost:0.3
finanl cost:1.538
ob cost:0.648,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.056
v=0.3,w scale:-0.2,0.2
ob cost:0.420,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.277
ob cost:0.420,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.181
ob cost:0.420,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.418
ob cost:0.423,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.859
ob cost:1.026,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.430
Time to generate:  2.3 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.8,3.8,1.3),75.7
(20210108 14:50:02.692)            dwa_main[ 107]<DEBUG>:  distance goal :1.7
v=0.1,w scale:-0.2,0.2
ob cost:0.416,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.397
ob cost:0.408,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.249
ob cost:0.408,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.321
ob cost:0.408,goalcost:0.0,dis_cost:0.6,speed_cost:0.4
finanl cost:1.447
ob cost:0.444,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.872
v=0.2,w scale:-0.2,0.2
ob cost:0.408,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.258
ob cost:0.408,goalcost:0.0,dis_cost:0.2,speed_cost:0.3
finanl cost:0.905
ob cost:0.408,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.304
ob cost:0.418,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.507
ob cost:0.619,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.038
v=0.3,w scale:-0.2,0.2
ob cost:0.408,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.280
ob cost:0.408,goalcost:0.0,dis_cost:0.5,speed_cost:0.2
finanl cost:1.182
ob cost:0.408,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.402
ob cost:0.408,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.843
ob cost:0.865,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.278
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.9,3.9,1.3),72.8
(20210108 14:50:02.699)            dwa_main[ 107]<DEBUG>:  distance goal :1.6
v=0.1,w scale:-0.2,0.2
ob cost:0.408,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.370
ob cost:0.396,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.204
ob cost:0.396,goalcost:0.0,dis_cost:0.5,speed_cost:0.4
finanl cost:1.287
ob cost:0.396,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.426
ob cost:0.429,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.866
v=0.2,w scale:-0.2,0.2
ob cost:0.396,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.251
ob cost:0.396,goalcost:0.0,dis_cost:0.2,speed_cost:0.3
finanl cost:0.947
ob cost:0.396,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.285
ob cost:0.396,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.479
ob cost:0.585,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:11.013
v=0.3,w scale:-0.2,0.2
ob cost:0.396,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.284
ob cost:0.396,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.182
ob cost:0.396,goalcost:0.2,dis_cost:0.6,speed_cost:0.2
finanl cost:1.388
ob cost:0.396,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.831
ob cost:0.738,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.160
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.9,4.0,1.2),70.0
(20210108 14:50:02.706)            dwa_main[ 107]<DEBUG>:  distance goal :1.5
v=0.1,w scale:-0.2,0.2
ob cost:0.400,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.342
ob cost:0.386,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.157
ob cost:0.386,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.252
ob cost:0.386,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.404
ob cost:0.422,goalcost:0.0,dis_cost:10.0,speed_cost:0.4
finanl cost:10.867
v=0.2,w scale:-0.2,0.2
ob cost:0.386,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.247
ob cost:0.386,goalcost:0.0,dis_cost:0.3,speed_cost:0.3
finanl cost:0.983
ob cost:0.386,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.270
ob cost:0.386,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.463
ob cost:0.548,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.984
v=0.3,w scale:-0.2,0.2
ob cost:0.386,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.289
ob cost:0.386,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.182
ob cost:0.386,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.375
ob cost:0.386,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.819
ob cost:0.640,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:11.069
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(8.9,4.1,1.2),67.1
(20210108 14:50:02.713)            dwa_main[ 107]<DEBUG>:  distance goal :1.4
v=0.1,w scale:-0.2,0.2
ob cost:0.393,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.315
ob cost:0.376,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.107
ob cost:0.376,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.215
ob cost:0.376,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.381
ob cost:0.412,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.864
v=0.2,w scale:-0.2,0.2
ob cost:0.376,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.245
ob cost:0.376,goalcost:0.0,dis_cost:0.3,speed_cost:0.3
finanl cost:1.018
ob cost:0.376,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.258
ob cost:0.376,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.825
ob cost:0.512,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.954
v=0.3,w scale:-0.2,0.2
ob cost:0.385,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.303
ob cost:0.376,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.181
ob cost:0.376,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:10.715
ob cost:0.376,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.807
ob cost:0.563,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.997
Time to generate:  1.3 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(9.0,4.2,1.1),64.2
(20210108 14:50:02.721)            dwa_main[ 107]<DEBUG>:  distance goal :1.3
v=0.1,w scale:-0.2,0.2
ob cost:0.386,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.288
ob cost:0.367,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.054
ob cost:0.367,goalcost:0.0,dis_cost:0.4,speed_cost:0.4
finanl cost:1.176
ob cost:0.367,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.357
ob cost:0.400,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.461
v=0.2,w scale:-0.2,0.2
ob cost:0.367,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.246
ob cost:0.367,goalcost:0.0,dis_cost:0.4,speed_cost:0.3
finanl cost:1.051
ob cost:0.367,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.251
ob cost:0.367,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.432
ob cost:0.477,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.925
v=0.3,w scale:-0.2,0.2
ob cost:0.394,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.326
ob cost:0.376,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.189
ob cost:0.367,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:10.699
ob cost:0.367,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.794
ob cost:0.502,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.940
Time to generate:  2.2 ms
control (v,w) (0.2,-0.1)
 start theta:1.6
car state:(9.0,4.3,1.1),61.4
(20210108 14:50:02.728)            dwa_main[ 107]<DEBUG>:  distance goal :1.2
v=0.1,w scale:-0.2,0.2
ob cost:0.380,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.262
ob cost:0.358,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.999
ob cost:0.358,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.136
ob cost:0.358,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.333
ob cost:0.388,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.853
v=0.2,w scale:-0.2,0.2
ob cost:0.365,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.254
ob cost:0.358,goalcost:0.0,dis_cost:0.4,speed_cost:0.3
finanl cost:1.081
ob cost:0.358,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.246
ob cost:0.358,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.417
ob cost:0.445,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.898
v=0.3,w scale:-0.2,0.2
ob cost:0.404,goalcost:0.1,dis_cost:0.6,speed_cost:0.2
finanl cost:1.349
ob cost:0.394,goalcost:0.0,dis_cost:0.6,speed_cost:0.2
finanl cost:1.222
ob cost:0.358,goalcost:0.1,dis_cost:10.0,speed_cost:0.2
finanl cost:10.683
ob cost:0.358,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.781
ob cost:0.453,goalcost:0.2,dis_cost:10.0,speed_cost:0.2
finanl cost:10.893
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.0,4.3,1.0),58.5
(20210108 14:50:02.735)            dwa_main[ 107]<DEBUG>:  distance goal :1.2
v=0.0,w scale:-0.2,0.2
ob cost:0.354,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.416
ob cost:0.354,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.416
ob cost:0.354,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.416
ob cost:0.354,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.416
ob cost:0.354,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.416
v=0.1,w scale:-0.2,0.2
ob cost:0.380,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.261
ob cost:0.354,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.981
ob cost:0.354,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.103
ob cost:0.354,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.314
ob cost:0.379,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.423
v=0.2,w scale:-0.2,0.2
ob cost:0.370,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.274
ob cost:0.354,goalcost:0.0,dis_cost:0.4,speed_cost:0.3
finanl cost:1.102
ob cost:0.354,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.235
ob cost:0.354,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.404
ob cost:0.422,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.877
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.1,4.3,1.0),55.6
(20210108 14:50:02.742)            dwa_main[ 107]<DEBUG>:  distance goal :1.1
v=0.0,w scale:-0.2,0.2
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.400
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.400
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.400
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.400
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.400
v=0.1,w scale:-0.2,0.2
ob cost:0.380,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.262
ob cost:0.351,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.970
ob cost:0.351,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.069
ob cost:0.351,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.293
ob cost:0.369,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.839
v=0.2,w scale:-0.2,0.2
ob cost:0.375,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.294
ob cost:0.351,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.123
ob cost:0.351,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.226
ob cost:0.351,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.793
ob cost:0.401,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.856
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.1,4.4,0.9),52.8
(20210108 14:50:02.749)            dwa_main[ 107]<DEBUG>:  distance goal :1.1
v=0.0,w scale:-0.2,0.2
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.383
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.383
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.383
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.383
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.383
v=0.1,w scale:-0.2,0.2
ob cost:0.381,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.263
ob cost:0.347,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.966
ob cost:0.347,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.032
ob cost:0.347,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.272
ob cost:0.360,goalcost:0.1,dis_cost:0.6,speed_cost:0.4
finanl cost:1.384
v=0.2,w scale:-0.2,0.2
ob cost:0.380,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.314
ob cost:0.347,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.143
ob cost:0.347,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.219
ob cost:0.347,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.376
ob cost:0.382,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.837
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.1,4.4,0.9),49.9
(20210108 14:50:02.757)            dwa_main[ 107]<DEBUG>:  distance goal :1.0
v=0.0,w scale:-0.2,0.2
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.367
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.367
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.367
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.367
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.367
v=0.1,w scale:-0.2,0.2
ob cost:0.381,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.265
ob cost:0.344,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.971
ob cost:0.344,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.996
ob cost:0.344,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.250
ob cost:0.350,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.363
v=0.2,w scale:-0.2,0.2
ob cost:0.385,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.334
ob cost:0.344,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.163
ob cost:0.344,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.213
ob cost:0.344,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.363
ob cost:0.364,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.819
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.2,4.5,0.8),47.0
(20210108 14:50:02.764)            dwa_main[ 107]<DEBUG>:  distance goal :1.0
v=0.0,w scale:-0.2,0.2
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.350
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.350
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.350
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.350
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.350
v=0.1,w scale:-0.2,0.2
ob cost:0.382,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.269
ob cost:0.341,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.983
ob cost:0.341,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.960
ob cost:0.341,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.227
ob cost:0.341,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.341
v=0.2,w scale:-0.2,0.2
ob cost:0.391,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.353
ob cost:0.341,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.183
ob cost:0.341,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.208
ob cost:0.341,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.769
ob cost:0.348,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.802
Time to generate:  1.7 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(9.2,4.5,0.8),47.0
(20210108 14:50:02.771)            dwa_main[ 107]<DEBUG>:  distance goal :0.9
v=0.0,w scale:-0.2,0.2
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.333
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.333
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.333
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.333
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.333
v=0.1,w scale:-0.2,0.2
ob cost:0.377,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.249
ob cost:0.338,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.966
ob cost:0.338,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.967
ob cost:0.338,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.223
ob cost:0.338,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.334
v=0.2,w scale:-0.2,0.2
ob cost:0.384,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.344
ob cost:0.338,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.187
ob cost:0.338,goalcost:0.1,dis_cost:0.5,speed_cost:0.3
finanl cost:1.218
ob cost:0.338,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.767
ob cost:0.342,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.797
Time to generate:  2.4 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.2,4.5,0.8),44.2
(20210108 14:50:02.778)            dwa_main[ 107]<DEBUG>:  distance goal :0.9
v=0.0,w scale:-0.2,0.2
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.315
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.315
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.315
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.315
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.315
v=0.1,w scale:-0.2,0.2
ob cost:0.377,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.254
ob cost:0.335,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.988
ob cost:0.335,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.945
ob cost:0.335,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.201
ob cost:0.335,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.318
v=0.2,w scale:-0.2,0.2
ob cost:0.390,goalcost:0.1,dis_cost:0.6,speed_cost:0.3
finanl cost:1.362
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.205
ob cost:0.335,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.214
ob cost:0.335,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.759
ob cost:0.335,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.789
Time to generate:  2.2 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(9.3,4.6,0.8),44.2
(20210108 14:50:02.786)            dwa_main[ 107]<DEBUG>:  distance goal :0.8
v=0.0,w scale:-0.2,0.2
ob cost:0.333,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.297
ob cost:0.333,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.297
ob cost:0.333,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.297
ob cost:0.333,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.297
ob cost:0.333,goalcost:0.0,dis_cost:0.5,speed_cost:0.5
finanl cost:1.297
v=0.1,w scale:-0.2,0.2
ob cost:0.372,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.236
ob cost:0.333,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.982
ob cost:0.333,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.964
ob cost:0.333,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.199
ob cost:0.333,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.808
v=0.2,w scale:-0.2,0.2
ob cost:0.383,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.795
ob cost:0.333,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.677
ob cost:0.333,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.679
ob cost:0.333,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.757
ob cost:0.333,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.787
Time to generate:  2.2 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(9.3,4.6,0.8),44.2
(20210108 14:50:02.793)            dwa_main[ 107]<DEBUG>:  distance goal :0.8
v=0.0,w scale:-0.2,0.2
ob cost:0.330,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.278
ob cost:0.330,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.278
ob cost:0.330,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.278
ob cost:0.330,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.278
ob cost:0.330,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.278
v=0.1,w scale:-0.2,0.2
ob cost:0.367,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.219
ob cost:0.330,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.981
ob cost:0.330,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.984
ob cost:0.330,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.198
ob cost:0.330,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.304
v=0.2,w scale:-0.2,0.2
ob cost:0.377,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.787
ob cost:0.336,goalcost:0.0,dis_cost:0.5,speed_cost:0.3
finanl cost:1.218
ob cost:0.330,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.677
ob cost:0.330,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.755
ob cost:0.330,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.785
Time to generate:  2.3 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.3,4.6,0.7),41.3
(20210108 14:50:02.800)            dwa_main[ 107]<DEBUG>:  distance goal :0.7
v=0.0,w scale:-0.2,0.2
ob cost:0.327,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.259
ob cost:0.327,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.259
ob cost:0.327,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.259
ob cost:0.327,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.259
ob cost:0.327,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.259
v=0.1,w scale:-0.2,0.2
ob cost:0.368,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.226
ob cost:0.327,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.009
ob cost:0.327,goalcost:0.0,dis_cost:0.2,speed_cost:0.4
finanl cost:0.984
ob cost:0.327,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.181
ob cost:0.327,goalcost:0.1,dis_cost:0.5,speed_cost:0.4
finanl cost:1.288
v=0.2,w scale:-0.2,0.2
ob cost:0.382,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.797
ob cost:0.340,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.689
ob cost:0.327,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.667
ob cost:0.327,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.746
ob cost:0.327,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.780
Time to generate:  2.2 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(9.4,4.7,0.7),41.3
(20210108 14:50:02.807)            dwa_main[ 107]<DEBUG>:  distance goal :0.7
v=0.0,w scale:-0.2,0.2
ob cost:0.325,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.238
ob cost:0.325,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.238
ob cost:0.325,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.238
ob cost:0.325,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.238
ob cost:0.325,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.238
v=0.1,w scale:-0.2,0.2
ob cost:0.364,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.211
ob cost:0.325,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.015
ob cost:0.325,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.008
ob cost:0.325,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.183
ob cost:0.325,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.802
v=0.2,w scale:-0.2,0.2
ob cost:0.376,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.789
ob cost:0.345,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.694
ob cost:0.325,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.666
ob cost:0.325,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.744
ob cost:0.325,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.778
Time to generate:  2.3 ms
control (v,w) (0.1,0.0)
 start theta:1.6
car state:(9.4,4.7,0.7),41.3
(20210108 14:50:02.814)            dwa_main[ 107]<DEBUG>:  distance goal :0.7
v=0.0,w scale:-0.2,0.2
ob cost:0.322,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.217
ob cost:0.322,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.217
ob cost:0.322,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.217
ob cost:0.322,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.217
ob cost:0.322,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.217
v=0.1,w scale:-0.2,0.2
ob cost:0.359,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.198
ob cost:0.322,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.024
ob cost:0.322,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.030
ob cost:0.322,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.186
ob cost:0.322,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.800
v=0.2,w scale:-0.2,0.2
ob cost:0.370,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.781
ob cost:0.351,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.698
ob cost:0.325,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.667
ob cost:0.322,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.742
ob cost:0.322,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.776
Time to generate:  2.2 ms
control (v,w) (0.1,-0.1)
 start theta:1.6
car state:(9.5,4.7,0.7),38.5
(20210108 14:50:02.822)            dwa_main[ 107]<DEBUG>:  distance goal :0.6
v=0.0,w scale:-0.2,0.2
ob cost:0.320,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.195
ob cost:0.320,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.195
ob cost:0.320,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.195
ob cost:0.320,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.195
ob cost:0.320,goalcost:0.0,dis_cost:0.4,speed_cost:0.5
finanl cost:1.195
v=0.1,w scale:-0.2,0.2
ob cost:0.360,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.207
ob cost:0.320,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.050
ob cost:0.320,goalcost:0.0,dis_cost:0.3,speed_cost:0.4
finanl cost:1.039
ob cost:0.320,goalcost:0.1,dis_cost:0.4,speed_cost:0.4
finanl cost:1.175
ob cost:0.320,goalcost:0.1,dis_cost:10.0,speed_cost:0.4
finanl cost:10.797
v=0.2,w scale:-0.2,0.2
ob cost:0.375,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.791
ob cost:0.355,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.709
ob cost:0.336,goalcost:0.0,dis_cost:10.0,speed_cost:0.3
finanl cost:10.671
ob cost:0.320,goalcost:0.1,dis_cost:10.0,speed_cost:0.3
finanl cost:10.734
ob cost:0.320,goalcost:0.2,dis_cost:10.0,speed_cost:0.3
finanl cost:10.770
Time to generate:  2.2 ms
control (v,w) (0.1,0.0)





sitemap