This class takes in published poses and writes them to file. More...
#include <Recorder.h>
Public Member Functions | |
| void | callback_odometry (const nav_msgs::OdometryPtr &msg) |
| Callback for nav_msgs::Odometry message types. More... | |
| void | callback_pose (const geometry_msgs::PoseStampedPtr &msg) |
| Callback for geometry_msgs::PoseStamped message types. More... | |
| void | callback_posecovariance (const geometry_msgs::PoseWithCovarianceStampedPtr &msg) |
| Callback for geometry_msgs::PoseWithCovarianceStamped message types. More... | |
| void | callback_transform (const geometry_msgs::TransformStampedPtr &msg) |
| Callback for geometry_msgs::TransformStamped message types. More... | |
| Recorder (std::string filename) | |
| Default constructor that will open the specified file on disk. If the output directory does not exists this will also create the directory path to this file. More... | |
Protected Member Functions | |
| void | write () |
| This is the main write function that will save to disk. This should be called after we have saved the desired pose to our class variables. More... | |
Protected Attributes | |
| Eigen::Matrix< double, 3, 3 > | cov_pos |
| Eigen::Matrix< double, 3, 3 > | cov_rot |
| bool | has_covariance = false |
| std::ofstream | outfile |
| Eigen::Vector3d | p_IinG |
| Eigen::Vector4d | q_ItoG |
| double | timestamp |
This class takes in published poses and writes them to file.
Original code is based on this modified posemsg_to_file. Output is in a text file that is space deliminated and can be read by all scripts. If we have a covariance then we also save the upper triangular part to file so we can calculate NEES values.
Definition at line 47 of file Recorder.h.
|
inline |
Default constructor that will open the specified file on disk. If the output directory does not exists this will also create the directory path to this file.
| filename | Desired file we want to "record" into |
Definition at line 55 of file Recorder.h.
|
inline |
Callback for nav_msgs::Odometry message types.
Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis). http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
| msg | New message |
Definition at line 92 of file Recorder.h.
|
inline |
Callback for geometry_msgs::PoseStamped message types.
| msg | New message |
Definition at line 110 of file Recorder.h.
|
inline |
Callback for geometry_msgs::PoseWithCovarianceStamped message types.
Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis). http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
| msg | New message |
Definition at line 125 of file Recorder.h.
|
inline |
Callback for geometry_msgs::TransformStamped message types.
| msg | New message |
Definition at line 143 of file Recorder.h.
|
inlineprotected |
This is the main write function that will save to disk. This should be called after we have saved the desired pose to our class variables.
Definition at line 155 of file Recorder.h.
|
protected |
Definition at line 187 of file Recorder.h.
|
protected |
Definition at line 186 of file Recorder.h.
|
protected |
Definition at line 182 of file Recorder.h.
|
protected |
Definition at line 179 of file Recorder.h.
|
protected |
Definition at line 185 of file Recorder.h.
|
protected |
Definition at line 184 of file Recorder.h.
|
protected |
Definition at line 183 of file Recorder.h.