lijiangnanbit/path_optimizer_2

Optimization-based real-time path planning for vehicles.

path_optimizer_2's Issues

fix bugs for Ubuntu 20.04 and ROS noetic

demonode.cpp around line 135

In ROS noetic , It seems that a "mark_frame_id" can't be named begin with '/'

In reference_path_impl.cpp , In function ' buildReferenceFromStates ' around the tenth line from the bottom

" return true " is forgotten by mistake, which lead to a mistake like :
" double free or corruption (!prev) " or " free(): double free detected in tcache 2 "

Interestingly, in Ubuntu 16.04 or older this function seems to return true by default.

参考论文和资料

大佬，对您的工作表示敬意。
您方便给出代码中平滑部分和轨迹优化部分的变量、约束和代价的资料嘛，从代码反向推导有点费事。

How to modify the code to make it for the differential robot

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

The result path may hit the obs

The result path may hit the obs.

test map file
test map file narrow_test-nav.zip

This for ROS2 Humble

https://github.com/GPrathap/path_optimizer_2.git

The equality constraints defined in reference splince smoothing

I'm not sure if the equality constraints defined in line 126 and 127 are correct. Can you please take a look and let me know?

path_optimizer_2/src/reference_path_smoother/tension_smoother_2.cpp

Line 126 in 39948ac

cons(cons_x_update_start_index + i, theta_start_index + i) = ds * sin(angle_list[i]);

I guess, those should be

   cons(cons_x_update_start_index + i, theta_start_index + i) = - ds * cos(angle_list[i]);
    cons(cons_y_update_start_index + i, theta_start_index + i) = -ds * sin(angle_list[i]);

setConstraints Function

The setConstraints Function seems extended by the paper 'Trajectory Planning Under Vehicle Dimension Constraints
Using Sequential Linear Programming', can you explain the part about extensions, or list more related materials? Thank you.

Reverse supported

Thanks for your package. I have a question: How can this package support REVERSE running? that is the start state heading is > 90° with the reference points line angle?

how to fixed the target pose

how to fixed the target pose, make the smooth path align with the target