with new code and old code, it occurs many loop warning like below....
please check the screenshot below, thanks!

And after check code I think I find out a temporary solution, maybe it is also the root cause:
in costmap_2d_ros.cpp file:
ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
at line 409,
r.sleep();
if (r.cycleTime() > ros::Duration(1 / frequency)) , the loop time must have some issues, maybe not correct.

I test the map loop time in function, most of them is actually very small, and can satisfy our needs.(based on small map 10*10m, not test large map.)
if we comment the r.sleep();
the output is very good, and almost no loop warning output. so the cycleTime's calculation must be not correct.
if no wrong, why we comment the sleep(), the function act good and loop warning disapper?

and with the same method, we can remove the controller loop warning in the same way.
it acts good finally.

could you please check above bug? thanks!