Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef CGRAPHSLAMENGINE_MR_H
00011 #define CGRAPHSLAMENGINE_MR_H
00012
00013 #include <ros/callback_queue.h>
00014
00015 #include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h"
00016 #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h"
00017 #include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h"
00018 #include "mrpt_graphslam_2d/CConnectionManager.h"
00019 #include "mrpt_graphslam_2d/misc/common.h"
00020
00021 #include <mrpt_msgs/NodeIDWithLaserScan.h>
00022 #include <mrpt_msgs/NodeIDWithPose_vec.h>
00023 #include <mrpt_msgs/NetworkOfPoses.h>
00024 #include <mrpt_msgs/GetCMGraph.h>
00025 #include <mrpt_bridge/network_of_poses.h>
00026 #include <mrpt_bridge/laser_scan.h>
00027 #include <sensor_msgs/LaserScan.h>
00028 #include <std_msgs/String.h>
00029 #include <mrpt/poses/CPosePDFGaussian.h>
00030 #include <mrpt/poses/CPosePDFSOG.h>
00031
00032 #include <mrpt/math/utils.h>
00033 #include <mrpt/utils/TColorManager.h>
00034 #include <mrpt/system/os.h>
00035 #include <mrpt/slam/CGridMapAligner.h>
00036 #include <mrpt/graphs/TMRSlamNodeAnnotations.h>
00037 #include <mrpt/graphslam/misc/TUncertaintyPath.h>
00038 #include <mrpt/graphslam/misc/TNodeProps.h>
00039
00040 #include <set>
00041 #include <iterator>
00042 #include <algorithm>
00043
00044 namespace mrpt { namespace graphslam {
00045
00049 template<class GRAPH_T>
00050 class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS<GRAPH_T>
00051 {
00052 public:
00053 typedef CGraphSlamEngine_ROS<GRAPH_T> parent_t;
00054 typedef CGraphSlamEngine_MR<GRAPH_T> self_t;
00055 typedef typename GRAPH_T::constraint_t constraint_t;
00056 typedef typename constraint_t::type_value pose_t;
00057 typedef std::pair<
00058 mrpt::utils::TNodeID,
00059 mrpt::obs::CObservation2DRangeScanPtr> MRPT_NodeIDWithLaserScan;
00060 typedef std::map<
00061 mrpt::utils::TNodeID,
00062 mrpt::obs::CObservation2DRangeScanPtr> nodes_to_scans2D_t;
00063 typedef std::vector<mrpt::vector_uint> partitions_t;
00064 typedef typename mrpt::graphs::detail::THypothesis<GRAPH_T> hypot_t;
00065 typedef std::vector<hypot_t> hypots_t;
00066 typedef std::vector<hypot_t*> hypotsp_t;
00067 typedef typename GRAPH_T::global_pose_t global_pose_t;
00068 typedef typename mrpt::graphslam::detail::TNodeProps<GRAPH_T> node_props_t;
00069 typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> path_t;
00070 typedef std::vector<path_t> paths_t;
00071 typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR<GRAPH_T> edge_reg_mr_t;
00072
00073 CGraphSlamEngine_MR(
00074 ros::NodeHandle* nh,
00075 const std::string& config_file,
00076 const std::string& rawlog_fname="",
00077 const std::string& fname_GT="",
00078 mrpt::graphslam::CWindowManager* win_manager=NULL,
00079 mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg=NULL,
00080 mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg=NULL,
00081 mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer=NULL
00082 );
00083
00084 ~CGraphSlamEngine_MR();
00085
00086 bool _execGraphSlamStep(
00087 mrpt::obs::CActionCollectionPtr& action,
00088 mrpt::obs::CSensoryFramePtr& observations,
00089 mrpt::obs::CObservationPtr& observation,
00090 size_t& rawlog_entry);
00091
00092 void initClass();
00093
00098 struct TNeighborAgentProps {
00100 TNeighborAgentProps(
00101 CGraphSlamEngine_MR<GRAPH_T>& engine_in,
00102 const mrpt_msgs::GraphSlamAgent& agent_in);
00104 ~TNeighborAgentProps();
00105
00108 void setupComm();
00112 void setupSubs();
00115 void setupSrvs();
00121 void fetchUpdatedNodesList(
00122 const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes);
00124 void fetchLastRegdIDScan(
00125 const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan);
00142 void getCachedNodes(
00143 mrpt::vector_uint* nodeIDs=NULL,
00144 std::map<
00145 mrpt::utils::TNodeID,
00146 node_props_t>* nodes_params=NULL,
00147 bool only_unused=true) const;
00150 void fillOptPaths(
00151 const std::set<mrpt::utils::TNodeID>& nodeIDs,
00152 paths_t* opt_paths) const;
00156 void computeGridMap() const;
00157 const mrpt::maps::COccupancyGridMap2DPtr& getGridMap() const;
00160 void getGridMap(mrpt::maps::COccupancyGridMap2DPtr& map) const;
00168 bool hasNewData() const;
00169 std::string getAgentNs() const { return this->agent.topic_namespace.data; }
00170 void resetFlags() const;
00171 bool operator==(
00172 const TNeighborAgentProps& other) const {
00173 return (this->agent == other.agent);
00174 }
00175 bool operator<(
00176 const TNeighborAgentProps& other) const {
00177 return (this->agent < other.agent);
00178 }
00182 const sensor_msgs::LaserScan* getLaserScanByNodeID(
00183 const mrpt::utils::TNodeID nodeID) const;
00184
00189 const mrpt_msgs::GraphSlamAgent agent;
00198 bool hasNewNodesBatch(int new_batch_size);
00199
00204 void setTColor(const mrpt::utils::TColor& color_in) { color = color_in; }
00207 mrpt::utils::TColor color;
00208
00212 std::set<mrpt::utils::TNodeID> nodeIDs_set;
00214 typename GRAPH_T::global_poses_t poses;
00216 std::vector<mrpt_msgs::NodeIDWithLaserScan> ros_scans;
00221 std::map<mrpt::utils::TNodeID, bool> nodeID_to_is_integrated;
00226 ros::Subscriber last_regd_nodes_sub;
00227 ros::Subscriber last_regd_id_scan_sub;
00228
00229 ros::ServiceClient cm_graph_srvclient;
00234 CGraphSlamEngine_MR<GRAPH_T>& engine;
00240 std::string last_regd_nodes_topic;
00241 std::string last_regd_id_scan_topic;
00242
00243 std::string cm_graph_service;
00248 mutable mrpt::maps::COccupancyGridMap2DPtr gridmap_cached;
00249
00250 mutable bool has_new_nodes;
00251 mutable bool has_new_scans;
00252
00253 int m_queue_size;
00256 ros::NodeHandle* nh;
00257 bool has_setup_comm;
00258
00263 mrpt::poses::CPose2D tf_self_to_neighbor_first_integrated_pose;
00267 std::pair<
00268 mrpt::utils::TNodeID,
00269 mrpt::poses::CPose2D> last_integrated_pair_neighbor_frame;
00270
00271 };
00272 typedef std::vector<TNeighborAgentProps*> neighbors_t;
00273
00274 const neighbors_t& getVecOfNeighborAgentProps() const {
00275 return m_neighbors;
00276 }
00283 bool isOwnNodeID(
00284 const mrpt::utils::TNodeID nodeID,
00285 const global_pose_t* pose_out=NULL) const;
00286
00287
00288 private:
00292 bool addNodeBatchesFromAllNeighbors();
00303 bool addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor);
00313 bool findTFsWithAllNeighbors();
00322 bool findTFWithNeighbor(TNeighborAgentProps* neighbor);
00329 bool getNeighborByAgentID(
00330 const std::string& agent_ID_str,
00331 TNeighborAgentProps*& neighbor) const;
00343 bool pubUpdatedNodesList();
00353 bool pubLastRegdIDScan();
00354
00355 void usePublishersBroadcasters();
00356
00357 void setupSubs();
00358 void setupPubs();
00359 void setupSrvs();
00360
00363 bool getCMGraph(
00364 mrpt_msgs::GetCMGraph::Request& req,
00365 mrpt_msgs::GetCMGraph::Response& res);
00366
00367 void readParams();
00368 void readROSParameters();
00369 void printParams() const;
00370 mrpt::poses::CPose3D getLSPoseForGridMapVisualization(
00371 const mrpt::utils::TNodeID nodeID) const;
00372 void setObjectPropsFromNodeID(
00373 const mrpt::utils::TNodeID nodeID,
00374 mrpt::opengl::CSetOfObjectsPtr& viz_object);
00379 void monitorNodeRegistration(
00380 bool registered=false,
00381 std::string class_name="Class");
00385 void getAllOwnNodes(
00386 std::set<mrpt::utils::TNodeID>* nodes_set) const;
00387 void getNodeIDsOfEstimatedTrajectory(
00388 std::set<mrpt::utils::TNodeID>* nodes_set) const;
00389 void getRobotEstimatedTrajectory(
00390 typename GRAPH_T::global_poses_t* graph_poses) const;
00391
00392
00398 neighbors_t m_neighbors;
00405 std::map<TNeighborAgentProps*, bool> m_neighbor_to_found_initial_tf;
00406
00413 ros::Publisher m_list_neighbors_pub;
00416 ros::Publisher m_last_regd_id_scan_pub;
00422 ros::Publisher m_last_regd_nodes_pub;
00423
00424 ros::ServiceServer m_cm_graph_srvserver;
00434 std::string m_mr_ns;
00438 std::string m_list_neighbors_topic;
00442 std::string m_last_regd_id_scan_topic;
00446 std::string m_last_regd_nodes_topic;
00447
00451 std::string m_cm_graph_service;
00452
00457 size_t m_nodes_to_laser_scans2D_last_size;
00460 size_t m_graph_nodes_last_size;
00461
00463 mrpt::graphslam::detail::CConnectionManager m_conn_manager;
00464
00465
00469 ros::NodeHandle* m_nh;
00470
00475 double m_offset_y_nrd;
00476 double m_offset_y_erd;
00477 double m_offset_y_gso;
00478 double m_offset_y_namespace;
00479
00480 int m_text_index_nrd;
00481 int m_text_index_erd;
00482 int m_text_index_gso;
00483 int m_text_index_namespace;
00490 bool m_registered_multiple_nodes;
00491
00495 ros::AsyncSpinner cm_graph_async_spinner;
00496
00504 bool m_pause_exec_on_mr_registration;
00505
00508 mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;
00509
00510 mrpt::utils::TColorManager m_neighbor_colors_manager;
00511
00512 std::string m_sec_alignment_params;
00513 std::string m_sec_mr_slam_params;
00514
00515 struct TOptions : mrpt::utils::CLoadableOptions {
00516
00517 typedef self_t engine_mr_t;
00518
00519 TOptions(const engine_mr_t& engine_in);
00520 ~TOptions();
00521 void loadFromConfigFile(
00522 const mrpt::utils::CConfigFileBase& source,
00523 const std::string& section);
00524 void dumpToTextStream(mrpt::utils::CStream& out) const;
00525
00526
00534 bool conservative_find_initial_tfs_to_neighbors;
00538 int nodes_integration_batch_size;
00545 int num_last_regd_nodes;
00552 int inter_group_node_count_thresh;
00556 int inter_group_node_count_thresh_minadv;
00557
00559 const engine_mr_t& engine;
00560
00561 } m_opts;
00562
00563 };
00564
00565
00566 } }
00567
00568
00569 #include "mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h"
00570
00571 #endif