dwa区域避障路径规划算法的优化
dwa区域避障路径规划算法一直在不断的优化和测试，最近又在算法中添加了距离目标的成本。在所有的成本消耗计算中，包括和添加了
航向得分 、速度得分 、障碍物距离得分，目标距离得分。程序中将所有的得分都归一化，也就是都是小于1的小数，这样累加求和比较容易。
因为这套程序要用到rovi 车中，所以对算法和避障要求非常严格。下图事模拟小车在行驶过程中突然遇到障碍物做的处理，以及直行的处理。
两张图中看到当障碍物的距离不同时旋转的弧度有很大的区别。
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)
