Revision history [back]

Question number one has a few answers:

If your measurement error is constant, you can define it using the ros paramater server in your launchfile for your robot. Then read the error paramater from the parameter server and insert it into the orientation covariance matrix in a geometry_msgs/PoseWithCovariance. You probably want to include that message as part of a nav_msgs/Odometry message.

In the following example, the parameter z_rotation_covariance is defined in the launchfile:

<launch>
  <param name="z_rotation_covariance" value="0.05" />
</launch>

To use the parameter value in code, you'll want to use the rosparam api for your client.

If your error is a calculated or accumulated error, you will want to set up a service to calculate the changes as a function on some input and store the error in rosbag and another service to retrieve the error from rosbag whenever you need to use it in the orientation covariance matrix in a geometry_msgs/PoseWithCovariance. You probably want to include that message as part of a nav_msgs/Odometry message.

You will need to subscribe to whatever topics are publishing the parameters of your error estimation function, probably using a message_filter to combine the messages of several different topics into a single callback. ApproximateTimeSynchronizer is a good filter for doing this. Then when your filter callback is called, call your error calculation/accumulation service to calculate and store your error, then you can retrieve it from rosbag at any time using your error retrieval service.

Question number two is simpler:

You will want to model your robot using urdf. URDF joint tags allow you to specify joint upper and lower limits, efforts, types, etc. You might look into using Gazebo instead of Stage. It allows you to add several additional tags to your urdf to model the physics properties of your robot during simulation.