graph_slam::GraphMapper Class Reference

Holds the state of the graph mapping algorithm, which runs in the background as long as this object exists. More...

#include <graph_mapper.h>

List of all members.

Public Member Functions

 GraphMapper ()
virtual ~GraphMapper ()
 Since this class can be subclassed.

Protected Member Functions

pose_graph::ConstraintGraph graphSnapshot () const
 Thread-safely get a copy of the current graph.

Protected Attributes

const bool add_new_nodes_
const double angle_threshold_
const std::string base_frame_
const ros::Duration constraint_wait_
warehouse::WarehouseClient db_
pose_graph::CachedNodeMap
< geometry_msgs::Pose > 
ff_poses_
const std::string fixed_frame_
pose_graph::ConstraintGraph graph_
bool initialized_
ros::Time last_node_addition_time_
pose_graph::NodePoseMap last_optimization_response_
std::set< unsigned > last_optimized_comp_
const ros::Duration loc_transform_wait_
LocalizationBuffer localizations_
const std::string optimization_algorithm_
const std::string optimization_frame_
bool optimize_flag_
ros::NodeHandle param_nh_
const double pos_threshold_
boost::optional
< tf::StampedTransform > 
ref_transform_
const ros::Duration ref_transform_timeout_
warehouse::Collection
< graph_mapping_msgs::ConstraintGraphMessage > 
saved_graph_
boost::shared_ptr
< tf::TransformListener > 
tf_
const ros::Duration update_duration_

Private Member Functions

void addConstraint (graph_mapping_msgs::GraphConstraint::ConstPtr constraint)
void addNodeAt (const ros::Time &t)
bool findNodeNear (const geometry_msgs::PoseStamped &l) const
tf::Pose fixedFramePoseAt (const ros::Time &t, const ros::Duration &wait) const
bool getPoses (graph_mapping_msgs::GetPoses::Request &req, graph_mapping_msgs::GetPoses::Response &resp)
virtual boost::optional
< ros::Time > 
newNodeTime ()
void optimize ()
void optimizeGraphComponent (unsigned n)
void publishRefTransform (const ros::TimerEvent &e)
void saveGraph ()
void saveGraphTimed (const ros::TimerEvent &e)
void updateGraph (const ros::TimerEvent &e)
void updateLocalization (graph_mapping_msgs::LocalizationDistribution::ConstPtr l)
void visualize (const ros::TimerEvent &e) const

Private Attributes

ros::Subscriber constraint_sub_
pose_graph::DiffPublisher diff_pub_
ros::ServiceServer get_poses_srv_
ros::Subscriber loc_sub_
ros::Publisher marker_pub_
boost::mutex mutex_
ros::NodeHandle nh_
ros::Publisher pose_pub_
ros::Timer ref_pub_timer_
ros::Timer save_timer_
tf::TransformBroadcaster tfb_
boost::scoped_ptr< Odometerupdate_odometer_
ros::Timer update_timer_
ros::Timer vis_timer_
pose_graph::ConstraintGraphVisualizer visualizer_

Detailed Description

Holds the state of the graph mapping algorithm, which runs in the background as long as this object exists.

Definition at line 58 of file graph_mapper.h.


Constructor & Destructor Documentation

graph_slam::GraphMapper::GraphMapper (  ) 

Constructing an instance of this class starts the graph slam algorithm. The constructor starts up timers for graph update and visualization, service callbacks for constraints and localization, and a service callback for saving the graph, and returns.

Topics and services are assumed to be in the default namespace, while parameters are looked up in the private namespace.

Assuming at least one saved graph exists, get the most recent one

Definition at line 80 of file graph_mapper.cpp.

graph_slam::GraphMapper::~GraphMapper (  )  [virtual]

Since this class can be subclassed.

Definition at line 138 of file graph_mapper.cpp.


Member Function Documentation

void graph_slam::GraphMapper::addConstraint ( graph_mapping_msgs::GraphConstraint::ConstPtr  constraint  )  [private]
void graph_slam::GraphMapper::addNodeAt ( const ros::Time &  t  )  [private]

Definition at line 283 of file graph_mapper.cpp.

bool graph_slam::GraphMapper::findNodeNear ( const geometry_msgs::PoseStamped &  l  )  const [private]
tf::Pose graph_slam::GraphMapper::fixedFramePoseAt ( const ros::Time &  t,
const ros::Duration &  wait 
) const [private]

Definition at line 273 of file graph_mapper.cpp.

bool graph_slam::GraphMapper::getPoses ( graph_mapping_msgs::GetPoses::Request &  req,
graph_mapping_msgs::GetPoses::Response &  resp 
) [private]
pg::ConstraintGraph graph_slam::GraphMapper::graphSnapshot (  )  const [protected]

Thread-safely get a copy of the current graph.

Definition at line 529 of file graph_mapper.cpp.

MaybeTime graph_slam::GraphMapper::newNodeTime (  )  [private, virtual]

Return a recent time when a new node can be added. By default, returns ros::Time::now() Called while holding lock

Definition at line 378 of file graph_mapper.cpp.

void graph_slam::GraphMapper::optimize (  )  [private]

Definition at line 388 of file graph_mapper.cpp.

void graph_slam::GraphMapper::optimizeGraphComponent ( unsigned  n  )  [private]
Todo:
This calls optimizer while holding lock

Definition at line 402 of file graph_mapper.cpp.

void graph_slam::GraphMapper::publishRefTransform ( const ros::TimerEvent &  e  )  [private]

Definition at line 226 of file graph_mapper.cpp.

void graph_slam::GraphMapper::saveGraph (  )  [private]

Definition at line 522 of file graph_mapper.cpp.

void graph_slam::GraphMapper::saveGraphTimed ( const ros::TimerEvent &  e  )  [private]

Definition at line 516 of file graph_mapper.cpp.

void graph_slam::GraphMapper::updateGraph ( const ros::TimerEvent &  e  )  [private]

Also, optimize if necessary

Definition at line 322 of file graph_mapper.cpp.

void graph_slam::GraphMapper::updateLocalization ( graph_mapping_msgs::LocalizationDistribution::ConstPtr  l  )  [private]
void graph_slam::GraphMapper::visualize ( const ros::TimerEvent &  e  )  const [private]

Definition at line 146 of file graph_mapper.cpp.


Member Data Documentation

Definition at line 92 of file graph_mapper.h.

Definition at line 84 of file graph_mapper.h.

const std::string graph_slam::GraphMapper::base_frame_ [protected]

Definition at line 86 of file graph_mapper.h.

ros::Subscriber graph_slam::GraphMapper::constraint_sub_ [private]

Definition at line 162 of file graph_mapper.h.

const ros::Duration graph_slam::GraphMapper::constraint_wait_ [protected]

Definition at line 88 of file graph_mapper.h.

warehouse::WarehouseClient graph_slam::GraphMapper::db_ [protected]

Definition at line 78 of file graph_mapper.h.

pose_graph::DiffPublisher graph_slam::GraphMapper::diff_pub_ [private]

Definition at line 163 of file graph_mapper.h.

pose_graph::CachedNodeMap<geometry_msgs::Pose> graph_slam::GraphMapper::ff_poses_ [protected]

Definition at line 105 of file graph_mapper.h.

const std::string graph_slam::GraphMapper::fixed_frame_ [protected]

Definition at line 86 of file graph_mapper.h.

ros::ServiceServer graph_slam::GraphMapper::get_poses_srv_ [private]

Definition at line 165 of file graph_mapper.h.

pose_graph::ConstraintGraph graph_slam::GraphMapper::graph_ [protected]

Definition at line 99 of file graph_mapper.h.

Definition at line 101 of file graph_mapper.h.

Definition at line 103 of file graph_mapper.h.

pose_graph::NodePoseMap graph_slam::GraphMapper::last_optimization_response_ [protected]

Definition at line 108 of file graph_mapper.h.

std::set<unsigned> graph_slam::GraphMapper::last_optimized_comp_ [protected]

Definition at line 107 of file graph_mapper.h.

ros::Subscriber graph_slam::GraphMapper::loc_sub_ [private]

Definition at line 162 of file graph_mapper.h.

const ros::Duration graph_slam::GraphMapper::loc_transform_wait_ [protected]

Definition at line 90 of file graph_mapper.h.

Definition at line 100 of file graph_mapper.h.

ros::Publisher graph_slam::GraphMapper::marker_pub_ [private]

Definition at line 164 of file graph_mapper.h.

boost::mutex graph_slam::GraphMapper::mutex_ [mutable, private]

Definition at line 157 of file graph_mapper.h.

ros::NodeHandle graph_slam::GraphMapper::nh_ [private]

Definition at line 158 of file graph_mapper.h.

const std::string graph_slam::GraphMapper::optimization_algorithm_ [protected]

Definition at line 89 of file graph_mapper.h.

const std::string graph_slam::GraphMapper::optimization_frame_ [protected]

Definition at line 86 of file graph_mapper.h.

Definition at line 104 of file graph_mapper.h.

ros::NodeHandle graph_slam::GraphMapper::param_nh_ [protected]

Definition at line 77 of file graph_mapper.h.

const double graph_slam::GraphMapper::pos_threshold_ [protected]

Definition at line 85 of file graph_mapper.h.

ros::Publisher graph_slam::GraphMapper::pose_pub_ [private]

Definition at line 164 of file graph_mapper.h.

Definition at line 166 of file graph_mapper.h.

boost::optional<tf::StampedTransform> graph_slam::GraphMapper::ref_transform_ [protected]

Definition at line 102 of file graph_mapper.h.

const ros::Duration graph_slam::GraphMapper::ref_transform_timeout_ [protected]

Definition at line 91 of file graph_mapper.h.

Definition at line 166 of file graph_mapper.h.

warehouse::Collection<graph_mapping_msgs::ConstraintGraphMessage> graph_slam::GraphMapper::saved_graph_ [protected]

Definition at line 106 of file graph_mapper.h.

boost::shared_ptr<tf::TransformListener> graph_slam::GraphMapper::tf_ [protected]

Definition at line 98 of file graph_mapper.h.

tf::TransformBroadcaster graph_slam::GraphMapper::tfb_ [private]

Definition at line 159 of file graph_mapper.h.

const ros::Duration graph_slam::GraphMapper::update_duration_ [protected]

Definition at line 87 of file graph_mapper.h.

Definition at line 161 of file graph_mapper.h.

Definition at line 166 of file graph_mapper.h.

Definition at line 166 of file graph_mapper.h.

pose_graph::ConstraintGraphVisualizer graph_slam::GraphMapper::visualizer_ [private]

Definition at line 160 of file graph_mapper.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs


graph_slam
Author(s): Bhaskara Marthi
autogenerated on Fri Jan 11 09:40:49 2013