MultiMapper.h
Go to the documentation of this file.
00001 #ifndef MULTI_MAPPER_H
00002 #define MULTI_MAPPER_H
00003 
00004 #include <ros/ros.h>
00005 #include <tf/transform_listener.h>
00006 #include <tf/transform_broadcaster.h>
00007 #include <nav_msgs/OccupancyGrid.h>
00008 #include <nav_msgs/GetMap.h>
00009 #include <geometry_msgs/PoseArray.h>
00010 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00011 #include <nav2d_msgs/LocalizedScan.h>
00012 #include <nav2d_localizer/SelfLocalizer.h>
00013 
00014 #include <OpenKarto/OpenKarto.h>
00015 
00016 #include <string>
00017 #include <map>
00018 
00019 #define ST_WAITING_FOR_MAP  10
00020 #define ST_LOCALIZING       20
00021 #define ST_MAPPING          30
00022 
00023 class MultiMapper
00024 {
00025 public:
00026         // Constructor & Destructor
00027         MultiMapper();
00028         ~MultiMapper();
00029 
00030         // Public methods
00031         void receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan);
00032         void receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr& scan);
00033         void receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose);
00034         void sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose);
00035         void onMessage(const void* sender, karto::MapperEventArguments& args);
00036         bool getMap(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
00037         void publishLoop();
00038         void publishTransform();
00039         void setScanSolver(karto::ScanSolver* scanSolver);
00040 
00041 private:
00042         // Private methods
00043         bool updateMap();
00044         bool sendMap();
00045         void setRobotPose(double x, double y, double yaw);
00046 
00047         // Particle filter to localize within received map
00048         SelfLocalizer* mSelfLocalizer;
00049         
00050         // Everything related to ROS
00051         tf::TransformListener mTransformListener;
00052         tf::TransformBroadcaster mTransformBroadcaster;
00053         tf::Transform mMapToOdometry;
00054         tf::Transform mOdometryOffset;
00055 
00056         nav_msgs::OccupancyGrid mGridMap;
00057 
00058         ros::ServiceServer mMapServer;
00059         ros::Publisher mMapPublisher;
00060         ros::Publisher mScanPublisher;
00061         ros::Publisher mVerticesPublisher;
00062         ros::Publisher mEdgesPublisher;
00063         ros::Publisher mPosePublisher;
00064         ros::Publisher mOtherRobotsPublisher;
00065         ros::Subscriber mLaserSubscriber;
00066         ros::Subscriber mScanSubscriber;
00067         ros::Subscriber mInitialPoseSubscriber;
00068 
00069         // Everything related to KARTO
00070         karto::LaserRangeFinderPtr mLaser;
00071         karto::SmartPointer<karto::OpenMapper> mMapper;
00072         std::map<int, karto::LaserRangeFinderPtr> mOtherLasers;
00073         bool mMapChanged;
00074 
00075         // Parameters and Variables
00076         int mRobotID;               // Who am I?
00077         double mMapResolution;      // Resolution of published grid map.
00078         double mRangeThreshold;     // Maximum range of laser sensor. (All robots MUST use the same due to Karto-Mapper!)
00079         double mMaxCovariance;      // When to accept the result of the particle filter?
00080         int mState;                     // What am I doing? (waiting, localizing, mapping)
00081         int mMapUpdateRate;             // Publish the map every # received updates.
00082         bool mPublishPoseGraph;     // Whether or not to publish the pose graph as marker-message.
00083         int mNodesAdded;            // Number of nodes added to the pose graph.
00084         int mMinMapSize;            // Minimum map size (# of nodes) needed for localization.
00085         ros::WallTime mLastMapUpdate;
00086 
00087         // Frames and Topics
00088         std::string mLaserFrame;
00089         std::string mRobotFrame;
00090         std::string mOdometryFrame;
00091         std::string mOffsetFrame;
00092         std::string mMapFrame;
00093         std::string mLaserTopic;
00094         std::string mMapTopic;
00095         std::string mMapService;
00096         std::string mScanInputTopic;
00097         std::string mScanOutputTopic;
00098 };
00099 
00100 #endif


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Mon Oct 6 2014 02:44:17