Added a ros_p3d plugin as the ground truth to the simulator model.

darwin_description/urdf/darwin.gazebo

@@ -363,4 +363,15 @@
     </plugin>
   </gazebo>
 
+  <gazebo>
+    <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
+      <topicName>/darwin/ground_truth</topicName>
+      <frameName>world</frameName>
+      <bodyName>base_link</bodyName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>20.0</updateRate>
+    </plugin>
+  </gazebo> 
+
 </robot>