Fix the segmentation fault at the PathPlanning and KMonitor

Signed-off-by: eldrad <nikofinas@gmail.com>
Remove a warning

Signed-off-by: eldrad <nikofinas@gmail.com>