Minor changes

- Added wait loop until connected to ROS
- Getting dimensions from parameter server