controller frequency in navigation stack

edit

Hy,

I'm having problems with navigation of my robot. I am using ROS navigation stack. The thing is my robot is driving like a snake around calculated global plan (it's like it can't drive straight). I think that my encoder odometry data and transforms are fine. I tried changing the parameters for velocity, acceleration and others but I got stuck. The oscillations around the global plan trajectory are smaller but they are still there. I think that increasing my controller loop frequency could help me but for some reason I can't speed it up.

I am using rpi2 and my laptop which is synchronized with the ros master on my rpi2. On rpi2 I have nodes for laser, odometry and node that publishes frame transforms (and ros master). On my laptop I'm running nodes for amcl, map_server, move_base package and rviz.

The default rate for the control loop frequency is set to 20Hz, but it keeps sending me warnings that it missed it's desired rate. The real control loop frequency is close to 4Hz (but I would really like to make it higher). When I look at my CPU usage it is less then 60% on both rpi2 and laptop so I wonder what is the problem and is it possible to speed it up.

Thanks in advance