Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #pragma once
00011
00012 #include <ros/callback_queue.h>
00013
00014 #include "mrpt_graphslam_2d/CGraphSlamEngine_ROS.h"
00015 #include "mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h"
00016 #include "mrpt_graphslam_2d/interfaces/CEdgeRegistrationDecider_MR.h"
00017 #include "mrpt_graphslam_2d/CConnectionManager.h"
00018 #include "mrpt_graphslam_2d/misc/common.h"
00019
00020 #include <mrpt_msgs/NodeIDWithLaserScan.h>
00021 #include <mrpt_msgs/NodeIDWithPose_vec.h>
00022 #include <mrpt_msgs/NetworkOfPoses.h>
00023 #include <mrpt_msgs/GetCMGraph.h>
00024 #include <mrpt_bridge/network_of_poses.h>
00025 #include <mrpt_bridge/laser_scan.h>
00026 #include <sensor_msgs/LaserScan.h>
00027 #include <std_msgs/String.h>
00028 #include <mrpt/poses/CPosePDFGaussian.h>
00029 #include <mrpt/poses/CPosePDFSOG.h>
00030
00031 #include <mrpt/math/utils.h>
00032 #include <mrpt/system/os.h>
00033 #include <mrpt/slam/CGridMapAligner.h>
00034 #include <mrpt/graphs/TMRSlamNodeAnnotations.h>
00035 #include <mrpt/graphslam/misc/TUncertaintyPath.h>
00036 #include <mrpt/graphslam/misc/TNodeProps.h>
00037
00038 #include <mrpt/version.h>
00039 #if MRPT_VERSION>=0x199
00040 #include <mrpt/img/TColorManager.h>
00041 #include <mrpt/img/TColor.h>
00042 #include <mrpt/graphs/TNodeID.h>
00043 #include <mrpt/config/CConfigFileBase.h>
00044 #include <mrpt/system/COutputLogger.h>
00045 #include <mrpt/containers/stl_containers_utils.h>
00046 using namespace mrpt::img;
00047 using namespace mrpt::graphs;
00048 using namespace mrpt::config;
00049 using namespace mrpt::system;
00050 using namespace mrpt::containers;
00051 #else
00052 #include <mrpt/utils/TColorManager.h>
00053 using namespace mrpt::utils;
00054 #endif
00055
00056
00057 #include <set>
00058 #include <iterator>
00059 #include <algorithm>
00060
00061 namespace mrpt { namespace graphslam {
00062
00066 template<class GRAPH_T>
00067 class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS<GRAPH_T>
00068 {
00069 public:
00070 typedef CGraphSlamEngine_ROS<GRAPH_T> parent_t;
00071 typedef CGraphSlamEngine_MR<GRAPH_T> self_t;
00072 typedef typename GRAPH_T::constraint_t constraint_t;
00073 typedef typename constraint_t::type_value pose_t;
00074 typedef std::pair<
00075 TNodeID,
00076 mrpt::obs::CObservation2DRangeScan::Ptr> MRPT_NodeIDWithLaserScan;
00077 typedef std::map<
00078 TNodeID,
00079 mrpt::obs::CObservation2DRangeScan::Ptr> nodes_to_scans2D_t;
00080 typedef std::vector<std::vector<TNodeID>> partitions_t;
00081 typedef typename mrpt::graphs::detail::THypothesis<GRAPH_T> hypot_t;
00082 typedef std::vector<hypot_t> hypots_t;
00083 typedef std::vector<hypot_t*> hypotsp_t;
00084 typedef typename GRAPH_T::global_pose_t global_pose_t;
00085 typedef typename mrpt::graphslam::detail::TNodeProps<GRAPH_T> node_props_t;
00086 typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> path_t;
00087 typedef std::vector<path_t> paths_t;
00088 typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR<GRAPH_T> edge_reg_mr_t;
00089
00090 CGraphSlamEngine_MR(
00091 ros::NodeHandle* nh,
00092 const std::string& config_file,
00093 const std::string& rawlog_fname="",
00094 const std::string& fname_GT="",
00095 mrpt::graphslam::CWindowManager* win_manager=NULL,
00096 mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg=NULL,
00097 mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg=NULL,
00098 mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer=NULL
00099 );
00100
00101 ~CGraphSlamEngine_MR();
00102
00103 bool _execGraphSlamStep(
00104 mrpt::obs::CActionCollection::Ptr& action,
00105 mrpt::obs::CSensoryFrame::Ptr& observations,
00106 mrpt::obs::CObservation::Ptr& observation,
00107 size_t& rawlog_entry);
00108
00109 void initClass();
00110
00115 struct TNeighborAgentProps {
00117 TNeighborAgentProps(
00118 CGraphSlamEngine_MR<GRAPH_T>& engine_in,
00119 const mrpt_msgs::GraphSlamAgent& agent_in);
00121 ~TNeighborAgentProps();
00122
00125 void setupComm();
00129 void setupSubs();
00132 void setupSrvs();
00138 void fetchUpdatedNodesList(
00139 const mrpt_msgs::NodeIDWithPose_vec::ConstPtr& nodes);
00141 void fetchLastRegdIDScan(
00142 const mrpt_msgs::NodeIDWithLaserScan::ConstPtr& last_regd_id_scan);
00159 void getCachedNodes(
00160 std::vector<TNodeID>* nodeIDs=NULL,
00161 std::map<
00162 TNodeID,
00163 node_props_t>* nodes_params=NULL,
00164 bool only_unused=true) const;
00167 void fillOptPaths(
00168 const std::set<TNodeID>& nodeIDs,
00169 paths_t* opt_paths) const;
00173 void computeGridMap() const;
00174 const mrpt::maps::COccupancyGridMap2D::Ptr& getGridMap() const;
00177 void getGridMap(mrpt::maps::COccupancyGridMap2D::Ptr& map) const;
00185 bool hasNewData() const;
00186 std::string getAgentNs() const { return this->agent.topic_namespace.data; }
00187 void resetFlags() const;
00188 bool operator==(
00189 const TNeighborAgentProps& other) const {
00190 return (this->agent == other.agent);
00191 }
00192 bool operator<(
00193 const TNeighborAgentProps& other) const {
00194 return (this->agent < other.agent);
00195 }
00199 const sensor_msgs::LaserScan* getLaserScanByNodeID(
00200 const TNodeID nodeID) const;
00201
00203 CGraphSlamEngine_MR<GRAPH_T>& engine;
00204
00208 const mrpt_msgs::GraphSlamAgent agent;
00217 bool hasNewNodesBatch(int new_batch_size);
00218
00223 void setTColor(const TColor& color_in) { color = color_in; }
00226 TColor color;
00227
00231 std::set<TNodeID> nodeIDs_set;
00233 typename GRAPH_T::global_poses_t poses;
00235 std::vector<mrpt_msgs::NodeIDWithLaserScan> ros_scans;
00240 std::map<TNodeID, bool> nodeID_to_is_integrated;
00245 ros::Subscriber last_regd_nodes_sub;
00246 ros::Subscriber last_regd_id_scan_sub;
00247
00248 ros::ServiceClient cm_graph_srvclient;
00256 std::string last_regd_nodes_topic;
00257 std::string last_regd_id_scan_topic;
00258
00259 std::string cm_graph_service;
00264 mutable mrpt::maps::COccupancyGridMap2D::Ptr gridmap_cached;
00265
00266 mutable bool has_new_nodes;
00267 mutable bool has_new_scans;
00268
00269 int m_queue_size;
00271 ros::NodeHandle* nh;
00272 bool has_setup_comm;
00273
00278 mrpt::poses::CPose2D tf_self_to_neighbor_first_integrated_pose;
00282 std::pair<
00283 TNodeID,
00284 mrpt::poses::CPose2D> last_integrated_pair_neighbor_frame;
00285
00286 };
00287 typedef std::vector<TNeighborAgentProps*> neighbors_t;
00288
00289 const neighbors_t& getVecOfNeighborAgentProps() const {
00290 return m_neighbors;
00291 }
00298 bool isOwnNodeID(
00299 const TNodeID nodeID,
00300 const global_pose_t* pose_out=NULL) const;
00301
00302
00303 private:
00307 bool addNodeBatchesFromAllNeighbors();
00318 bool addNodeBatchFromNeighbor(TNeighborAgentProps* neighbor);
00328 bool findTFsWithAllNeighbors();
00337 bool findTFWithNeighbor(TNeighborAgentProps* neighbor);
00344 bool getNeighborByAgentID(
00345 const std::string& agent_ID_str,
00346 TNeighborAgentProps*& neighbor) const;
00358 bool pubUpdatedNodesList();
00368 bool pubLastRegdIDScan();
00369
00370 void usePublishersBroadcasters();
00371
00372 void setupSubs();
00373 void setupPubs();
00374 void setupSrvs();
00375
00378 bool getCMGraph(
00379 mrpt_msgs::GetCMGraph::Request& req,
00380 mrpt_msgs::GetCMGraph::Response& res);
00381
00382 void readParams();
00383 void readROSParameters();
00384 void printParams() const;
00385 mrpt::poses::CPose3D getLSPoseForGridMapVisualization(
00386 const TNodeID nodeID) const;
00387 void setObjectPropsFromNodeID(
00388 const TNodeID nodeID,
00389 mrpt::opengl::CSetOfObjects::Ptr& viz_object);
00394 void monitorNodeRegistration(
00395 bool registered=false,
00396 std::string class_name="Class");
00400 void getAllOwnNodes(
00401 std::set<TNodeID>* nodes_set) const;
00402 void getNodeIDsOfEstimatedTrajectory(
00403 std::set<TNodeID>* nodes_set) const;
00404 void getRobotEstimatedTrajectory(
00405 typename GRAPH_T::global_poses_t* graph_poses) const;
00406
00407
00413 neighbors_t m_neighbors;
00420 std::map<TNeighborAgentProps*, bool> m_neighbor_to_found_initial_tf;
00421
00428 ros::Publisher m_list_neighbors_pub;
00431 ros::Publisher m_last_regd_id_scan_pub;
00437 ros::Publisher m_last_regd_nodes_pub;
00438
00439 ros::ServiceServer m_cm_graph_srvserver;
00449 std::string m_mr_ns;
00453 std::string m_list_neighbors_topic;
00457 std::string m_last_regd_id_scan_topic;
00461 std::string m_last_regd_nodes_topic;
00462
00466 std::string m_cm_graph_service;
00467
00471 size_t m_nodes_to_laser_scans2D_last_size;
00472
00474 mrpt::graphslam::detail::CConnectionManager m_conn_manager;
00475
00476
00480 ros::NodeHandle* m_nh;
00482 size_t m_graph_nodes_last_size;
00483
00488 double m_offset_y_nrd;
00489 double m_offset_y_erd;
00490 double m_offset_y_gso;
00491 double m_offset_y_namespace;
00492
00493 int m_text_index_nrd;
00494 int m_text_index_erd;
00495 int m_text_index_gso;
00496 int m_text_index_namespace;
00503 bool m_registered_multiple_nodes;
00504
00508 ros::AsyncSpinner cm_graph_async_spinner;
00509
00512 mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options;
00513
00514 TColorManager m_neighbor_colors_manager;
00515
00516 std::string m_sec_alignment_params;
00517 std::string m_sec_mr_slam_params;
00518
00519 struct TOptions : CLoadableOptions {
00520
00521 typedef self_t engine_mr_t;
00522
00523 TOptions(const engine_mr_t& engine_in);
00524 ~TOptions();
00525 void loadFromConfigFile(
00526 const CConfigFileBase& source,
00527 const std::string& section);
00528 #if MRPT_VERSION>=0x199
00529 void dumpToTextStream(std::ostream& out) const;
00530 #else
00531 void dumpToTextStream(mrpt::utils::CStream& out) const;
00532 #endif
00533
00534
00542 bool conservative_find_initial_tfs_to_neighbors;
00546 int nodes_integration_batch_size;
00553 int num_last_regd_nodes;
00560 size_t inter_group_node_count_thresh;
00564 size_t inter_group_node_count_thresh_minadv;
00565
00567 const engine_mr_t& engine;
00568
00569 } m_opts;
00570
00571 };
00572
00573
00574 } }
00575
00576
00577 #include "mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h"
00578