OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Public Member Functions | Private Member Functions | Private Attributes | List of all members
okvis::Publisher Class Reference

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 &parameters)
 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...
 

Detailed Description

This class handles the publishing to either ROS topics or files.

Constructor & Destructor Documentation

okvis::Publisher::Publisher ( )

Default constructor.

okvis::Publisher::~Publisher ( )
okvis::Publisher::Publisher ( ros::NodeHandle &  nh)

Constructor. Calls setNodeHandle().

Parameters
nhThe ROS node handle for publishing.

Member Function Documentation

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.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp of state.
T_WSThe pose.
speedAndBiasesThe speeds and IMU biases.
omega_SRotational 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.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp of state.
T_WSThe pose.
speedAndBiasesThe speeds and IMU biases.
omega_SRotation speed of the sensor frame.
extrinsicsCamera extrinsics.
void okvis::Publisher::csvSaveLandmarksAsCallback ( const okvis::Time t,
const okvis::MapPointVector actualLandmarks,
const okvis::MapPointVector transferredLandmarks 
)

Set and write landmarks to file.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp.
actualLandmarksLandmarks.
transferredLandmarksLandmarks 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.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp of state.
T_WSThe pose.
speedAndBiasesThe speeds and IMU biases.
omega_SRotational 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.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp.
actualLandmarksLandmarks.
transferredLandmarksLandmarks 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.

Remarks
This can be registered with the VioInterface.
Parameters
tTimestamp of pose.
T_WSThe pose.
void okvis::Publisher::publishTransform ( )

Publish the T_WS transform.

bool okvis::Publisher::setCsvFile ( std::fstream &  csvFile)

Set an odometry output CSV file.

Parameters
csvFileThe file
bool okvis::Publisher::setCsvFile ( std::string &  csvFileName)

Set an odometry output CSV file.

Parameters
csvFileNameThe filename of a new file
bool okvis::Publisher::setCsvFile ( std::string  csvFileName)

Set an odometry output CSV file.

Parameters
csvFileNameThe 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.

Parameters
csvFileThe file
bool okvis::Publisher::setLandmarksCsvFile ( std::string &  csvFileName)

Set a CVS file where the landmarks will be saved to.

Parameters
csvFileNameThe filename of a new file
bool okvis::Publisher::setLandmarksCsvFile ( std::string  csvFileName)

Set a CVS file where the landmarks will be saved to.

Parameters
csvFileNameThe filename of a new file
void okvis::Publisher::setNodeHandle ( ros::NodeHandle &  nh)

Set the node handle and advertise topics.

Parameters
nhThe 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.

Parameters
T_WSThe pose.
speedAndBiasesThe speeds and biases.
omega_SRotational speed of Sensor frame (w.r.t. to inertial frame W)
void okvis::Publisher::setParameters ( const okvis::VioParameters parameters)
inline

Set the parameters.

Parameters
parametersThe 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.

Parameters
pointsMatchedVector of 3D points that have been matched with existing landmarks.
pointsUnmatchedVector of 3D points that were not matched with existing landmarks.
pointsTransferredVector 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.

Parameters
T_WSThe pose.
void okvis::Publisher::setTime ( const okvis::Time t)
inline

Set the time for the next message to be published.

bool okvis::Publisher::writeCsvDescription ( )
private

Write CSV header.

bool okvis::Publisher::writeLandmarksCsvDescription ( )
private

Write CSV header for landmarks file.

Member Data Documentation

ros::Time okvis::Publisher::_t
private

Header timestamp.

std::shared_ptr<std::fstream> okvis::Publisher::csvFile_
private

CSV file to save state in.

std::shared_ptr<std::fstream> okvis::Publisher::csvLandmarksFile_
private

CSV file to save landmarks in.

uint32_t okvis::Publisher::ctr2_
private

The counter for the amount of transferred points. Used for the seq parameter in the header.

std::vector<cv::Mat> okvis::Publisher::images_
private

The images.

std::vector<image_transport::ImageTransport> okvis::Publisher::imageTransportVector_
private

The image transporters.

ros::Time okvis::Publisher::lastOdometryTime2_
private

Timestamp of the last published odometry message. (publishOdometry())

ros::Time okvis::Publisher::lastOdometryTime_
private

Timestamp of the last broadcasted transform. (publishPose())

ros::Time okvis::Publisher::lastTransfromTime_
private

Timestamp of the last published transform. (publishTransform())

visualization_msgs::Marker okvis::Publisher::meshMsg_
private

Mesh message.

ros::NodeHandle* okvis::Publisher::nh_
private

The node handle.

nav_msgs::Odometry okvis::Publisher::odometryMsg_
private

Odometry message.

okvis::VioParameters okvis::Publisher::parameters_
private

All the parameters including publishing options.

nav_msgs::Path okvis::Publisher::path_
private

The path message.

okvis::MapPointVector okvis::Publisher::pointsMatched2_
private

Matched points vector.

pcl::PointCloud<pcl::PointXYZRGB> okvis::Publisher::pointsMatched_
private

Point cloud for matched points.

pcl::PointCloud<pcl::PointXYZRGB> okvis::Publisher::pointsTransferred_
private

Point cloud for transferred/marginalised points.

pcl::PointCloud<pcl::PointXYZRGB> okvis::Publisher::pointsUnmatched_
private

Point cloud for unmatched points.

geometry_msgs::TransformStamped okvis::Publisher::poseMsg_
private

Pose message.

std::vector<image_transport::Publisher> okvis::Publisher::pubImagesVector_
private

The publisher for the images.

ros::Publisher okvis::Publisher::pubMesh_
private

The publisher for a robot / camera mesh.

ros::Publisher okvis::Publisher::pubObometry_
private

The publisher for the odometry.

ros::Publisher okvis::Publisher::pubPath_
private

The publisher for the path.

ros::Publisher okvis::Publisher::pubPointsMatched_
private

The publisher for matched points.

ros::Publisher okvis::Publisher::pubPointsTransferred_
private

The publisher for transferred/marginalised points.

ros::Publisher okvis::Publisher::pubPointsUnmatched_
private

The publisher for unmatched points.

tf::TransformBroadcaster okvis::Publisher::pubTf_
private

The transform broadcaster.

ros::Publisher okvis::Publisher::pubTransform_
private

The publisher for the transform.


The documentation for this class was generated from the following files: