How to modify the code to make it for the differential robot #5
Description
I modify some code to make it for the differential robot, but it has some error by bounds. How to modify the code ?
modify codes
`
cv::Mat img_src = cv::imread(image_dir, CV_8UC1);
double resolution = 0.05;//0.2; // in meter
DEFINE_double(car_width, 0.46, "");
DEFINE_double(car_length, 0.56, "");
DEFINE_double(safety_margin, 0.1, "mandatory safety margin");
DEFINE_double(wheel_base, 0.4, "wheel base");
DEFINE_double(rear_length, -0.26, "rear axle to rear edge");
DEFINE_double(front_length, 0.26, "rear axle to front edge");
DEFINE_double(max_steering_angle, 100.0 * M_PI / 180.0, "");
std::vector ReferencePathImpl::getClearanceWithDirectionStrict(const PathOptimizationNS::State &state,
const PathOptimizationNS::Map &map) {
// TODO: too much repeated code!
double delta_s = 0.05;//0.3;
static auto search_radius = 0.3;//0.5;
`
The prints with error:
I0115 10:58:31.684042 12794 demo2.cpp:225] reference_rcv:1 start_state_rcv:1 end_state_rcv:1 I0115 10:58:31.684828 12794 demo2.cpp:229] start PathOptimizer, end_state_rcv:1 I0115 10:58:31.685194 12794 path_optimizer.cpp:46] smoothing_method:TENSION2 reference_points_size:7 I0115 10:58:31.685456 12794 reference_path_smoother.cpp:21] type:TENSION2 I0115 10:58:31.685575 12794 path_optimizer.cpp:51] reference_path_smoother->solve I0115 10:58:31.685719 12794 reference_path_smoother.cpp:34] ReferencePathSmoother::solve: I0115 10:58:31.685817 12794 reference_path_smoother.cpp:39] bSpline I0115 10:58:31.685937 12794 reference_path_smoother.cpp:505] BSpline_Control_points_size:7 I0115 10:58:31.686065 12794 reference_path_smoother.cpp:512] BSpline length:7.61634 delta_t:0.131297 I0115 10:58:31.686211 12794 reference_path_smoother.cpp:524] BSpline x_list_size:9 I0115 10:58:31.686314 12794 reference_path_smoother.cpp:41] smooth I0115 10:58:31.686408 12794 tension_smoother.cpp:21] TensionSmoother::smooth I0115 10:58:31.686563 12794 tension_smoother_2.cpp:28] TensionSmoother2::osqpSmooth I0115 10:58:31.686988 12794 reference_path_smoother.cpp:43] graphSearchDp I0115 10:58:31.687775 12794 reference_path_smoother.cpp:45] postSmooth I0115 10:58:31.691109 12794 base_solver.cpp:38] Ref length 7.63333, precise_planning_size_ 26 I0115 10:58:31.694758 12794 time_recorder.cpp:20] ========Printing time for Set Cost======== I0115 10:58:31.694934 12794 time_recorder.cpp:22] set heassian cost 0.015 ms. I0115 10:58:31.695097 12794 time_recorder.cpp:22] return matrix cost 0.042 ms. I0115 10:58:31.695196 12794 time_recorder.cpp:25] Total time cost: 0.057 ms. I0115 10:58:31.695502 12794 time_recorder.cpp:27] ========End printing time for Set Cost======== I0115 10:58:31.695819 12794 base_solver.cpp:227] kappa limit -14.1782 ERROR in validate_data: Lower bound at index 78 is greater than upper bound: 1.4178e+01 > -1.4178e+01 ERROR in osqp_setup: Problem data validation. [OsqpEigen::Solver::initSolver] Unable to setup the workspace. E0115 10:58:31.697428 12794 path_optimizer.cpp:148] Pre solving failed! I0115 10:58:31.697907 12794 reference_path_impl.cpp:92] front: (10.9506, 4.97484), bound: (-1.47, 1.77). I0115 10:58:31.698422 12794 reference_path_impl.cpp:93] rear: (11.4602, 4.87131), bound: (-1.57, 1.67). I0115 10:58:31.698604 12794 reference_path_impl.cpp:92] front: (10.6566, 5.03458), bound: (-1.42002, 1.76998). I0115 10:58:31.698957 12794 reference_path_impl.cpp:93] rear: (11.1662, 4.93103), bound: (-1.47001, 1.71999). I0115 10:58:31.699108 12794 reference_path_impl.cpp:92] front: (10.3627, 5.0944), bound: (-1.37003, 1.86997). I0115 10:58:31.699229 12794 reference_path_impl.cpp:93] rear: (10.8722, 4.99074), bound: (-1.47003, 1.76997). I0115 10:58:31.699333 12794 reference_path_impl.cpp:92] front: (10.0687, 5.15434), bound: (-1.32005, 1.91995). I0115 10:58:31.699493 12794 reference_path_impl.cpp:93] rear: (10.5782, 5.05051), bound: (-1.42004, 1.81996). I0115 10:58:31.699709 12794 reference_path_impl.cpp:92] front: (9.7748, 5.21442), bound: (-1.27006, 1.96994). I0115 10:58:31.699839 12794 reference_path_impl.cpp:93] rear: (10.2843, 5.11036), bound: (-1.32006, 1.86994). I0115 10:58:31.700063 12794 reference_path_impl.cpp:92] front: (9.4809, 5.2747), bound: (-1.22005, 2.01995). I0115 10:58:31.700232 12794 reference_path_impl.cpp:93] rear: (9.99032, 5.17033), bound: (-1.32007, 1.91993). I0115 10:58:31.700367 12794 reference_path_impl.cpp:92] front: (9.18702, 5.33505), bound: (-1.16995, 2.02005). I0115 10:58:31.700490 12794 reference_path_impl.cpp:93] rear: (9.69641, 5.23053), bound: (-1.27001, 2.01999). I0115 10:58:31.700618 12794 reference_path_impl.cpp:92] front: (8.89308, 5.39513), bound: (-1.11985, 2.07015). I0115 10:58:31.700744 12794 reference_path_impl.cpp:93] rear: (9.40253, 5.29093), bound: (-1.21991, 2.07009). I0115 10:58:31.701093 12794 reference_path_impl.cpp:92] front: (8.59903, 5.45464), bound: (-1.06974, 3.82026). I0115 10:58:31.701514 12794 reference_path_impl.cpp:93] rear: (9.10865, 5.35123), bound: (-1.1698, 2.0202). I0115 10:58:31.701682 12794 reference_path_impl.cpp:92] front: (8.30483, 5.51333), bound: (-1.01964, 2.12036). I0115 10:58:31.701802 12794 reference_path_impl.cpp:93] rear: (8.8147, 5.41118), bound: (-1.1197, 2.0703). I0115 10:58:31.701912 12794 reference_path_impl.cpp:92] front: (8.01044, 5.57092), bound: (-1.01956, 1.72044). I0115 10:58:31.702193 12794 reference_path_impl.cpp:93] rear: (8.52065, 5.4705), bound: (-1.0696, 3.5704). I0115 10:58:31.710667 12794 reference_path_impl.cpp:92] front: (7.71583, 5.62729), bound: (-0.969609, 1.77039). I0115 10:58:31.710932 12794 reference_path_impl.cpp:93] rear: (8.22642, 5.52883), bound: (-1.01958, 1.87042). I0115 10:58:31.712373 12794 reference_path_impl.cpp:92] front: (7.42104, 5.68272), bound: (-0.919668, 4.47033). I0115 10:58:31.712960 12794 reference_path_impl.cpp:93] rear: (7.93196, 5.58598), bound: (-1.01963, 1.72037). I0115 10:58:31.713850 12794 reference_path_impl.cpp:92] front: (7.12609, 5.73737), bound: (-0.869726, 2.97027). I0115 10:58:31.714452 12794 reference_path_impl.cpp:93] rear: (7.63728, 5.64208), bound: (-0.969692, 1.82031). I0115 10:58:31.714937 12794 reference_path_impl.cpp:92] front: (6.83098, 5.7914), bound: (-0.819784, 2.77022). I0115 10:58:31.715240 12794 reference_path_impl.cpp:93] rear: (7.34239, 5.6973), bound: (-0.86975, 4.17025). I0115 10:58:31.715425 12794 reference_path_impl.cpp:92] front: (6.53572, 5.84497), bound: (-0.7698, 2.6702). I0115 10:58:31.715656 12794 reference_path_impl.cpp:93] rear: (7.0473, 5.75179), bound: (-0.819809, 2.92019). I0115 10:58:31.715914 12794 reference_path_impl.cpp:92] front: (6.24026, 5.89785), bound: (-0.719395, 2.5706). I0115 10:58:31.716126 12794 reference_path_impl.cpp:93] rear: (6.75207, 5.80591), bound: (-0.819655, 2.77035). I0115 10:58:31.716316 12794 reference_path_impl.cpp:92] front: (5.94439, 5.94852), bound: (-0.668943, 2.52106). I0115 10:58:31.716488 12794 reference_path_impl.cpp:93] rear: (6.45675, 5.85976), bound: (-0.769204, 2.6208). I0115 10:58:31.716624 12794 reference_path_impl.cpp:92] front: (5.64798, 5.99571), bound: (-0.66849, 2.47151). I0115 10:58:31.716778 12794 reference_path_impl.cpp:93] rear: (6.16123, 5.91219), bound: (-0.718752, 2.57125). I0115 10:58:31.716984 12794 reference_path_impl.cpp:92] front: (5.35097, 6.03822), bound: (-0.618036, 2.42196). I0115 10:58:31.717136 12794 reference_path_impl.cpp:93] rear: (5.86536, 5.96203), bound: (-0.718298, 2.5217). I0115 10:58:31.717253 12794 reference_path_impl.cpp:92] front: (5.05328, 6.07484), bound: (-0.617594, 2.57241). I0115 10:58:31.717398 12794 reference_path_impl.cpp:93] rear: (5.56897, 6.00806), bound: (-0.667844, 2.47216). I0115 10:58:31.717746 12794 reference_path_impl.cpp:92] front: (4.75486, 6.10462), bound: (-0.617787, 2.72221). I0115 10:58:31.718593 12794 reference_path_impl.cpp:93] rear: (5.27187, 6.04893), bound: (-0.617566, 2.42243). I0115 10:58:31.718951 12794 reference_path_impl.cpp:92] front: (4.45592, 6.12895), bound: (-0.618216, 2.77178). I0115 10:58:31.719151 12794 reference_path_impl.cpp:93] rear: (4.97389, 6.08302), bound: (-0.567968, 2.52203). I0115 10:58:31.719308 12794 reference_path_impl.cpp:92] front: (4.15658, 6.14922), bound: (-0.568644, 2.72136). I0115 10:58:31.719470 12794 reference_path_impl.cpp:93] rear: (4.67518, 6.11107), bound: (-0.618397, 2.7216). I0115 10:58:31.719640 12794 reference_path_impl.cpp:92] front: (3.85695, 6.16654), bound: (-0.56907, 2.72093). I0115 10:58:31.719806 12794 reference_path_impl.cpp:93] rear: (4.37595, 6.13423), bound: (-0.568824, 2.77118). I0115 10:58:31.719985 12794 reference_path_impl.cpp:92] front: (3.55716, 6.18204), bound: (-0.569495, 2.7705). I0115 10:58:31.720088 12794 reference_path_impl.cpp:93] rear: (4.07638, 6.1536), bound: (-0.56925, 2.72075). E0115 10:58:31.720198 12794 path_optimizer.cpp:69] Path optimization FAILED! E0115 10:58:31.735548 12794 path_optimizer.cpp:70] ==========================optimizePath======================== I0115 10:58:31.737421 12794 demo2.cpp:247] start PathOptimizer, opt_ok:0 I0115 10:58:32.253443 12794 demo2.cpp:225] reference_rcv:1 start_state_rcv:1 end_state_rcv:1