SnapMapICP.cpp File Reference

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl/ros/conversions.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/transform_listener.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <boost/thread/mutex.hpp>

Include dependency graph for SnapMapICP.cpp:

Go to the source code of this file.

Typedefs
typedef pcl::PointCloud < pcl::PointXYZ >	PointCloudT
Functions
bool	getTransform (tf::StampedTransform &trans, const std::string parent_frame, const std::string child_frame, const ros::Time stamp)
pcl::KdTree< pcl::PointXYZ >::Ptr	getTree (pcl::PointCloud< pcl::PointXYZ >::Ptr cloudb)
int	main (int argc, char **argv)
void	mapCallback (const nav_msgs::OccupancyGrid &msg)
void	matrixAsTransfrom (const Eigen::Matrix4f &out_mat, tf::Transform &bt)
void	scanCallback (const sensor_msgs::LaserScan::ConstPtr &scan_in)
void	updateParams ()
Variables
int	actScan = 0
double	AGE_THRESHOLD = 1
double	ANGLE_THRESHOLD = 0.01
double	ANGLE_UPPER_THRESHOLD = M_PI / 6
std::string	BASE_LASER_FRAME = "/base_laser_link"
sensor_msgs::PointCloud2	cloud2
sensor_msgs::PointCloud2	cloud2transformed
boost::shared_ptr < pcl::PointCloud < pcl::PointXYZ > >	cloud_xyz
int	count_sc_ = 0
double	DIST_THRESHOLD = 0.05
double	ICP_FITNESS_THRESHOLD = 100.1
double	ICP_INLIER_DIST = 0.1
double	ICP_INLIER_THRESHOLD = 0.9
double	ICP_NUM_ITER = 250
ros::Time	last_processed_scan
int	lastScan = 0
int	lastTimeSent = -1000
tf::TransformListener *	listener_ = 0
pcl::KdTree< pcl::PointXYZ >::Ptr	mapTree
ros::NodeHandle *	nh = 0
std::string	ODOM_FRAME = "/odom_combined"
boost::shared_ptr < sensor_msgs::PointCloud2 >	output_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2())
ros::Time	paramsWereUpdated
double	POSE_COVARIANCE_TRANS = 1.5
laser_geometry::LaserProjection *	projector_ = 0
ros::Publisher	pub_info_
ros::Publisher	pub_output_
ros::Publisher	pub_output_scan
ros::Publisher	pub_output_scan_transformed
ros::Publisher	pub_pose
boost::mutex	scan_callback_mutex
boost::shared_ptr < sensor_msgs::PointCloud2 >	scan_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2())
double	SCAN_RATE = 2
double	UPDATE_AGE_THRESHOLD = 1
bool	use_sim_time = false
bool	we_have_a_map = false
bool	we_have_a_scan = false
bool	we_have_a_scan_transformed = false

---

Typedef Documentation

typedef pcl::PointCloud<pcl::PointXYZ> PointCloudT

Definition at line 93 of file SnapMapICP.cpp.

---

Function Documentation

bool getTransform	(	tf::StampedTransform &	trans,
		const std::string	parent_frame,
		const std::string	child_frame,
		const ros::Time	stamp
	)

Definition at line 214 of file SnapMapICP.cpp.

pcl::KdTree<pcl::PointXYZ>::Ptr getTree

(

pcl::PointCloud< pcl::PointXYZ >::Ptr

cloudb

)

Definition at line 155 of file SnapMapICP.cpp.

int main	(	int	argc,
		char **	argv
	)

Definition at line 514 of file SnapMapICP.cpp.

void mapCallback

(

const nav_msgs::OccupancyGrid &

msg

)

Definition at line 165 of file SnapMapICP.cpp.

void matrixAsTransfrom	(	const Eigen::Matrix4f &	out_mat,
		tf::Transform &	bt
	)			[inline]

Definition at line 126 of file SnapMapICP.cpp.

void scanCallback

(

const sensor_msgs::LaserScan::ConstPtr &

scan_in

)

Definition at line 243 of file SnapMapICP.cpp.

void updateParams

(

)

Definition at line 492 of file SnapMapICP.cpp.

---

Variable Documentation

int actScan = 0

Definition at line 105 of file SnapMapICP.cpp.

double AGE_THRESHOLD = 1

Definition at line 65 of file SnapMapICP.cpp.

double ANGLE_THRESHOLD = 0.01

Definition at line 62 of file SnapMapICP.cpp.

double ANGLE_UPPER_THRESHOLD = M_PI / 6

Definition at line 63 of file SnapMapICP.cpp.

std::string BASE_LASER_FRAME = "/base_laser_link"

Definition at line 76 of file SnapMapICP.cpp.

sensor_msgs::PointCloud2 cloud2

Definition at line 90 of file SnapMapICP.cpp.

sensor_msgs::PointCloud2 cloud2transformed

Definition at line 91 of file SnapMapICP.cpp.

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz

Definition at line 150 of file SnapMapICP.cpp.

int count_sc_ = 0

Definition at line 212 of file SnapMapICP.cpp.

double DIST_THRESHOLD = 0.05

Definition at line 60 of file SnapMapICP.cpp.

double ICP_FITNESS_THRESHOLD = 100.1

Definition at line 58 of file SnapMapICP.cpp.

double ICP_INLIER_DIST = 0.1

Definition at line 69 of file SnapMapICP.cpp.

double ICP_INLIER_THRESHOLD = 0.9

Definition at line 68 of file SnapMapICP.cpp.

double ICP_NUM_ITER = 250

Definition at line 72 of file SnapMapICP.cpp.

ros::Time last_processed_scan

Definition at line 241 of file SnapMapICP.cpp.

int lastScan = 0

Definition at line 104 of file SnapMapICP.cpp.

int lastTimeSent = -1000

Definition at line 210 of file SnapMapICP.cpp.

tf::TransformListener* listener_ = 0

Definition at line 89 of file SnapMapICP.cpp.

pcl::KdTree<pcl::PointXYZ>::Ptr mapTree

Definition at line 152 of file SnapMapICP.cpp.

ros::NodeHandle* nh = 0

Definition at line 79 of file SnapMapICP.cpp.

std::string ODOM_FRAME = "/odom_combined"

Definition at line 77 of file SnapMapICP.cpp.

boost::shared_ptr< sensor_msgs::PointCloud2> output_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2())

Definition at line 95 of file SnapMapICP.cpp.

ros::Time paramsWereUpdated

Definition at line 489 of file SnapMapICP.cpp.

double POSE_COVARIANCE_TRANS = 1.5

Definition at line 71 of file SnapMapICP.cpp.

laser_geometry::LaserProjection* projector_ = 0

Definition at line 88 of file SnapMapICP.cpp.

ros::Publisher pub_info_

Definition at line 83 of file SnapMapICP.cpp.

ros::Publisher pub_output_

Definition at line 80 of file SnapMapICP.cpp.

ros::Publisher pub_output_scan

Definition at line 81 of file SnapMapICP.cpp.

ros::Publisher pub_output_scan_transformed

Definition at line 82 of file SnapMapICP.cpp.

ros::Publisher pub_pose

Definition at line 85 of file SnapMapICP.cpp.

boost::mutex scan_callback_mutex

Definition at line 54 of file SnapMapICP.cpp.

boost::shared_ptr< sensor_msgs::PointCloud2> scan_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2())

Definition at line 96 of file SnapMapICP.cpp.

double SCAN_RATE = 2

Definition at line 74 of file SnapMapICP.cpp.

double UPDATE_AGE_THRESHOLD = 1

Definition at line 66 of file SnapMapICP.cpp.

bool use_sim_time = false

Definition at line 102 of file SnapMapICP.cpp.

bool we_have_a_map = false

Definition at line 98 of file SnapMapICP.cpp.

bool we_have_a_scan = false

Definition at line 99 of file SnapMapICP.cpp.

bool we_have_a_scan_transformed = false

Definition at line 100 of file SnapMapICP.cpp.