This master thesis is aimed at developing a ROS compatible obstacle avoidance module for a Segway RMP400 platform using lidar sensors. The algorithm implemented supports different 3D scenarios, such as slopes and precipices. Given that the currently available navigation modules in the ROS stack work only for 2D, significant modification of the navigation stack was mandatory. The proposed solution does not take into account the non-holonomic constraints of the platform. An alternative method is developed to tackle this issue for the case of tele operated motion.