map.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017-9, Ubiquity Robotics
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  *
26  * The views and conclusions contained in the software and documentation are
27  * those of the authors and should not be interpreted as representing official
28  * policies, either expressed or implied, of the FreeBSD Project.
29  *
30  */
31 #ifndef MAP_H
32 #define MAP_H
33 
34 #include <ros/ros.h>
40 #include <visualization_msgs/Marker.h>
41 
42 #include <opencv2/aruco.hpp>
43 #include <opencv2/calib3d.hpp>
44 #include <opencv2/highgui.hpp>
45 
46 #include <fiducial_msgs/FiducialMapEntry.h>
47 #include <fiducial_msgs/FiducialMapEntryArray.h>
48 
49 #include <list>
50 #include <string>
51 
52 #include <geometry_msgs/PoseWithCovarianceStamped.h>
53 #include <tf2/convert.h>
55 
56 #include <std_srvs/Empty.h>
57 #include <fiducial_slam/AddFiducial.h>
58 
60 
61 // An observation of a single fiducial in a single image
62 class Observation {
63 public:
64  int fid;
67 
69 
70  Observation(int fid, const tf2::Stamped<TransformWithVariance> &camFid);
71 };
72 
73 // A single fiducial that is in the map
74 class Fiducial {
75 public:
76  int id;
77  int numObs;
78  bool visible;
79  std::set<int> links; // Stores the IDs of connected fiducials
80 
83 
84  void update(const tf2::Stamped<TransformWithVariance> &newPose);
85 
86  Fiducial() {}
87 
88  Fiducial(int id, const tf2::Stamped<TransformWithVariance> &pose);
89 };
90 
91 // Class containing map data
92 class Map {
93 public:
96  std::unique_ptr<tf2_ros::TransformListener> listener;
97 
102 
105  bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
106  bool addFiducialCallback(fiducial_slam::AddFiducial::Request &req,
107  fiducial_slam::AddFiducial::Response &res);
108 
109  std::string mapFilename;
110  std::string mapFrame;
111  std::string odomFrame;
112  std::string cameraFrame;
113  std::string baseFrame;
117 
119  bool readOnly;
120  int frameNum;
123 
125  std::vector<double> covarianceDiagonal;
126 
127  bool havePose;
131  geometry_msgs::TransformStamped poseTf;
132 
133  std::map<int, Fiducial> fiducials;
135 
136  Map(ros::NodeHandle &nh);
137  void update();
138  void update(std::vector<Observation> &obs, const ros::Time &time);
139  void autoInit(const std::vector<Observation> &obs, const ros::Time &time);
140  int updatePose(std::vector<Observation> &obs, const ros::Time &time,
142  void updateMap(const std::vector<Observation> &obs, const ros::Time &time,
143  const tf2::Stamped<TransformWithVariance> &cameraPose);
144  void handleAddFiducial(const std::vector<Observation> &obs);
145 
146  bool loadMap();
147  bool loadMap(std::string filename);
148  bool saveMap();
149  bool saveMap(std::string filename);
150 
151  void publishTf();
152  void publishMap();
153  void publishMarker(Fiducial &fid);
154  void publishMarkers();
155  void drawLine(const tf2::Vector3 &p0, const tf2::Vector3 &p1);
156 
157  bool lookupTransform(const std::string &from, const std::string &to, const ros::Time &time,
158  tf2::Transform &T) const;
159 };
160 
161 #endif
ros::Time lastPublished
Definition: map.h:82
ros::Publisher cameraPosePub
Definition: map.h:101
double future_date_transforms
Definition: map.h:114
tf2::Stamped< TransformWithVariance > T_fidCam
Definition: map.h:65
std::string odomFrame
Definition: map.h:111
ros::ServiceServer clearSrv
Definition: map.h:103
bool publishPoseTf
Definition: map.h:129
tf2_ros::Buffer tfBuffer
Definition: map.h:95
std::string cameraFrame
Definition: map.h:112
int id
Definition: map.h:76
bool havePose
Definition: map.h:127
std::vector< double > covarianceDiagonal
Definition: map.h:125
std::unique_ptr< tf2_ros::TransformListener > listener
Definition: map.h:96
Definition: map.h:74
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
geometry_msgs::TransformStamped poseTf
Definition: map.h:131
bool isInitializingMap
Definition: map.h:118
ros::ServiceServer addSrv
Definition: map.h:104
bool visible
Definition: map.h:78
std::map< int, Fiducial > fiducials
Definition: map.h:133
std::set< int > links
Definition: map.h:79
std::string mapFrame
Definition: map.h:110
Definition: map.h:92
ros::Publisher mapPub
Definition: map.h:99
ros::Time tfPublishTime
Definition: map.h:130
float tfPublishInterval
Definition: map.h:128
int fid
Definition: map.h:64
ros::Publisher robotPosePub
Definition: map.h:100
int initialFrameNum
Definition: map.h:121
bool readOnly
Definition: map.h:119
int fiducialToAdd
Definition: map.h:134
bool overridePublishedCovariance
Definition: map.h:124
int originFid
Definition: map.h:122
Observation()
Definition: map.h:68
tf2_ros::TransformBroadcaster broadcaster
Definition: map.h:94
int frameNum
Definition: map.h:120
Fiducial()
Definition: map.h:86
tf2::Stamped< TransformWithVariance > pose
Definition: map.h:81
bool publish_6dof_pose
Definition: map.h:115
int numObs
Definition: map.h:77
double multiErrorThreshold
Definition: map.h:116
std::string mapFilename
Definition: map.h:109
tf2::Stamped< TransformWithVariance > T_camFid
Definition: map.h:66
ros::Publisher markerPub
Definition: map.h:98
std::string baseFrame
Definition: map.h:113


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Fri May 1 2020 04:04:05