MultiMapper.h
Go to the documentation of this file.
1 #ifndef MULTI_MAPPER_H
2 #define MULTI_MAPPER_H
3 
4 #include <ros/ros.h>
7 #include <nav_msgs/OccupancyGrid.h>
8 #include <nav_msgs/GetMap.h>
9 #include <geometry_msgs/PoseArray.h>
10 #include <geometry_msgs/PoseWithCovarianceStamped.h>
11 #include <nav2d_msgs/LocalizedScan.h>
13 
14 #include <OpenKarto/OpenKarto.h>
15 
16 #include <string>
17 #include <map>
18 
19 #define ST_WAITING_FOR_MAP 10
20 #define ST_LOCALIZING 20
21 #define ST_MAPPING 30
22 
24 {
25 public:
26  // Constructor & Destructor
27  MultiMapper();
28  ~MultiMapper();
29 
30  // Public methods
31  void receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr& scan);
32  void receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr& scan);
33  void receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose);
34  void sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose);
35  void onMessage(const void* sender, karto::MapperEventArguments& args);
36  bool getMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
37  void publishLoop();
38  void publishTransform();
39  void setScanSolver(karto::ScanSolver* scanSolver);
40 
41 private:
42  // Private methods
43  bool updateMap();
44  bool sendMap();
45  void setRobotPose(double x, double y, double yaw);
46  karto::LocalizedRangeScan* createFromRosMessage(const sensor_msgs::LaserScan& scan, const karto::Identifier& robot);
47 
48  // Particle filter to localize within received map
50 
51  // Everything related to ROS
56 
57  nav_msgs::OccupancyGrid mGridMap;
58 
69 
70  // Everything related to KARTO
73  std::map<int, karto::LaserRangeFinderPtr> mOtherLasers;
75 
76  // Parameters and Variables
77  int mRobotID; // Who am I?
78  double mMapResolution; // Resolution of published grid map.
79  double mRangeThreshold; // Maximum range of laser sensor. (All robots MUST use the same due to Karto-Mapper!)
80  double mMaxCovariance; // When to accept the result of the particle filter?
81  int mState; // What am I doing? (waiting, localizing, mapping)
82  int mMapUpdateRate; // Publish the map every # received updates.
83  bool mPublishPoseGraph; // Whether or not to publish the pose graph as marker-message.
84  int mNodesAdded; // Number of nodes added to the pose graph.
85  int mMinMapSize; // Minimum map size (# of nodes) needed for localization.
87 
88  // Frames and Topics
89  std::string mLaserFrame;
90  std::string mRobotFrame;
91  std::string mOdometryFrame;
92  std::string mOffsetFrame;
93  std::string mMapFrame;
94  std::string mLaserTopic;
95  std::string mMapTopic;
96  std::string mMapService;
97  std::string mScanInputTopic;
98  std::string mScanOutputTopic;
99 };
100 
101 #endif
bool getMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
nav_msgs::OccupancyGrid mGridMap
Definition: MultiMapper.h:57
void setScanSolver(karto::ScanSolver *scanSolver)
void sendLocalizedScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
ros::Subscriber mLaserSubscriber
Definition: MultiMapper.h:66
ros::Subscriber mScanSubscriber
Definition: MultiMapper.h:67
std::string mRobotFrame
Definition: MultiMapper.h:90
std::string mScanOutputTopic
Definition: MultiMapper.h:98
std::string mLaserTopic
Definition: MultiMapper.h:94
std::string mOffsetFrame
Definition: MultiMapper.h:92
SelfLocalizer * mSelfLocalizer
Definition: MultiMapper.h:49
bool mMapChanged
Definition: MultiMapper.h:74
void publishLoop()
bool sendMap()
std::string mMapTopic
Definition: MultiMapper.h:95
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::string mScanInputTopic
Definition: MultiMapper.h:97
ros::WallTime mLastMapUpdate
Definition: MultiMapper.h:86
void publishTransform()
karto::SmartPointer< karto::OpenMapper > mMapper
Definition: MultiMapper.h:72
ros::Publisher mVerticesPublisher
Definition: MultiMapper.h:62
karto::LaserRangeFinderPtr mLaser
Definition: MultiMapper.h:71
ros::Publisher mMapPublisher
Definition: MultiMapper.h:60
void receiveLaserScan(const sensor_msgs::LaserScan::ConstPtr &scan)
ros::Publisher mScanPublisher
Definition: MultiMapper.h:61
ros::Publisher mEdgesPublisher
Definition: MultiMapper.h:63
tf::TransformListener mTransformListener
Definition: MultiMapper.h:52
karto::LocalizedRangeScan * createFromRosMessage(const sensor_msgs::LaserScan &scan, const karto::Identifier &robot)
ros::Publisher mPosePublisher
Definition: MultiMapper.h:64
int mNodesAdded
Definition: MultiMapper.h:84
tf::Transform mOdometryOffset
Definition: MultiMapper.h:55
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::ServiceServer mMapServer
Definition: MultiMapper.h:59
void onMessage(const void *sender, karto::MapperEventArguments &args)
std::string mMapService
Definition: MultiMapper.h:96
int mMapUpdateRate
Definition: MultiMapper.h:82
std::string mLaserFrame
Definition: MultiMapper.h:89
ros::Subscriber mInitialPoseSubscriber
Definition: MultiMapper.h:68
void setRobotPose(double x, double y, double yaw)
std::map< int, karto::LaserRangeFinderPtr > mOtherLasers
Definition: MultiMapper.h:73
void receiveLocalizedScan(const nav2d_msgs::LocalizedScan::ConstPtr &scan)
std::string mMapFrame
Definition: MultiMapper.h:93
ros::Publisher mOtherRobotsPublisher
Definition: MultiMapper.h:65
tf::Transform mMapToOdometry
Definition: MultiMapper.h:54
double mRangeThreshold
Definition: MultiMapper.h:79
std::string mOdometryFrame
Definition: MultiMapper.h:91
bool mPublishPoseGraph
Definition: MultiMapper.h:83
tf::TransformBroadcaster mTransformBroadcaster
Definition: MultiMapper.h:53
int mMinMapSize
Definition: MultiMapper.h:85
void receiveInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose)
bool updateMap()
double mMapResolution
Definition: MultiMapper.h:78
double mMaxCovariance
Definition: MultiMapper.h:80


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24