Plugin for Publishing Robot Pose to ROS from Gazebo

This morning, I wanted to have a Gazebo plugin that publishes the robot's pose in Gazebo.  Right now, I've got a python script that prints to a text file, but it would be easier for analysis to just have all the data in the same bag file. 


It took an hour, but now I've got the robot's pose published as a ROS topic.

******
NOTE! The published pose messages from this new way match the Gazebo print statements of the get_model_state call on the mobile_base in the world frame to 0.01 meters.
******


Here's how I did it:
https://answers.ros.org/question/222033/how-do-i-publish-gazebo-position-of-a-robot-model-on-odometry-topic/

Recommends something like this should be added to the URDF (robot specification)

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<rpyOffsets>0 0 0</rpyOffsets>  <xyzOffsets>0 0 0</xyzOffsets>  <frameName>world</frameName>  <gaussianNoise>0.01</gaussianNoise>  <topicName>gazebo_robot_pose</topicName>  <bodyName>base_link</bodyName>  <updateRate>50.0</updateRate>  <alwaysOn>true</alwaysOn>
</plugin>


Notes:

The difficult part was finding which file to use.  Here's what I did:
1. I went to the turtlebot_gazebo package and looked at the turtlebot_world.launch file to figure out where the turtlebot URDF files are.

2. Turns out, there's a turtlebot_description package with relevant files.  I opened turtlebot_description\urdf and found a file called turtlebot_gazebo.urdf.xacro

3. I added a new <gazebo> tag and put the plugin section within the new <gazebo> section.

**Originally, I used "mobile_base" for the <bodyName>, but I got an error that mobile_base wasn't a valid bodyName value. I switched back to using base_link, and the results are working fine.



This was a helpful example of where to put the plugin code - note that the plugin is within its own <gazebo> tag in the xacro:
https://github.com/tu-darmstadt-ros-pkg/taurob_tracker_common/blob/master/taurob_tracker_description/urdf/tracker_chassis.gazebo.xacro.xml#L44


The source of that .so file is described here:

##what is libgazebo_ros_p3d.so? You can find source code from gazebo_ros_pkgs repository.
Go to gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h.

/*
BriefNote:
Compute relative position of [bodyName] frame with respect to [frameName] frame.
And publish it as [topicName] with [updateRate].
It seems that it does not have physical control of the model.
*/

https://github.com/jaejunlee0538/ua_ros_p3dx