As I mentioned previously, one of the main issues I am currently stuck with is that after sending a goal to a client there are several problems of monitoring it. In the beginning I have tried method wait_for_result but the problem is that this method pauses whole MyAlgorithm thread and everything got stuck. The second idea was to implement separate thread that can keep track of goals. I created a separate threadGoalSender class which takes main functionality and is similar to ThreadPublisher, which in its own thread continuously sends/publishes goals/messages. It works fine with ROS messages, but as I found out it doesn’t work with Actionlib client instance which sends goals to move_base package. As I understood, each time it receives new goal it starts to re-build it path and calculate costmaps, and this is why jerky motion is taking place. This behaviour is shown in the following video:
For now, I removed this separate thread and will try to solve the problem without it.