Added function to convert PCL point to geometry_msgs point