Struct responsible for holding properties (nodeIDs, node positions, LaserScans) that have been registered by a nearby GraphSlamAgent. More...
#include <CGraphSlamEngine_MR.h>
Public Member Functions | |
void | computeGridMap () const |
Using the fetched LaserScans and nodeID positions, compute the occupancy gridmap of the given neighbor. More... | |
void | fillOptPaths (const std::set< TNodeID > &nodeIDs, paths_t *opt_paths) const |
Fill the optimal paths for each combination of the given nodeIDs. More... | |
std::string | getAgentNs () const |
void | getCachedNodes (std::vector< TNodeID > *nodeIDs=NULL, std::map< TNodeID, node_props_t > *nodes_params=NULL, bool only_unused=true) const |
Return cached list of nodeIDs (with their corresponding poses, LaserScans) More... | |
const mrpt::maps::COccupancyGridMap2D::Ptr & | getGridMap () const |
void | getGridMap (mrpt::maps::COccupancyGridMap2D::Ptr &map) const |
Fill the given occupancy gridmap object with the current neighbor's grdmap. More... | |
const sensor_msgs::LaserScan * | getLaserScanByNodeID (const TNodeID nodeID) const |
bool | hasNewData () const |
Return True if there are new data (node positions and corresponding LaserScans available) More... | |
bool | hasNewNodesBatch (int new_batch_size) |
Decide whether there are enough new nodes + scans that have not been integrated in the graph yet. More... | |
bool | operator< (const TNeighborAgentProps &other) const |
bool | operator== (const TNeighborAgentProps &other) const |
void | resetFlags () const |
void | setTColor (const TColor &color_in) |
Set the color related to the current neighbor agent. More... | |
void | setupComm () |
Wrapper for calling setupSubs, setupSrvs. More... | |
void | setupSrvs () |
Setup necessary services for neighbor. More... | |
void | setupSubs () |
Setup the necessary subscribers for fetching nodes, laserScans for the current neighbor. More... | |
TNeighborAgentProps (CGraphSlamEngine_MR< GRAPH_T > &engine_in, const mrpt_msgs::GraphSlamAgent &agent_in) | |
Constructor. More... | |
~TNeighborAgentProps () | |
Destructor. More... | |
Subscriber callback methods | |
Methods to be called when data is received on the subscribed topics | |
void | fetchUpdatedNodesList (const mrpt_msgs::NodeIDWithPose_vec::ConstPtr &nodes) |
Update nodeIDs + corresponding estimated poses. More... | |
void | fetchLastRegdIDScan (const mrpt_msgs::NodeIDWithLaserScan::ConstPtr &last_regd_id_scan) |
Fill the LaserScan of the last registered nodeID. More... | |
Public Attributes | |
const mrpt_msgs::GraphSlamAgent | agent |
TColor | color |
Unique color of current TNeighborAgentProps instance. More... | |
CGraphSlamEngine_MR< GRAPH_T > & | engine |
mrpt::maps::COccupancyGridMap2D::Ptr | gridmap_cached |
Pointer to the gridmap object of the neighbor. More... | |
bool | has_new_nodes |
bool | has_new_scans |
bool | has_setup_comm |
std::pair< TNodeID, mrpt::poses::CPose2D > | last_integrated_pair_neighbor_frame |
Last nodeID/CPose2D of neighbor that was integrated - Pose is taken with regards to neighbor's frame of reference. More... | |
int | m_queue_size |
ros::NodeHandle * | nh |
NodeHandle passed by the calling CGraphSlamEngine_MR class. More... | |
mrpt::poses::CPose2D | tf_self_to_neighbor_first_integrated_pose |
CPose2D connecting own graph origin to neighbor first received and integrated node. If this pose is 0 then it is not yet computed. More... | |
Neighbor cached properties | |
std::set< TNodeID > | nodeIDs_set |
NodeIDs that I have received from this graphSLAM agent. More... | |
GRAPH_T::global_poses_t | poses |
Poses that I have received from this graphSLAM agent. More... | |
std::vector< mrpt_msgs::NodeIDWithLaserScan > | ros_scans |
ROS LaserScans that I have received from this graphSLAM agent. More... | |
std::map< TNodeID, bool > | nodeID_to_is_integrated |
Have I already integrated this node in my graph? More... | |
Subscriber/Services Instances | |
ros::Subscriber | last_regd_nodes_sub |
ros::Subscriber | last_regd_id_scan_sub |
ros::ServiceClient | cm_graph_srvclient |
Full topic names / service names | |
Names of the full topic paths that the neighbor publishes nodes, LaserScans at. | |
std::string | last_regd_nodes_topic |
std::string | last_regd_id_scan_topic |
std::string | cm_graph_service |
Struct responsible for holding properties (nodeIDs, node positions, LaserScans) that have been registered by a nearby GraphSlamAgent.
Definition at line 115 of file CGraphSlamEngine_MR.h.
mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::TNeighborAgentProps | ( | CGraphSlamEngine_MR< GRAPH_T > & | engine_in, |
const mrpt_msgs::GraphSlamAgent & | agent_in | ||
) |
Constructor.
Definition at line 1002 of file CGraphSlamEngine_MR_impl.h.
mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::~TNeighborAgentProps | ( | ) |
Destructor.
Definition at line 1035 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::computeGridMap | ( | ) | const |
Using the fetched LaserScans and nodeID positions, compute the occupancy gridmap of the given neighbor.
Definition at line 1313 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::fetchLastRegdIDScan | ( | const mrpt_msgs::NodeIDWithLaserScan::ConstPtr & | last_regd_id_scan | ) |
Fill the LaserScan of the last registered nodeID.
Definition at line 1137 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::fetchUpdatedNodesList | ( | const mrpt_msgs::NodeIDWithPose_vec::ConstPtr & | nodes | ) |
Update nodeIDs + corresponding estimated poses.
Definition at line 1073 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::fillOptPaths | ( | const std::set< TNodeID > & | nodeIDs, |
paths_t * | opt_paths | ||
) | const |
Fill the optimal paths for each combination of the given nodeIDs.
Definition at line 1274 of file CGraphSlamEngine_MR_impl.h.
|
inline |
Definition at line 186 of file CGraphSlamEngine_MR.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::getCachedNodes | ( | std::vector< TNodeID > * | nodeIDs = NULL , |
std::map< TNodeID, node_props_t > * | nodes_params = NULL , |
||
bool | only_unused = true |
||
) | const |
Return cached list of nodeIDs (with their corresponding poses, LaserScans)
[in] | only_unused | Include only the nodes that have not already been used in the current CGraphSlamEngine's graph |
[out] | nodeIDs | Pointer to vector of nodeIDs that are actually returned. This argument is redundant but may be convinient in case that just the nodeIDs are required |
[out] | node_params | Pointer to the map of nodeIDs Corresponding properties that is to be filled by the method |
Definition at line 1155 of file CGraphSlamEngine_MR_impl.h.
const mrpt::maps::COccupancyGridMap2D::Ptr & mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::getGridMap | ( | ) | const |
Definition at line 1346 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::getGridMap | ( | mrpt::maps::COccupancyGridMap2D::Ptr & | map | ) | const |
Fill the given occupancy gridmap object with the current neighbor's grdmap.
Definition at line 1357 of file CGraphSlamEngine_MR_impl.h.
const sensor_msgs::LaserScan * mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::getLaserScanByNodeID | ( | const TNodeID | nodeID | ) | const |
Utility method for fetching the ROS LaserScan that corresponds to a nodeID.
Definition at line 1253 of file CGraphSlamEngine_MR_impl.h.
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::hasNewData | ( | ) | const |
Return True if there are new data (node positions and corresponding LaserScans available)
Caller is responsible of setting the underlying has_new_* variables to false using the resetFlags method upon successful integration of (some of) the nodes
Definition at line 1371 of file CGraphSlamEngine_MR_impl.h.
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::hasNewNodesBatch | ( | int | new_batch_size | ) |
Decide whether there are enough new nodes + scans that have not been integrated in the graph yet.
Method takes in account only the nodeIDs that have a valid LaserScan
Definition at line 1235 of file CGraphSlamEngine_MR_impl.h.
|
inline |
Definition at line 192 of file CGraphSlamEngine_MR.h.
|
inline |
Definition at line 188 of file CGraphSlamEngine_MR.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::resetFlags | ( | ) | const |
Definition at line 1229 of file CGraphSlamEngine_MR_impl.h.
|
inline |
Set the color related to the current neighbor agent.
Handy in visualization operations
Definition at line 223 of file CGraphSlamEngine_MR.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::setupComm | ( | ) |
Wrapper for calling setupSubs, setupSrvs.
Definition at line 1038 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::setupSrvs | ( | ) |
Setup necessary services for neighbor.
Definition at line 1053 of file CGraphSlamEngine_MR_impl.h.
void mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::setupSubs | ( | ) |
Setup the necessary subscribers for fetching nodes, laserScans for the current neighbor.
Definition at line 1060 of file CGraphSlamEngine_MR_impl.h.
const mrpt_msgs::GraphSlamAgent mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::agent |
GraphSlamAgent instance of the neighbor.
Definition at line 208 of file CGraphSlamEngine_MR.h.
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::cm_graph_service |
Definition at line 259 of file CGraphSlamEngine_MR.h.
ros::ServiceClient mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::cm_graph_srvclient |
Definition at line 248 of file CGraphSlamEngine_MR.h.
TColor mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::color |
Unique color of current TNeighborAgentProps instance.
Definition at line 226 of file CGraphSlamEngine_MR.h.
CGraphSlamEngine_MR<GRAPH_T>& mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::engine |
Ref to the outer class.
Definition at line 203 of file CGraphSlamEngine_MR.h.
|
mutable |
Pointer to the gridmap object of the neighbor.
Definition at line 264 of file CGraphSlamEngine_MR.h.
|
mutable |
Definition at line 266 of file CGraphSlamEngine_MR.h.
|
mutable |
Definition at line 267 of file CGraphSlamEngine_MR.h.
bool mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::has_setup_comm |
Definition at line 272 of file CGraphSlamEngine_MR.h.
std::pair< TNodeID, mrpt::poses::CPose2D> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::last_integrated_pair_neighbor_frame |
Last nodeID/CPose2D of neighbor that was integrated - Pose is taken with regards to neighbor's frame of reference.
Definition at line 284 of file CGraphSlamEngine_MR.h.
ros::Subscriber mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::last_regd_id_scan_sub |
Definition at line 246 of file CGraphSlamEngine_MR.h.
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::last_regd_id_scan_topic |
Definition at line 257 of file CGraphSlamEngine_MR.h.
ros::Subscriber mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::last_regd_nodes_sub |
Definition at line 245 of file CGraphSlamEngine_MR.h.
std::string mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::last_regd_nodes_topic |
Definition at line 256 of file CGraphSlamEngine_MR.h.
int mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::m_queue_size |
Definition at line 269 of file CGraphSlamEngine_MR.h.
ros::NodeHandle* mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::nh |
NodeHandle passed by the calling CGraphSlamEngine_MR class.
Definition at line 271 of file CGraphSlamEngine_MR.h.
std::map<TNodeID, bool> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::nodeID_to_is_integrated |
Have I already integrated this node in my graph?
Definition at line 240 of file CGraphSlamEngine_MR.h.
std::set<TNodeID> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::nodeIDs_set |
NodeIDs that I have received from this graphSLAM agent.
Definition at line 231 of file CGraphSlamEngine_MR.h.
GRAPH_T::global_poses_t mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::poses |
Poses that I have received from this graphSLAM agent.
Definition at line 233 of file CGraphSlamEngine_MR.h.
std::vector<mrpt_msgs::NodeIDWithLaserScan> mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::ros_scans |
ROS LaserScans that I have received from this graphSLAM agent.
Definition at line 235 of file CGraphSlamEngine_MR.h.
mrpt::poses::CPose2D mrpt::graphslam::CGraphSlamEngine_MR< GRAPH_T >::TNeighborAgentProps::tf_self_to_neighbor_first_integrated_pose |
CPose2D connecting own graph origin to neighbor first received and integrated node. If this pose is 0 then it is not yet computed.
Definition at line 278 of file CGraphSlamEngine_MR.h.