OKVIS ROS
|
This class handles the publishing to either ROS topics or files. More...
#include <Publisher.hpp>
Public Member Functions | |
Publisher () | |
Default constructor. More... | |
~Publisher () | |
Publisher (ros::NodeHandle &nh) | |
Constructor. Calls setNodeHandle(). More... | |
Setters | |
void | setNodeHandle (ros::NodeHandle &nh) |
Set the node handle and advertise topics. More... | |
bool | setCsvFile (std::fstream &csvFile) |
Set an odometry output CSV file. More... | |
bool | setCsvFile (std::string &csvFileName) |
Set an odometry output CSV file. More... | |
bool | setCsvFile (std::string csvFileName) |
Set an odometry output CSV file. More... | |
bool | setLandmarksCsvFile (std::fstream &csvFile) |
Set a CVS file where the landmarks will be saved to. More... | |
bool | setLandmarksCsvFile (std::string &csvFileName) |
Set a CVS file where the landmarks will be saved to. More... | |
bool | setLandmarksCsvFile (std::string csvFileName) |
Set a CVS file where the landmarks will be saved to. More... | |
void | setPose (const okvis::kinematics::Transformation &T_WS) |
Set the pose message that is published next. More... | |
void | setOdometry (const okvis::kinematics::Transformation &T_WS, const okvis::SpeedAndBiases &speedAndBiases, const Eigen::Vector3d &omega_S) |
Set the odometry message that is published next. More... | |
void | setParameters (const okvis::VioParameters ¶meters) |
Set the parameters. More... | |
void | setPoints (const okvis::MapPointVector &pointsMatched, const okvis::MapPointVector &pointsUnmatched, const okvis::MapPointVector &pointsTransferred) |
Set the points that are published next. More... | |
void | setTime (const okvis::Time &t) |
Set the time for the next message to be published. More... | |
void | setImages (const std::vector< cv::Mat > &images) |
Set the images to be published next. More... | |
void | setPath (const okvis::kinematics::Transformation &T_WS) |
Add a pose to the path that is published next. The path contains a maximum of pathLength_ (change with setPathLength)poses that are published. Once the maximum is reached, the last pose is copied in a new path message. The rest are deleted. More... | |
Publish | |
void | publishPose () |
Publish the pose. More... | |
void | publishTransform () |
Publish the T_WS transform. More... | |
void | publishStateAsCallback (const okvis::Time &t, const okvis::kinematics::Transformation &T_WS) |
Set and publish pose. More... | |
void | publishFullStateAsCallback (const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S) |
Set and publish full state. More... | |
void | publishLandmarksAsCallback (const okvis::Time &t, const okvis::MapPointVector &actualLandmarks, const okvis::MapPointVector &transferredLandmarks) |
Set and publish landmarks. More... | |
void | csvSaveFullStateAsCallback (const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S) |
Set and write full state to CSV file. More... | |
void | csvSaveFullStateWithExtrinsicsAsCallback (const okvis::Time &t, const okvis::kinematics::Transformation &T_WS, const Eigen::Matrix< double, 9, 1 > &speedAndBiases, const Eigen::Matrix< double, 3, 1 > &omega_S, const std::vector< okvis::kinematics::Transformation, Eigen::aligned_allocator< okvis::kinematics::Transformation > > &extrinsics) |
Set and write full state including camera extrinsics to file. More... | |
void | csvSaveLandmarksAsCallback (const okvis::Time &t, const okvis::MapPointVector &actualLandmarks, const okvis::MapPointVector &transferredLandmarks) |
Set and write landmarks to file. More... | |
void | publishOdometry () |
Publish the last set odometry. More... | |
void | publishPoints () |
Publish the last set points. More... | |
void | publishImages () |
Publish the last set images. More... | |
void | publishPath () |
Publish the last set path. More... | |
Private Member Functions | |
bool | writeCsvDescription () |
Write CSV header. More... | |
bool | writeLandmarksCsvDescription () |
Write CSV header for landmarks file. More... | |
Private Attributes | |
ros::Time | lastOdometryTime_ |
Timestamp of the last broadcasted transform. (publishPose()) More... | |
ros::Time | lastOdometryTime2_ |
Timestamp of the last published odometry message. (publishOdometry()) More... | |
ros::Time | lastTransfromTime_ |
Timestamp of the last published transform. (publishTransform()) More... | |
okvis::VioParameters | parameters_ |
All the parameters including publishing options. More... | |
uint32_t | ctr2_ |
The counter for the amount of transferred points. Used for the seq parameter in the header. More... | |
std::shared_ptr< std::fstream > | csvFile_ |
CSV file to save state in. More... | |
std::shared_ptr< std::fstream > | csvLandmarksFile_ |
CSV file to save landmarks in. More... | |
Node and subscriber related | |
ros::NodeHandle * | nh_ |
The node handle. More... | |
tf::TransformBroadcaster | pubTf_ |
The transform broadcaster. More... | |
ros::Publisher | pubPointsMatched_ |
The publisher for matched points. More... | |
ros::Publisher | pubPointsUnmatched_ |
The publisher for unmatched points. More... | |
ros::Publisher | pubPointsTransferred_ |
The publisher for transferred/marginalised points. More... | |
ros::Publisher | pubObometry_ |
The publisher for the odometry. More... | |
ros::Publisher | pubPath_ |
The publisher for the path. More... | |
ros::Publisher | pubTransform_ |
The publisher for the transform. More... | |
ros::Publisher | pubMesh_ |
The publisher for a robot / camera mesh. More... | |
std::vector < image_transport::Publisher > | pubImagesVector_ |
The publisher for the images. More... | |
std::vector < image_transport::ImageTransport > | imageTransportVector_ |
The image transporters. More... | |
To be published | |
ros::Time | _t |
Header timestamp. More... | |
geometry_msgs::TransformStamped | poseMsg_ |
Pose message. More... | |
nav_msgs::Odometry | odometryMsg_ |
Odometry message. More... | |
okvis::MapPointVector | pointsMatched2_ |
Matched points vector. More... | |
pcl::PointCloud< pcl::PointXYZRGB > | pointsMatched_ |
Point cloud for matched points. More... | |
pcl::PointCloud< pcl::PointXYZRGB > | pointsUnmatched_ |
Point cloud for unmatched points. More... | |
pcl::PointCloud< pcl::PointXYZRGB > | pointsTransferred_ |
Point cloud for transferred/marginalised points. More... | |
std::vector< cv::Mat > | images_ |
The images. More... | |
nav_msgs::Path | path_ |
The path message. More... | |
visualization_msgs::Marker | meshMsg_ |
Mesh message. More... | |
This class handles the publishing to either ROS topics or files.
okvis::Publisher::Publisher | ( | ) |
Default constructor.
okvis::Publisher::~Publisher | ( | ) |
okvis::Publisher::Publisher | ( | ros::NodeHandle & | nh | ) |
Constructor. Calls setNodeHandle().
nh | The ROS node handle for publishing. |
void okvis::Publisher::csvSaveFullStateAsCallback | ( | const okvis::Time & | t, |
const okvis::kinematics::Transformation & | T_WS, | ||
const Eigen::Matrix< double, 9, 1 > & | speedAndBiases, | ||
const Eigen::Matrix< double, 3, 1 > & | omega_S | ||
) |
Set and write full state to CSV file.
t | Timestamp of state. |
T_WS | The pose. |
speedAndBiases | The speeds and IMU biases. |
omega_S | Rotational speed of the sensor frame. |
void okvis::Publisher::csvSaveFullStateWithExtrinsicsAsCallback | ( | const okvis::Time & | t, |
const okvis::kinematics::Transformation & | T_WS, | ||
const Eigen::Matrix< double, 9, 1 > & | speedAndBiases, | ||
const Eigen::Matrix< double, 3, 1 > & | omega_S, | ||
const std::vector< okvis::kinematics::Transformation, Eigen::aligned_allocator< okvis::kinematics::Transformation > > & | extrinsics | ||
) |
Set and write full state including camera extrinsics to file.
t | Timestamp of state. |
T_WS | The pose. |
speedAndBiases | The speeds and IMU biases. |
omega_S | Rotation speed of the sensor frame. |
extrinsics | Camera extrinsics. |
void okvis::Publisher::csvSaveLandmarksAsCallback | ( | const okvis::Time & | t, |
const okvis::MapPointVector & | actualLandmarks, | ||
const okvis::MapPointVector & | transferredLandmarks | ||
) |
Set and write landmarks to file.
t | Timestamp. |
actualLandmarks | Landmarks. |
transferredLandmarks | Landmarks that were marginalised out. |
void okvis::Publisher::publishFullStateAsCallback | ( | const okvis::Time & | t, |
const okvis::kinematics::Transformation & | T_WS, | ||
const Eigen::Matrix< double, 9, 1 > & | speedAndBiases, | ||
const Eigen::Matrix< double, 3, 1 > & | omega_S | ||
) |
Set and publish full state.
t | Timestamp of state. |
T_WS | The pose. |
speedAndBiases | The speeds and IMU biases. |
omega_S | Rotational speed of the sensor frame. |
void okvis::Publisher::publishImages | ( | ) |
Publish the last set images.
void okvis::Publisher::publishLandmarksAsCallback | ( | const okvis::Time & | t, |
const okvis::MapPointVector & | actualLandmarks, | ||
const okvis::MapPointVector & | transferredLandmarks | ||
) |
Set and publish landmarks.
t | Timestamp. |
actualLandmarks | Landmarks. |
transferredLandmarks | Landmarks that were marginalised out. |
void okvis::Publisher::publishOdometry | ( | ) |
Publish the last set odometry.
void okvis::Publisher::publishPath | ( | ) |
Publish the last set path.
void okvis::Publisher::publishPoints | ( | ) |
Publish the last set points.
void okvis::Publisher::publishPose | ( | ) |
Publish the pose.
void okvis::Publisher::publishStateAsCallback | ( | const okvis::Time & | t, |
const okvis::kinematics::Transformation & | T_WS | ||
) |
Set and publish pose.
t | Timestamp of pose. |
T_WS | The pose. |
void okvis::Publisher::publishTransform | ( | ) |
Publish the T_WS transform.
bool okvis::Publisher::setCsvFile | ( | std::fstream & | csvFile | ) |
Set an odometry output CSV file.
csvFile | The file |
bool okvis::Publisher::setCsvFile | ( | std::string & | csvFileName | ) |
Set an odometry output CSV file.
csvFileName | The filename of a new file |
bool okvis::Publisher::setCsvFile | ( | std::string | csvFileName | ) |
Set an odometry output CSV file.
csvFileName | The filename of a new file |
void okvis::Publisher::setImages | ( | const std::vector< cv::Mat > & | images | ) |
Set the images to be published next.
bool okvis::Publisher::setLandmarksCsvFile | ( | std::fstream & | csvFile | ) |
Set a CVS file where the landmarks will be saved to.
csvFile | The file |
bool okvis::Publisher::setLandmarksCsvFile | ( | std::string & | csvFileName | ) |
Set a CVS file where the landmarks will be saved to.
csvFileName | The filename of a new file |
bool okvis::Publisher::setLandmarksCsvFile | ( | std::string | csvFileName | ) |
Set a CVS file where the landmarks will be saved to.
csvFileName | The filename of a new file |
void okvis::Publisher::setNodeHandle | ( | ros::NodeHandle & | nh | ) |
Set the node handle and advertise topics.
nh | The ROS node handle. |
void okvis::Publisher::setOdometry | ( | const okvis::kinematics::Transformation & | T_WS, |
const okvis::SpeedAndBiases & | speedAndBiases, | ||
const Eigen::Vector3d & | omega_S | ||
) |
Set the odometry message that is published next.
T_WS | The pose. |
speedAndBiases | The speeds and biases. |
omega_S | Rotational speed of Sensor frame (w.r.t. to inertial frame W) |
|
inline |
Set the parameters.
parameters | The parameters. |
void okvis::Publisher::setPath | ( | const okvis::kinematics::Transformation & | T_WS | ) |
Add a pose to the path that is published next. The path contains a maximum of pathLength_ (change with setPathLength)poses that are published. Once the maximum is reached, the last pose is copied in a new path message. The rest are deleted.
void okvis::Publisher::setPoints | ( | const okvis::MapPointVector & | pointsMatched, |
const okvis::MapPointVector & | pointsUnmatched, | ||
const okvis::MapPointVector & | pointsTransferred | ||
) |
Set the points that are published next.
pointsMatched | Vector of 3D points that have been matched with existing landmarks. |
pointsUnmatched | Vector of 3D points that were not matched with existing landmarks. |
pointsTransferred | Vector of landmarks that have been marginalised out. |
void okvis::Publisher::setPose | ( | const okvis::kinematics::Transformation & | T_WS | ) |
Set the pose message that is published next.
T_WS | The pose. |
|
inline |
Set the time for the next message to be published.
|
private |
Write CSV header.
|
private |
Write CSV header for landmarks file.
|
private |
Header timestamp.
|
private |
CSV file to save state in.
|
private |
CSV file to save landmarks in.
|
private |
The counter for the amount of transferred points. Used for the seq parameter in the header.
|
private |
The images.
|
private |
The image transporters.
|
private |
Timestamp of the last published odometry message. (publishOdometry())
|
private |
Timestamp of the last broadcasted transform. (publishPose())
|
private |
Timestamp of the last published transform. (publishTransform())
|
private |
Mesh message.
|
private |
The node handle.
|
private |
Odometry message.
|
private |
All the parameters including publishing options.
|
private |
The path message.
|
private |
Matched points vector.
|
private |
Point cloud for matched points.
|
private |
Point cloud for transferred/marginalised points.
|
private |
Point cloud for unmatched points.
|
private |
Pose message.
|
private |
The publisher for the images.
|
private |
The publisher for a robot / camera mesh.
|
private |
The publisher for the odometry.
|
private |
The publisher for the path.
|
private |
The publisher for matched points.
|
private |
The publisher for transferred/marginalised points.
|
private |
The publisher for unmatched points.
|
private |
The transform broadcaster.
|
private |
The publisher for the transform.