laser_slam_mapper::Mapper Class Reference

List of all members.

Public Member Functions
	Mapper ()
	Initialize node.
Private Member Functions
void	addGrid (unsigned n)
nm::OccupancyGrid::ConstPtr	computeLocalGrid (unsigned g) const
	Compute local occupancy grid.
unsigned	coveredBy (unsigned n, const Grids &g, double r) const
unsigned	coveredBy (unsigned n, unsigned 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)
void	updateEdgeInfo (unsigned g, unsigned g2)
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
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_
wh::Collection< 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_
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_
wh::WarehouseClient	slam_db_
ros::ServiceServer	switch_grid_srv_
tf::TransformListener	tf_
tf::TransformBroadcaster	tfb_
tmap::TopologicalMap	tmap_
wh::WarehouseClient	topo_db_
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 88 of file mapper.cpp.

Constructor & Destructor Documentation

laser_slam_mapper::Mapper::Mapper ( )

Initialize node.

Definition at line 347 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 490 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 689 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.

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 466 of file mapper.cpp.

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 560 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 610 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 576 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 681 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 649 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 390 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 660 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 846 of file mapper.cpp.

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

Publish reference transforms over ros.

Definition at line 854 of file mapper.cpp.

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

Definition at line 865 of file mapper.cpp.

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

Save local grid to db.

Definition at line 822 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 633 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 553 of file mapper.cpp.

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

Postcondition:: Possible edges to all topologically close nodes to g have been updated

Definition at line 542 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 503 of file mapper.cpp.

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

Periodically update and publish graph.

Definition at line 404 of file mapper.cpp.

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

Visualize graph.

Definition at line 738 of file mapper.cpp.

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

Visualize the grid we're currently localized wrt.

Definition at line 745 of file mapper.cpp.

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

Visualize edges between grids of the topological map.