ROS+Gazebo Phase 2