The ICPslamWrapper class provides 2d icp based SLAM from MRPT libraries. More...
#include <mrpt_icp_slam_2d_wrapper.h>
Public Member Functions | |
void | get_param () |
read the parameters from launch file More... | |
ICPslamWrapper () | |
constructor More... | |
void | init () |
initialize publishers subscribers and icp slam More... | |
void | init3Dwindow () |
init 3D window from mrpt lib More... | |
bool | is_file_exists (const std::string &name) |
check the existance of the file More... | |
void | laserCallback (const sensor_msgs::LaserScan &_msg) |
callback function for the laser scans More... | |
void | publishMapPose () |
publish point and/or grid map and robot pose More... | |
void | publishTF () |
publis tf tree More... | |
void | publishTrajectoryTimerCallback (const ros::TimerEvent &event) |
the callback for publish trajectory More... | |
bool | rawlogPlay () |
play rawlog file More... | |
void | read_iniFile (std::string ini_filename) |
read ini file More... | |
void | run3Dwindow () |
run 3D window update from mrpt lib More... | |
void | updateSensorPose (std::string _frame_id) |
update the pose of the sensor with respect to the robot More... | |
void | updateTrajectoryTimerCallback (const ros::TimerEvent &event) |
the callback for update trajectory More... | |
~ICPslamWrapper () | |
destructor More... | |
Protected Attributes | |
std::string | base_frame_id |
robot frame More... | |
bool | CAMERA_3DSCENE_FOLLOWS_ROBOT |
std::string | global_frame_id |
/map frame More... | |
std::string | ini_filename |
name of ini file More... | |
bool | isObsBasedRawlog |
std::map< std::string, mrpt::poses::CPose3D > | laser_poses_ |
laser scan poses with respect to the map More... | |
tf::TransformListener | listenerTF_ |
transform listener More... | |
std::vector< CObservation2DRangeScan::Ptr > | lst_current_laser_scans |
CMetricMapBuilderICP | mapBuilder |
icp slam class More... | |
const CMultiMetricMap * | metric_map_ |
receive map after iteration of SLAM to metric map More... | |
ros::NodeHandle | n_ |
Node Handle. More... | |
CObservation::Ptr | observation |
CSensoryFrame::Ptr | observations |
std::string | odom_frame_id |
/odom frame More... | |
nav_msgs::Path | path |
trajectory path More... | |
geometry_msgs::PoseStamped | pose |
the robot pose More... | |
ros::Publisher | pub_map_ |
ros::Publisher | pub_metadata_ |
ros::Publisher | pub_point_cloud_ |
publishers for map and pose particles More... | |
ros::Publisher | pub_pose_ |
ros::Timer | publish_trajectory_timer |
timer for publish trajectory 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::Subscriber > | sensorSub_ |
list of sensors topics More... | |
bool | SHOW_LASER_SCANS_3D |
bool | SHOW_PROGRESS_3D_REAL_TIME |
int | SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS |
ros::Time | stamp |
timestamp for observations 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... | |
mrpt::system::TTimeStamp | timeLastUpdate_ |
last update of the pose and map More... | |
ros::Publisher | trajectory_pub_ |
trajectory publisher More... | |
double | trajectory_publish_rate |
trajectory publish rate(Hz) More... | |
double | trajectory_update_rate |
trajectory update rate(Hz) More... | |
ros::Timer | update_trajector_timer |
timer for update trajectory More... | |
mrpt::gui::CDisplayWindow3D::Ptr | win3D_ |
MRPT window. More... | |
The ICPslamWrapper class provides 2d icp based SLAM from MRPT libraries.
Definition at line 92 of file mrpt_icp_slam_2d_wrapper.h.
ICPslamWrapper::ICPslamWrapper | ( | ) |
constructor
Definition at line 22 of file mrpt_icp_slam_2d_wrapper.cpp.
ICPslamWrapper::~ICPslamWrapper | ( | ) |
destructor
Definition at line 32 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::get_param | ( | ) |
read the parameters from launch file
Definition at line 96 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::init | ( | ) |
initialize publishers subscribers and icp slam
Create publishers///
Create subscribers///
Definition at line 270 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::init3Dwindow | ( | ) |
init 3D window from mrpt lib
Definition at line 125 of file mrpt_icp_slam_2d_wrapper.cpp.
bool ICPslamWrapper::is_file_exists | ( | const std::string & | name | ) |
check the existance of the file
Definition at line 50 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::laserCallback | ( | const sensor_msgs::LaserScan & | _msg | ) |
callback function for the laser scans
Given the laser scans, implement one SLAM update, publish map and pose.
_msg | the laser scan message |
Definition at line 328 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::publishMapPose | ( | ) |
publish point and/or grid map and robot pose
Definition at line 354 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::publishTF | ( | ) |
publis tf tree
Definition at line 527 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::publishTrajectoryTimerCallback | ( | const ros::TimerEvent & | event | ) |
the callback for publish trajectory
event | the event for publish trajectory |
Definition at line 565 of file mrpt_icp_slam_2d_wrapper.cpp.
bool ICPslamWrapper::rawlogPlay | ( | ) |
play rawlog file
Definition at line 429 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::read_iniFile | ( | std::string | ini_filename | ) |
read ini file
ini_filename | the name of the ini file to read |
Definition at line 55 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::run3Dwindow | ( | ) |
run 3D window update from mrpt lib
Definition at line 137 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::updateSensorPose | ( | std::string | _frame_id | ) |
update the pose of the sensor with respect to the robot
frame_id | the frame of the sensors |
Definition at line 405 of file mrpt_icp_slam_2d_wrapper.cpp.
void ICPslamWrapper::updateTrajectoryTimerCallback | ( | const ros::TimerEvent & | event | ) |
the callback for update trajectory
event | the event for update trajectory |
Definition at line 557 of file mrpt_icp_slam_2d_wrapper.cpp.
|
protected |
robot frame
Definition at line 189 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 230 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
/map frame
Definition at line 187 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
name of ini file
Definition at line 186 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 226 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
laser scan poses with respect to the map
Definition at line 203 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
transform listener
Definition at line 212 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 225 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
icp slam class
Definition at line 180 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
receive map after iteration of SLAM to metric map
Definition at line 208 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Node Handle.
Definition at line 181 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 218 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 217 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
/odom frame
Definition at line 188 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
trajectory path
Definition at line 193 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
the robot pose
Definition at line 190 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 210 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 210 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
publishers for map and pose particles
Definition at line 210 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 210 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
timer for publish trajectory
Definition at line 196 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
name of rawlog file
Definition at line 185 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
true if rawlog file exists
Definition at line 183 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
delay of replay from rawlog file
Definition at line 182 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
2D laser scans
Definition at line 202 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
list of sensors topics
Definition at line 206 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 229 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 227 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
Definition at line 228 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
timestamp for observations
Definition at line 221 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
the time which take one SLAM update execution
Definition at line 216 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
transform broadcaster
Definition at line 213 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
timer for SLAM performance evaluation
Definition at line 215 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
last update of the pose and map
Definition at line 219 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
trajectory publisher
Definition at line 192 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
trajectory publish rate(Hz)
Definition at line 199 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
trajectory update rate(Hz)
Definition at line 198 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
timer for update trajectory
Definition at line 195 of file mrpt_icp_slam_2d_wrapper.h.
|
protected |
MRPT window.
Definition at line 223 of file mrpt_icp_slam_2d_wrapper.h.