Public Member Functions
	Mapper ()
	Initialize node.
Private Member Functions
void	addGrid (unsigned n)
nm::OccupancyGrid::ConstPtr	computeLocalGrid (unsigned g) const
	Compute local occupancy grid.
om::OcTree	computeLocalOctomap (unsigned g) const
	Compute local octomap.
unsigned	coveredBy (unsigned n, unsigned g, double r) const
unsigned	coveredBy (unsigned n, const Grids &g, double r) const
Grids	currentRelevantGrids () const
void	diffCB (OptionalDiff diff, const pg::ConstraintGraph &g)
	Receive a diff to the pose graph.
void	ensureNodeCovered (unsigned n)
void	locCB (const gmm::LocalizationDistribution &dist)
	Receive a localization wrt slam graph.
tf::Pose	nodeGridTransform (unsigned n, unsigned g) const
	Transform from pose graph node frame to local grid frame.
Nodes	nodesOnGrid (unsigned g) const
NodePoses	optimizePoses (const pg::NodeSet &comp) const
Nodes	overlayingNodes (unsigned g) const
tf::Pose	poseOnGrid (unsigned n, unsigned g) const
void	publishGraph ()
	Publish graph over ros.
void	publishRefTransforms (const ros::TimerEvent &e)
	Publish reference transforms over ros.
void	saveCurrentGrid ()
void	saveGrid (unsigned g)
	Save local grid to db.
bool	switchGrid (msg::SwitchGrid::Request &req, msg::SwitchGrid::Response &resp)
	Switch grid based on external request.
Nodes	topologicallyCloseNodes (unsigned g, const double r) const
	Nodes that are topologically close in slam graph.
void	updateEdgeInfo (unsigned g, unsigned g2)
void	updateEdgeInfo (unsigned g)
void	updateLoop (const ros::TimerEvent &e)
	Periodically update and publish graph.
void	visualize (const ros::TimerEvent &e)
	Visualize graph.
void	visualizeCurrentGrid ()
	Visualize the grid we're currently localized wrt.
void	visualizeEdges ()
	Visualize edges between grids of the topological map.
void	visualizeGridBoundaries ()
	Visualize the boundaries of all grids.
Private Attributes
pg::CachedNodeMap < sm::PointCloud2 >	clouds_
NodeGridMap	covering_grids_
boost::optional< unsigned >	current_grid_
pg::DiffSubscriber	diff_sub_
ros::ServiceClient	get_poses_client_
ros::Publisher	graph_pub_
Nodes	grid_nodes_
const double	grid_overlap_
ros::Publisher	grid_pub_
map< unsigned, ros::Time >	grid_save_times_
const double	grid_size_
mr::MessageCollection < nm::OccupancyGrid >	grids_
ros::Time	last_diff_time_
boost::optional< gm::PoseStamped >	last_loc_
ros::Publisher	loc_pub_
ros::Subscriber	loc_sub_
ros::Publisher	marker_pub_
boost::mutex	mutex_
unsigned	next_edge_id_
ros::NodeHandle	nh_
ros::Publisher	octomap_pub_
NodePoses	opt_poses_
ros::NodeHandle	param_nh_
Nodes	pending_nodes_
pg::ConstraintGraph	pose_graph_
const string	ref_frame_
const double	resolution_
const double	robot_radius_
pg::CachedNodeMap < ls::LocalizedScan >	scans_
ros::ServiceServer	switch_grid_srv_
tf::TransformListener	tf_
tf::TransformBroadcaster	tfb_
tmap::TopologicalMap	tmap_
ros::Timer	transform_pub_timer_
bool	update_flag_
const double	update_rate_
ros::Timer	update_timer_
ros::Timer	vis_timer_

Detailed Description

Definition at line 96 of file mapper.cpp.

Constructor & Destructor Documentation

laser_slam_mapper::Mapper::Mapper ( )

Initialize node.

Definition at line 359 of file mapper.cpp.

Member Function Documentation

void laser_slam_mapper::Mapper::addGrid ( unsigned n ) [private]

Postcondition:: There's a new grid centered at n

Definition at line 501 of file mapper.cpp.

nm::OccupancyGrid::ConstPtr laser_slam_mapper::Mapper::computeLocalGrid ( unsigned g ) const [private]

Compute local occupancy grid.

Set up data structure to do ray tracing

Add the clouds

Possibly clear out free space based on robot poses

Definition at line 700 of file mapper.cpp.

om::OcTree laser_slam_mapper::Mapper::computeLocalOctomap ( unsigned g ) const [private]

Compute local octomap.

Definition at line 745 of file mapper.cpp.

unsigned laser_slam_mapper::Mapper::coveredBy	(	unsigned	n,
		unsigned	g,
		double	r
	)		const `[private]`

Return values:

Is	node n covered by (near center of) grid centered at g? If so, return g, else return 0.

Definition at line 477 of file mapper.cpp.

unsigned laser_slam_mapper::Mapper::coveredBy	(	unsigned	n,
		const Grids &	g,
		double	r
	)		const `[private]`

Return values:

Is	node n covered by one of the grids? If so, return the grid id. Else return 0.

Grids laser_slam_mapper::Mapper::currentRelevantGrids ( ) const [private]

Grids that are currently relevant to update. This is currently just the current reference grid we're localized wrt

Definition at line 571 of file mapper.cpp.

void laser_slam_mapper::Mapper::diffCB	(	OptionalDiff	diff,
		const pg::ConstraintGraph &	g
	)		`[private]`

Receive a diff to the pose graph.

Todo:: deal with edges

Definition at line 621 of file mapper.cpp.

void laser_slam_mapper::Mapper::ensureNodeCovered ( unsigned n ) [private]

Postcondition:: Ensure that there is a grid g that covers node n

void laser_slam_mapper::Mapper::locCB ( const gmm::LocalizationDistribution & dist ) [private]

Receive a localization wrt slam graph.

Definition at line 587 of file mapper.cpp.

tf::Pose laser_slam_mapper::Mapper::nodeGridTransform	(	unsigned	n,
		unsigned	g
	)		const `[private]`

Transform from pose graph node frame to local grid frame.

Definition at line 692 of file mapper.cpp.

Nodes laser_slam_mapper::Mapper::nodesOnGrid ( unsigned g ) const [private]

Return values:

Nodes that are on a given local grid

Definition at line 660 of file mapper.cpp.

NodePoses laser_slam_mapper::Mapper::optimizePoses ( const pg::NodeSet & comp ) const [private]

Return values:

Optimized poses of some nodes

Definition at line 401 of file mapper.cpp.

Nodes laser_slam_mapper::Mapper::overlayingNodes ( unsigned g ) const [private]

Return values:

Nodes which are topologically close to the grid and whose scan barycenters are near the center

Definition at line 671 of file mapper.cpp.

tf::Pose laser_slam_mapper::Mapper::poseOnGrid	(	unsigned	n,
		unsigned	g
	)		const `[private]`

Return values:

Pose	of node n wrt grid g

void laser_slam_mapper::Mapper::publishGraph ( ) [private]

Publish graph over ros.

Definition at line 944 of file mapper.cpp.

void laser_slam_mapper::Mapper::publishRefTransforms ( const ros::TimerEvent & e ) [private]

Publish reference transforms over ros.

Definition at line 952 of file mapper.cpp.

void laser_slam_mapper::Mapper::saveCurrentGrid ( ) [private]

Definition at line 963 of file mapper.cpp.

void laser_slam_mapper::Mapper::saveGrid ( unsigned g ) [private]

Save local grid to db.

Definition at line 923 of file mapper.cpp.

bool laser_slam_mapper::Mapper::switchGrid	(	msg::SwitchGrid::Request &	req,
		msg::SwitchGrid::Response &	resp
	)		`[private]`

Switch grid based on external request.

Definition at line 644 of file mapper.cpp.

Nodes laser_slam_mapper::Mapper::topologicallyCloseNodes	(	unsigned	g,
		const double	r
	)		const `[private]`

Nodes that are topologically close in slam graph.

Definition at line 564 of file mapper.cpp.

void laser_slam_mapper::Mapper::updateEdgeInfo	(	unsigned	g,
		unsigned	g2
	)		`[private]`

Postcondition:: An edge exists between g and g2 if they're close enough, and the transform on the edge is updated based on the current optimized poses

Definition at line 514 of file mapper.cpp.

void laser_slam_mapper::Mapper::updateEdgeInfo ( unsigned g ) [private]