The EKFslamWrapper class provides the ROS wrapper for EKF SLAM 3d from MRPT libraries. More...

#include <mrpt_ekf_slam_3d_wrapper.h>

Inheritance diagram for EKFslamWrapper:

Public Member Functions
void	computeEllipseOrientationScale (tf::Quaternion &orientation, Eigen::Vector3d &scale, const mrpt::math::CMatrixDouble covariance)
	compute the orientation and scale of covariance ellipsoids
	EKFslamWrapper ()
	constructor
void	get_param ()
	read the parameters from launch file
void	init ()
	initialize publishers subscribers and EKF 3d slam
bool	is_file_exists (const std::string &name)
	check the existance of the file
void	landmarkCallback (const mrpt_msgs::ObservationRangeBearing &_msg)
	callback function for the landmarks
void	makeRightHanded (Eigen::Matrix3d &eigenvectors, Eigen::Vector3d &eigenvalues)
	compute the correct orientation and scale of covariance ellipsoids (make sure that we output covariance ellipsoids for right handed system of coordinates)
void	odometryForCallback (CObservationOdometry::Ptr &_odometry, const std_msgs::Header &_msg_header)
	get the odometry for received observation
void	publishTF ()
	publis tf tree
bool	rawlogPlay ()
	play rawlog file
void	updateSensorPose (std::string _frame_id)
	update the pose of the sensor with respect to the robot
void	viz_dataAssociation ()
	visualize the data associations for the landmarks observed by robot at the each step
void	viz_state ()
	visualize the covariance ellipsoids for robot and landmarks
bool	waitForTransform (mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
	wait for transfor between odometry frame and the robot frame
	~EKFslamWrapper ()
	destructor
Private Attributes
std::string	base_frame_id
	robot frame
ros::Publisher	data_association_viz_pub_
double	ellipse_scale_
std::string	global_frame_id
	/map frame
std::string	ini_filename
	name of ini file
std::map< std::string, mrpt::poses::CPose3D >	landmark_poses_
	landmark poses with respect to the map
tf::TransformListener	listenerTF_
	transform listener
ros::NodeHandle	n_
	Node handler.
std::string	odom_frame_id
	/odom frame
std::string	rawlog_filename
	name of rawlog file
bool	rawlog_play_
	true if rawlog file exists
double	rawlog_play_delay
	delay of replay from rawlog file
std::string	sensor_source
	2D laser scans
std::vector< ros::Subscriber >	sensorSub_
	list of sensors topics
ros::Publisher	state_viz_pub_
float	t_exec
	the time which take one SLAM update execution
tf::TransformBroadcaster	tf_broadcaster_
	transform broadcaster
CTicTac	tictac
	timer for SLAM performance evaluation

Detailed Description

The EKFslamWrapper class provides the ROS wrapper for EKF SLAM 3d from MRPT libraries.

Definition at line 65 of file mrpt_ekf_slam_3d_wrapper.h.

Constructor & Destructor Documentation

EKFslamWrapper::EKFslamWrapper ( )

constructor

Definition at line 14 of file mrpt_ekf_slam_3d_wrapper.cpp.

EKFslamWrapper::~EKFslamWrapper ( )

destructor

Definition at line 18 of file mrpt_ekf_slam_3d_wrapper.cpp.

Member Function Documentation

void EKFslamWrapper::computeEllipseOrientationScale	(	tf::Quaternion &	orientation,
		Eigen::Vector3d &	scale,
		const mrpt::math::CMatrixDouble	covariance
	)

compute the orientation and scale of covariance ellipsoids

Parameters:

orientation	the orientation of the ellipsoid in Quaternions
scale	the vector of the eigen values for calculating the size of the ellipse
covariance	covariance matrix for current landmarks or robot pose

Definition at line 255 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::get_param ( )

read the parameters from launch file

Definition at line 24 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::init ( )

initialize publishers subscribers and EKF 3d slam

Create subscribers///

Definition at line 50 of file mrpt_ekf_slam_3d_wrapper.cpp.

bool EKFslamWrapper::is_file_exists ( const std::string & name )

check the existance of the file

Returns:: true if file exists

Definition at line 19 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::landmarkCallback ( const mrpt_msgs::ObservationRangeBearing & _msg )

callback function for the landmarks

Given the landmarks wait for odometry, create the pair of action and observation, implement one SLAM update, publish map and pose.

Parameters:

_msg	the landmark message

Definition at line 150 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::makeRightHanded	(	Eigen::Matrix3d &	eigenvectors,
		Eigen::Vector3d &	eigenvalues
	)

compute the correct orientation and scale of covariance ellipsoids (make sure that we output covariance ellipsoids for right handed system of coordinates)

Parameters:

eigenvectors	the 3x3 matrix of eigenvectors
eigenvalues	the 3d vector of eigen values

Definition at line 233 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::odometryForCallback	(	CObservationOdometry::Ptr &	_odometry,
		const std_msgs::Header &	_msg_header
	)

get the odometry for received observation

Parameters:

_odometry	odometry for received observation
_msg_header	timestamp of the observation

Definition at line 92 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::publishTF ( )

publis tf tree

Definition at line 445 of file mrpt_ekf_slam_3d_wrapper.cpp.

bool EKFslamWrapper::rawlogPlay ( )

play rawlog file

Returns:: true if rawlog file exists and played

Definition at line 189 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::updateSensorPose ( std::string _frame_id )

update the pose of the sensor with respect to the robot

Parameters:

frame_id the frame of the sensors

Definition at line 107 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::viz_dataAssociation ( )

visualize the data associations for the landmarks observed by robot at the each step

Definition at line 299 of file mrpt_ekf_slam_3d_wrapper.cpp.

void EKFslamWrapper::viz_state ( )

visualize the covariance ellipsoids for robot and landmarks

Definition at line 360 of file mrpt_ekf_slam_3d_wrapper.cpp.

bool EKFslamWrapper::waitForTransform	(	mrpt::poses::CPose3D &	des,
		const std::string &	target_frame,
		const std::string &	source_frame,
		const ros::Time &	time,
		const ros::Duration &	timeout,
		const ros::Duration &	polling_sleep_duration = `ros::Duration(0.01)`
	)

wait for transfor between odometry frame and the robot frame

Parameters:

des	position of the robot with respect to odometry frame
target_frame	the odometry tf frame
source_frame	the robot tf frame
time	timestamp of the observation for which we want to retrieve the position of the robot
timeout	timeout for odometry waiting
polling_sleep_duration	timeout for transform wait