Public Member Functions | Private Attributes | List of all members
EKFslamWrapper Class Reference

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

#include <mrpt_ekf_slam_2d_wrapper.h>

Inheritance diagram for EKFslamWrapper:
Inheritance graph
[legend]

Public Member Functions

void computeEllipseOrientationScale2D (tf::Quaternion &orientation, Eigen::Vector2d &scale, const mrpt::math::CMatrixDouble covariance)
 compute the orientation and scale of covariance ellipsoids More...
 
 EKFslamWrapper ()
 constructor More...
 
void get_param ()
 read the parameters from launch file More...
 
void init ()
 initialize publishers subscribers and EKF 2d slam More...
 
bool is_file_exists (const std::string &name)
 check the existance of the file More...
 
void landmarkCallback (const mrpt_msgs::ObservationRangeBearing &_msg)
 callback function for the landmarks More...
 
void makeRightHanded (Eigen::Matrix2d &eigenvectors, Eigen::Vector2d &eigenvalues)
 compute the correct orientation and scale of covariance ellipsoids (make sure that we output covariance ellipsoids for right handed system of coordinates) More...
 
void odometryForCallback (CObservationOdometry::Ptr &_odometry, const std_msgs::Header &_msg_header)
 get the odometry for received observation More...
 
void publishTF ()
 publish tf tree More...
 
bool rawlogPlay ()
 play rawlog file More...
 
void updateSensorPose (std::string _frame_id)
 update the pose of the sensor with respect to the robot More...
 
void viz_dataAssociation ()
 visualize the data associations for the landmarks observed by robot at the each step More...
 
void viz_state ()
 visualize the covariance ellipsoids for robot and landmarks More...
 
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 transform between odometry frame and the robot frame More...
 
 ~EKFslamWrapper ()
 destructor More...
 

Private Attributes

std::string base_frame_id
 robot frame More...
 
ros::Publisher data_association_viz_pub_
 
double ellipse_scale_
 Scale of covariance ellipses. More...
 
std::string global_frame_id
 /map frame More...
 
std::string ini_filename
 name of ini file More...
 
std::map< std::string, mrpt::poses::CPose3Dlandmark_poses_
 landmark poses with respect to the map More...
 
tf::TransformListener listenerTF_
 transform listener More...
 
ros::NodeHandle n_
 Node handler. More...
 
std::string odom_frame_id
 /odom frame More...
 
std::string rawlog_filename
 name of rawlog file More...
 
bool rawlog_play_
 true if rawlog file exists More...
 
double rawlog_play_delay
 delay of replay from rawlog file More...
 
std::string sensor_source
 2D laser scans More...
 
std::vector< ros::SubscribersensorSub_
 list of sensors topics More...
 
ros::Publisher state_viz_pub_
 publishers More...
 
float t_exec
 the time which take one SLAM update execution More...
 
tf::TransformBroadcaster tf_broadcaster_
 transform broadcaster More...
 
CTicTac tictac
 timer for SLAM performance evaluation More...
 
- Private Attributes inherited from EKFslam
CActionCollection::Ptr action
 actions More...
 
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
 
CMatrixDouble fullCov_
 full covariance matrix More...
 
CVectorDouble fullState_
 full state vector More...
 
std::map< unsigned int, CLandmark::TLandmarkIDLM_IDs_
 vector of the landmarks ID More...
 
std::vector< mrpt::math::TPoint2DLMs_
 vector of the landmarks More...
 
CRangeBearingKFSLAM2D mapping
 EKF slam 2d class. More...
 
vector< TPose3DmeanPath
 
CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_
 used if there are is not odom More...
 
CActionRobotMovement2D::TMotionModelOptions motion_model_options_
 used with odom value motion noise More...
 
mrpt::poses::CPose2D odomLastObservation_
 last observation of odometry More...
 
CPosePDFGaussian robotPose_
 current robot pose More...
 
CSensoryFrame::Ptr sf
 observations More...
 
bool SHOW_3D_LIVE
 
mrpt::system::TTimeStamp timeLastUpdate_
 last update of the pose and map More...
 
bool use_motion_model_default_options_
 used default odom_params More...
 
mrpt::gui::CDisplayWindow3D::Ptr win3d
 MRPT window. More...
 

Additional Inherited Members

- Private Member Functions inherited from EKFslam
 EKFslam ()
 constructor More...
 
void init3Dwindow ()
 init 3D window from mrpt lib More...
 
void landmark_to_3d (const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p)
 convert landmark to 3d point More...
 
void observation (CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
 calculate the actions from odometry model for current observation More...
 
void read_iniFile (std::string ini_filename)
 read ini file More...
 
void run3Dwindow ()
 run 3D window update from mrpt lib More...
 
virtual ~EKFslam ()
 destructor More...
 

Detailed Description

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

Definition at line 64 of file mrpt_ekf_slam_2d_wrapper.h.

Constructor & Destructor Documentation

EKFslamWrapper::EKFslamWrapper ( )

constructor

Definition at line 11 of file mrpt_ekf_slam_2d_wrapper.cpp.

EKFslamWrapper::~EKFslamWrapper ( )

destructor

Definition at line 15 of file mrpt_ekf_slam_2d_wrapper.cpp.

Member Function Documentation

void EKFslamWrapper::computeEllipseOrientationScale2D ( tf::Quaternion orientation,
Eigen::Vector2d scale,
const mrpt::math::CMatrixDouble  covariance 
)

compute the orientation and scale of covariance ellipsoids

Parameters
orientationthe orientation of the ellipsoid in Quaternions
scalethe vector of the eigen values for calculating the size of the ellipse
covariancecovariance matrix for current landmarks or robot pose

Definition at line 255 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::get_param ( )

read the parameters from launch file

Definition at line 21 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::init ( )

initialize publishers subscribers and EKF 2d slam

Create subscribers///

Definition at line 47 of file mrpt_ekf_slam_2d_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 16 of file mrpt_ekf_slam_2d_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
_msgthe landmark message

Definition at line 148 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::makeRightHanded ( Eigen::Matrix2d eigenvectors,
Eigen::Vector2d eigenvalues 
)

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

Parameters
eigenvectorsthe 2x2 matrix of eigenvectors
eigenvaluesthe 2d vector of eigen values

Definition at line 231 of file mrpt_ekf_slam_2d_wrapper.cpp.

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

get the odometry for received observation

Parameters
_odometryodometry for received observation
_msg_headertimestamp of the observation

Definition at line 90 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::publishTF ( )

publish tf tree

Definition at line 441 of file mrpt_ekf_slam_2d_wrapper.cpp.

bool EKFslamWrapper::rawlogPlay ( )

play rawlog file

Returns
true if rawlog file exists and played

Definition at line 187 of file mrpt_ekf_slam_2d_wrapper.cpp.

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

update the pose of the sensor with respect to the robot

Parameters
frame_idthe frame of the sensors

Definition at line 105 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::viz_dataAssociation ( )

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

Definition at line 298 of file mrpt_ekf_slam_2d_wrapper.cpp.

void EKFslamWrapper::viz_state ( )

visualize the covariance ellipsoids for robot and landmarks

Definition at line 358 of file mrpt_ekf_slam_2d_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 transform between odometry frame and the robot frame

Parameters
desposition of the robot with respect to odometry frame
target_framethe odometry tf frame
source_framethe robot tf frame
timetimestamp of the observation for which we want to retrieve the position of the robot
timeouttimeout for odometry waiting
polling_sleep_durationtimeout for transform wait
Returns
true if there is transform from odometry to the robot

Definition at line 130 of file mrpt_ekf_slam_2d_wrapper.cpp.

Member Data Documentation

std::string EKFslamWrapper::base_frame_id
private

robot frame

Definition at line 178 of file mrpt_ekf_slam_2d_wrapper.h.

ros::Publisher EKFslamWrapper::data_association_viz_pub_
private

Definition at line 188 of file mrpt_ekf_slam_2d_wrapper.h.

double EKFslamWrapper::ellipse_scale_
private

Scale of covariance ellipses.

Definition at line 170 of file mrpt_ekf_slam_2d_wrapper.h.

std::string EKFslamWrapper::global_frame_id
private

/map frame

Definition at line 176 of file mrpt_ekf_slam_2d_wrapper.h.

std::string EKFslamWrapper::ini_filename
private

name of ini file

Definition at line 175 of file mrpt_ekf_slam_2d_wrapper.h.

std::map<std::string, mrpt::poses::CPose3D> EKFslamWrapper::landmark_poses_
private

landmark poses with respect to the map

Definition at line 183 of file mrpt_ekf_slam_2d_wrapper.h.

tf::TransformListener EKFslamWrapper::listenerTF_
private

transform listener

Definition at line 189 of file mrpt_ekf_slam_2d_wrapper.h.

ros::NodeHandle EKFslamWrapper::n_
private

Node handler.

Definition at line 168 of file mrpt_ekf_slam_2d_wrapper.h.

std::string EKFslamWrapper::odom_frame_id
private

/odom frame

Definition at line 177 of file mrpt_ekf_slam_2d_wrapper.h.

std::string EKFslamWrapper::rawlog_filename
private

name of rawlog file

Definition at line 174 of file mrpt_ekf_slam_2d_wrapper.h.

bool EKFslamWrapper::rawlog_play_
private

true if rawlog file exists

Definition at line 171 of file mrpt_ekf_slam_2d_wrapper.h.

double EKFslamWrapper::rawlog_play_delay
private

delay of replay from rawlog file

Definition at line 169 of file mrpt_ekf_slam_2d_wrapper.h.

std::string EKFslamWrapper::sensor_source
private

2D laser scans

Definition at line 181 of file mrpt_ekf_slam_2d_wrapper.h.

std::vector<ros::Subscriber> EKFslamWrapper::sensorSub_
private

list of sensors topics

Definition at line 173 of file mrpt_ekf_slam_2d_wrapper.h.

ros::Publisher EKFslamWrapper::state_viz_pub_
private

publishers

Definition at line 188 of file mrpt_ekf_slam_2d_wrapper.h.

float EKFslamWrapper::t_exec
private

the time which take one SLAM update execution

Definition at line 186 of file mrpt_ekf_slam_2d_wrapper.h.

tf::TransformBroadcaster EKFslamWrapper::tf_broadcaster_
private

transform broadcaster

Definition at line 190 of file mrpt_ekf_slam_2d_wrapper.h.

CTicTac EKFslamWrapper::tictac
private

timer for SLAM performance evaluation

Definition at line 185 of file mrpt_ekf_slam_2d_wrapper.h.


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


mrpt_ekf_slam_2d
Author(s): Jose Luis, Vladislav Tananaev
autogenerated on Sat May 2 2020 03:43:58