Classes | Public Types | Public Member Functions | Private Member Functions | Private Attributes
mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T > Class Template Reference

mrpt::graphslam::CGraphSlamEngine derived class for executing multi-robot graphSLAM More...

#include <CGraphSlamEngine_MR.h>

Inheritance diagram for mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >:
Inheritance graph
[legend]

List of all members.

Classes

struct  TNeighborAgentProps
 Struct responsible for holding properties (nodeIDs, node positions, LaserScans) that have been registered by a nearby GraphSlamAgent. More...
struct  TOptions

Public Types

typedef GRAPH_T::constraint_t constraint_t
typedef
mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR
< GRAPH_T > 
edge_reg_mr_t
typedef GRAPH_T::global_pose_t global_pose_t
typedef
mrpt::graphs::detail::THypothesis
< GRAPH_T > 
hypot_t
typedef std::vector< hypot_thypots_t
typedef std::vector< hypot_t * > hypotsp_t
typedef std::pair
< mrpt::utils::TNodeID,
mrpt::obs::CObservation2DRangeScanPtr > 
MRPT_NodeIDWithLaserScan
typedef std::vector
< TNeighborAgentProps * > 
neighbors_t
typedef
mrpt::graphslam::detail::TNodeProps
< GRAPH_T > 
node_props_t
typedef std::map
< mrpt::utils::TNodeID,
mrpt::obs::CObservation2DRangeScanPtr > 
nodes_to_scans2D_t
typedef CGraphSlamEngine_ROS
< GRAPH_T > 
parent_t
typedef std::vector
< mrpt::vector_uint > 
partitions_t
typedef
mrpt::graphslam::TUncertaintyPath
< GRAPH_T > 
path_t
typedef std::vector< path_tpaths_t
typedef constraint_t::type_value pose_t
typedef CGraphSlamEngine_MR
< GRAPH_T > 
self_t

Public Member Functions

bool _execGraphSlamStep (mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &observation, size_t &rawlog_entry)
 CGraphSlamEngine_MR (ros::NodeHandle *nh, const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager *win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=NULL)
const neighbors_tgetVecOfNeighborAgentProps () const
void initClass ()
 Initialize object instance.
bool isOwnNodeID (const mrpt::utils::TNodeID nodeID, const global_pose_t *pose_out=NULL) const
 Return true if current CGraphSlamEngine_MR object initially registered this nodeID, false otherwise.
 ~CGraphSlamEngine_MR ()

Private Member Functions

bool addNodeBatchesFromAllNeighbors ()
 Traverse all neighbors for which I know the inter-graph transformation, run addNodeBatchFromNeighbor.
bool addNodeBatchFromNeighbor (TNeighborAgentProps *neighbor)
 neighbors for which I know the inter-graph transformation, add new batches of fetches nodeIDs and scans
bool findTFsWithAllNeighbors ()
 Using map-merging find a potentuial transofrmation between own graph map to another agent's map and using that transformation add other agent's nodes to own graph.
bool findTFWithNeighbor (TNeighborAgentProps *neighbor)
 Method is used only when I haven't found any transformation between mine and the current neighbor's graph. When I do find one, I just add every new nodeID + LS directly in own graph since I can now connect the new ones with the already integrated nodes of the neighbor.
void getAllOwnNodes (std::set< mrpt::utils::TNodeID > *nodes_set) const
 Fill given set with the nodeIDs that were initially registered by the current graphSLAM engine (and not by any neighboring agent.
bool getCMGraph (mrpt_msgs::GetCMGraph::Request &req, mrpt_msgs::GetCMGraph::Response &res)
 Compute and fill the Condensed Measurements Graph.
mrpt::poses::CPose3D getLSPoseForGridMapVisualization (const mrpt::utils::TNodeID nodeID) const
bool getNeighborByAgentID (const std::string &agent_ID_str, TNeighborAgentProps *&neighbor) const
 Fill the TNeighborAgentProps instance based on the given agent_ID_str string that is provided.
void getNodeIDsOfEstimatedTrajectory (std::set< mrpt::utils::TNodeID > *nodes_set) const
void getRobotEstimatedTrajectory (typename GRAPH_T::global_poses_t *graph_poses) const
void monitorNodeRegistration (bool registered=false, std::string class_name="Class")
 Overriden method that takes in account registration of multiple nodes of other running graphSLAM agents.
void printParams () const
bool pubLastRegdIDScan ()
 Update the last registered NodeID and LaserScan of the current agent.
bool pubUpdatedNodesList ()
 Update the last registered NodeIDs and corresponding positions of the current agent.
void readParams ()
 Read the problem configuration parameters.
void readROSParameters ()
 Read configuration parameters from the ROS parameter server.
void setObjectPropsFromNodeID (const mrpt::utils::TNodeID nodeID, mrpt::opengl::CSetOfObjectsPtr &viz_object)
void setupPubs ()
void setupSrvs ()
void setupSubs ()
void usePublishersBroadcasters ()
 Provide feedback about the SLAM operation.

Private Attributes

ros::AsyncSpinner cm_graph_async_spinner
 AsyncSpinner that is used to query the CM-Graph service in case a new request arrives.
mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options
 Parameters used during the alignment operation.
mrpt::graphslam::detail::CConnectionManager m_conn_manager
 CConnectionManager instance.
size_t m_graph_nodes_last_size
 Last known size of the m_nodes map.
mrpt::utils::TColorManager m_neighbor_colors_manager
std::map< TNeighborAgentProps
*, bool > 
m_neighbor_to_found_initial_tf
 Map from neighbor instance to transformation from own to neighbor's graph.
neighbors_t m_neighbors
 GraphSlamAgent instance pointer to TNeighborAgentProps.
ros::NodeHandlem_nh
 NodeHandle pointer - inherited by the parent. Redefined here for convenience.
size_t m_nodes_to_laser_scans2D_last_size
 Last known size of the m_nodes_to_laser_scans2D map.
mrpt::graphslam::CGraphSlamEngine_MR::TOptions m_opts
bool m_pause_exec_on_mr_registration
 Indicates if the program is going to pause on multiple-robot nodes registration.
bool m_registered_multiple_nodes
 Indicates whether multiple nodes were just registered. Used for checking correct node registration in the monitorNodeRgistration method.
std::string m_sec_alignment_params
std::string m_sec_mr_slam_params
Subscribers - Publishers

ROS Topic Subscriber/Publisher/Service instances

ros::Publisher m_list_neighbors_pub
ros::Publisher m_last_regd_id_scan_pub
 Publisher of the laserScan + the corresponding (last) registered node.
ros::Publisher m_last_regd_nodes_pub
 Publisher of the last registered nodeIDs and positions.
ros::ServiceServer m_cm_graph_srvserver
Topic Names

Names of the topics that the class instance subscribes or publishes to

std::string m_mr_ns
 Condensed Measurements topic namespace.
std::string m_list_neighbors_topic
 Name of topic at which we publish information about the agents that we can currently communicate with.
std::string m_last_regd_id_scan_topic
 Name of the topic that the last registered laser scan (+ corresponding nodeID) is published at.
std::string m_last_regd_nodes_topic
 Name of the topic that the last X registered nodes + positions are going to be published at.
std::string m_cm_graph_service
 Name of the server which is to be called when other agent wants to have a subgraph of certain nodes returned.
double m_offset_y_nrd
 Display the Deciders/Optimizers with which we are running as well as the namespace of the current agent.
double m_offset_y_erd
double m_offset_y_gso
double m_offset_y_namespace
int m_text_index_nrd
int m_text_index_erd
int m_text_index_gso
int m_text_index_namespace

Detailed Description

template<class GRAPH_T>
class mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >

mrpt::graphslam::CGraphSlamEngine derived class for executing multi-robot graphSLAM

Definition at line 50 of file CGraphSlamEngine_MR.h.


Member Typedef Documentation

template<class GRAPH_T>
typedef GRAPH_T::constraint_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::constraint_t

Definition at line 55 of file CGraphSlamEngine_MR.h.

Definition at line 71 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef GRAPH_T::global_pose_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::global_pose_t

Definition at line 67 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef mrpt::graphs::detail::THypothesis<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypot_t

Definition at line 64 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::vector<hypot_t> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypots_t

Definition at line 65 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::vector<hypot_t*> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::hypotsp_t

Definition at line 66 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::pair< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::MRPT_NodeIDWithLaserScan

Definition at line 59 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::vector<TNeighborAgentProps*> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::neighbors_t

Definition at line 272 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef mrpt::graphslam::detail::TNodeProps<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::node_props_t

Definition at line 68 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::nodes_to_scans2D_t

Definition at line 62 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef CGraphSlamEngine_ROS<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::parent_t

Definition at line 53 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::vector<mrpt::vector_uint> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::partitions_t

Definition at line 63 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::path_t

Definition at line 69 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef std::vector<path_t> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::paths_t

Definition at line 70 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef constraint_t::type_value mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pose_t

Definition at line 56 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
typedef CGraphSlamEngine_MR<GRAPH_T> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::self_t

Definition at line 54 of file CGraphSlamEngine_MR.h.


Constructor & Destructor Documentation

template<class GRAPH_T >
mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::CGraphSlamEngine_MR ( ros::NodeHandle nh,
const std::string &  config_file,
const std::string &  rawlog_fname = "",
const std::string &  fname_GT = "",
mrpt::graphslam::CWindowManager *  win_manager = NULL,
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *  node_reg = NULL,
mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *  edge_reg = NULL,
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *  optimizer = NULL 
)

Definition at line 15 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::~CGraphSlamEngine_MR ( )

Definition at line 48 of file CGraphSlamEngine_MR_impl.h.


Member Function Documentation

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::_execGraphSlamStep ( mrpt::obs::CActionCollectionPtr &  action,
mrpt::obs::CSensoryFramePtr &  observations,
mrpt::obs::CObservationPtr &  observation,
size_t &  rawlog_entry 
) [virtual]

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 435 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::addNodeBatchesFromAllNeighbors ( ) [private]

Traverse all neighbors for which I know the inter-graph transformation, run addNodeBatchFromNeighbor.

Definition at line 63 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::addNodeBatchFromNeighbor ( TNeighborAgentProps neighbor) [private]

neighbors for which I know the inter-graph transformation, add new batches of fetches nodeIDs and scans

Try to integrate the newly received nodeIDs and laser Scans in own graph as a batch

Note:
Batch integration (instead of individual nodeID + LaserScan every time a new one is received) is preferred since the neighbor is given time to optimize the node positions

Definition at line 84 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::findTFsWithAllNeighbors ( ) [private]

Using map-merging find a potentuial transofrmation between own graph map to another agent's map and using that transformation add other agent's nodes to own graph.

Method delegates task to the findMatchesWithNeighboor method.

Returns:
True on successful graph integration with any neighbor, false otherwise

Definition at line 207 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::findTFWithNeighbor ( TNeighborAgentProps neighbor) [private]

Method is used only when I haven't found any transformation between mine and the current neighbor's graph. When I do find one, I just add every new nodeID + LS directly in own graph since I can now connect the new ones with the already integrated nodes of the neighbor.

See also:
findTFsWithAllNeighbors

Definition at line 254 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getAllOwnNodes ( std::set< mrpt::utils::TNodeID > *  nodes_set) const [private]

Fill given set with the nodeIDs that were initially registered by the current graphSLAM engine (and not by any neighboring agent.

Definition at line 1008 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getCMGraph ( mrpt_msgs::GetCMGraph::Request &  req,
mrpt_msgs::GetCMGraph::Response &  res 
) [private]

Compute and fill the Condensed Measurements Graph.

Definition at line 917 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
mrpt::poses::CPose3D mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getLSPoseForGridMapVisualization ( const mrpt::utils::TNodeID  nodeID) const [private]

Definition at line 589 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getNeighborByAgentID ( const std::string &  agent_ID_str,
TNeighborAgentProps *&  neighbor 
) const [private]

Fill the TNeighborAgentProps instance based on the given agent_ID_str string that is provided.

Returns:
False if the agent_ID_str doesn't correspond to any registered TNeighborAgentProps, True otherwise

Definition at line 618 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getNodeIDsOfEstimatedTrajectory ( std::set< mrpt::utils::TNodeID > *  nodes_set) const [private]

Definition at line 1021 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getRobotEstimatedTrajectory ( typename GRAPH_T::global_poses_t *  graph_poses) const [private]

Definition at line 992 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T>
const neighbors_t& mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::getVecOfNeighborAgentProps ( ) const [inline]

Definition at line 274 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::initClass ( )

Initialize object instance.

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 471 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::isOwnNodeID ( const mrpt::utils::TNodeID  nodeID,
const global_pose_t pose_out = NULL 
) const

Return true if current CGraphSlamEngine_MR object initially registered this nodeID, false otherwise.

Parameters:
[in]nodeIDnodeID for which the query is made. runtime_error In case given nodeID isn't registered at all in the graph

Definition at line 807 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::monitorNodeRegistration ( bool  registered = false,
std::string  class_name = "Class" 
) [private]

Overriden method that takes in account registration of multiple nodes of other running graphSLAM agents.

Definition at line 973 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::printParams ( ) const [private]

Definition at line 865 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pubLastRegdIDScan ( ) [private]

Update the last registered NodeID and LaserScan of the current agent.

Note:
Method is automatically called from usePublishersBroadcasters, but it publishes to the corresponding topics only on new node/LS additions
Returns:
true on publication to corresponding topics, false otherwise
See also:
pubUpdatedNodesList

Definition at line 756 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::pubUpdatedNodesList ( ) [private]

Update the last registered NodeIDs and corresponding positions of the current agent.

Note:
Method is automatically called from usePublishersBroadcasters, but it publishes to the corresponding topics only on new node additions. Node positions may change due to optimization but we will be notified of this change anyway after a new node addition.
Returns:
true on publication to corresponding topics, false otherwise
See also:
pubLastRegdIDScan

Definition at line 698 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::readParams ( ) [private]

Read the problem configuration parameters.

See also:
readROSParameters, CGraphSlamEngine::loadParams

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 840 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::readROSParameters ( ) [private]

Read configuration parameters from the ROS parameter server.

Note:
Method is automatically called on object construction.
See also:
readParams, initClass

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 857 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setObjectPropsFromNodeID ( const mrpt::utils::TNodeID  nodeID,
mrpt::opengl::CSetOfObjectsPtr &  viz_object 
) [private]

Definition at line 946 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupPubs ( ) [private, virtual]

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 877 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupSrvs ( ) [private, virtual]

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 904 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::setupSubs ( ) [private, virtual]

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 874 of file CGraphSlamEngine_MR_impl.h.

template<class GRAPH_T >
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::usePublishersBroadcasters ( ) [private, virtual]

Provide feedback about the SLAM operation.

Method makes the necessary calls to all the publishers of the class and broadcaster instances

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 639 of file CGraphSlamEngine_MR_impl.h.


Member Data Documentation

template<class GRAPH_T>
ros::AsyncSpinner mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::cm_graph_async_spinner [private]

AsyncSpinner that is used to query the CM-Graph service in case a new request arrives.

Definition at line 495 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
mrpt::slam::CGridMapAligner::TConfigParams mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_alignment_options [private]

Parameters used during the alignment operation.

Definition at line 508 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_cm_graph_service [private]

Name of the server which is to be called when other agent wants to have a subgraph of certain nodes returned.

Definition at line 451 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
ros::ServiceServer mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_cm_graph_srvserver [private]

Definition at line 424 of file CGraphSlamEngine_MR.h.

CConnectionManager instance.

Definition at line 463 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
size_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_graph_nodes_last_size [private]

Last known size of the m_nodes map.

Definition at line 460 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_id_scan_pub [private]

Publisher of the laserScan + the corresponding (last) registered node.

Definition at line 416 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_id_scan_topic [private]

Name of the topic that the last registered laser scan (+ corresponding nodeID) is published at.

Definition at line 442 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_nodes_pub [private]

Publisher of the last registered nodeIDs and positions.

Note:
see m_num_last_regd_nodes variable for the exact number of published nodeID and position pairs.

Definition at line 422 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_last_regd_nodes_topic [private]

Name of the topic that the last X registered nodes + positions are going to be published at.

Definition at line 446 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
ros::Publisher mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_list_neighbors_pub [private]

Definition at line 413 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_list_neighbors_topic [private]

Name of topic at which we publish information about the agents that we can currently communicate with.

Definition at line 438 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_mr_ns [private]

Condensed Measurements topic namespace.

Definition at line 434 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
mrpt::utils::TColorManager mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbor_colors_manager [private]

Definition at line 510 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::map<TNeighborAgentProps*, bool> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbor_to_found_initial_tf [private]

Map from neighbor instance to transformation from own to neighbor's graph.

As long as the corresponding variable for a specific agent is False current engine keeps trying the map-matching proc to find a common transformation between own and neighbor graphs.

Definition at line 405 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
neighbors_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_neighbors [private]

GraphSlamAgent instance pointer to TNeighborAgentProps.

Note:
elements of vector should persist even if the neighbor is no longer in range since they contain previous laser scans.

Definition at line 398 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
ros::NodeHandle* mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_nh [private]

NodeHandle pointer - inherited by the parent. Redefined here for convenience.

Reimplemented from mrpt::graphslam::CGraphSlamEngine_ROS< GRAPH_T >.

Definition at line 469 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
size_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_nodes_to_laser_scans2D_last_size [private]

Last known size of the m_nodes_to_laser_scans2D map.

Definition at line 457 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
double mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_offset_y_erd [private]

Definition at line 476 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
double mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_offset_y_gso [private]

Definition at line 477 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
double mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_offset_y_namespace [private]

Definition at line 478 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
double mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_offset_y_nrd [private]

Display the Deciders/Optimizers with which we are running as well as the namespace of the current agent.

Definition at line 475 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_pause_exec_on_mr_registration [private]

Indicates if the program is going to pause on multiple-robot nodes registration.

Note:
For debugging reasons only,
Todo:
Consider having this as a macro that is to be disabled on release code

Definition at line 504 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_registered_multiple_nodes [private]

Indicates whether multiple nodes were just registered. Used for checking correct node registration in the monitorNodeRgistration method.

Definition at line 490 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_sec_alignment_params [private]

Definition at line 512 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_sec_mr_slam_params [private]

Definition at line 513 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
int mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_text_index_erd [private]

Definition at line 481 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
int mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_text_index_gso [private]

Definition at line 482 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
int mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_text_index_namespace [private]

Definition at line 483 of file CGraphSlamEngine_MR.h.

template<class GRAPH_T>
int mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::m_text_index_nrd [private]

Definition at line 480 of file CGraphSlamEngine_MR.h.


The documentation for this class was generated from the following files:


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Sun Sep 17 2017 03:02:04