00001 /* +---------------------------------------------------------------------------+ 00002 | Mobile Robot Programming Toolkit (MRPT) | 00003 | http://www.mrpt.org/ | 00004 | | 00005 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | 00006 | See: http://www.mrpt.org/Authors - All rights reserved. | 00007 | Released under BSD License. See details in http://www.mrpt.org/License | 00008 +---------------------------------------------------------------------------+ */ 00009 #pragma once 00010 00011 #include <ros/ros.h> 00012 #include <nav_msgs/OccupancyGrid.h> 00013 #include <nav_msgs/Path.h> 00014 #include <mrpt_bridge/map.h> 00015 #include <mrpt_msgs/GraphSlamAgents.h> 00016 #include <mrpt/utils/COutputLogger.h> 00017 #include <mrpt/maps/COccupancyGridMap2D.h> 00018 #include <mrpt/math/utils.h> 00019 00020 namespace mrpt { namespace graphslam { 00021 00027 struct TNeighborAgentMapProps 00028 { 00029 TNeighborAgentMapProps( 00030 mrpt::utils::COutputLogger* logger_in, 00031 const mrpt_msgs::GraphSlamAgent& agent_in, 00032 ros::NodeHandle* nh_in); 00033 00034 void readROSParameters(); 00035 void setupComm(); 00036 void setupSubs(); 00037 void setupPubs(); 00041 void updateGridMap( 00042 const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap); 00046 void updateRobotTrajectory( 00047 const nav_msgs::Path::ConstPtr& nav_robot_traj); 00048 00052 size_t getNodeCount() { return nav_robot_trajectory->poses.size(); } 00053 00054 mrpt::utils::COutputLogger* m_logger; 00055 ros::NodeHandle* nh; 00057 const mrpt_msgs::GraphSlamAgent& agent; 00059 nav_msgs::OccupancyGrid::ConstPtr nav_map; 00061 nav_msgs::Path::ConstPtr nav_robot_trajectory; 00065 ros::Subscriber map_sub; 00066 ros::Subscriber robot_trajectory_sub; 00071 std::string map_topic; 00072 std::string robot_trajectory_topic; 00074 size_t queue_size; 00075 bool has_init_class; 00076 bool has_setup_comm; 00077 00078 00079 }; // end of TNeighborAgentMapProps 00080 00081 } } // end of namespaces 00082