Publish organized pointcloud to filtered_cloud topic

Add two parameter for sensor:
  * fitlered_cloud_use_color: If this parameter is set to True, filtered_cloud will have rgb field.
  * filtered_cloud_keep_organized: If this parameter is set to True, filtered_cloud will be a organized pointcloud. which keeps structure of depth image.

In order to implement it, several methods are updated to template method to take into account point type.

* non filtered pointcloud

![a](https://cloud.githubusercontent.com/assets/40454/4870875/154dcf32-618b-11e4-9768-7c8a0cd4d2e4.png)
* filtered pointcloud (organized and colored)

![b](https://cloud.githubusercontent.com/assets/40454/4870876/170500a2-618b-11e4-9ea5-7cf701277234.png)


based on 
* https://github.com/ros-planning/moveit_ros/pull/518 
* https://github.com/ros-planning/moveit_ros/pull/517