slam_mapper.cpp
Go to the documentation of this file.
1 /*
2  * slam_mapper
3  * Copyright (c) 2018, Simbe Robotics
4  * Copyright (c) 2018, Steve Macenski
5  * Copyright (c) 2019, Samsung Research America
6  *
7  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
8  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
9  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
10  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
11  *
12  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
13  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
14  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
15  * CONDITIONS.
16  *
17  */
18 
19 /* Author: Steven Macenski */
20 
22 
23 namespace mapper_utils
24 {
25 
26 /*****************************************************************************/
28 /*****************************************************************************/
29 {
30  mapper_ = std::make_unique<karto::Mapper>();
31 }
32 
33 /*****************************************************************************/
35 /*****************************************************************************/
36 {
37  mapper_.reset();
38 }
39 
40 /*****************************************************************************/
42 /*****************************************************************************/
43 {
44  return mapper_.get();
45 }
46 
47 /*****************************************************************************/
49 /*****************************************************************************/
50 {
51  mapper_.reset(mapper);
52 }
53 
54 /*****************************************************************************/
56 /*****************************************************************************/
57 {
58  mapper_->ClearLocalizationBuffer();
59 }
60 
61 /*****************************************************************************/
63 /*****************************************************************************/
64 {
65  karto::OccupancyGrid* occ_grid = nullptr;
66  return karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(),
67  resolution);
68 }
69 
70 /*****************************************************************************/
72 /*****************************************************************************/
73 {
74  tf2::Transform new_pose;
75  new_pose.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
77  q.setRPY(0., 0., pose.GetHeading());
78  new_pose.setRotation(q);
79  return new_pose;
80 };
81 
82 /*****************************************************************************/
84 /*****************************************************************************/
85 {
86  karto::Pose2 transformed_pose;
87  transformed_pose.SetX(pose.getOrigin().x());
88  transformed_pose.SetY(pose.getOrigin().y());
89  transformed_pose.SetHeading(tf2::getYaw(pose.getRotation()));
90  return transformed_pose;
91 };
92 
93 /*****************************************************************************/
95 /*****************************************************************************/
96 {
97  bool use_scan_matching;
98  if(nh.getParam("use_scan_matching", use_scan_matching))
99  {
100  mapper_->setParamUseScanMatching(use_scan_matching);
101  }
102 
103  bool use_scan_barycenter;
104  if(nh.getParam("use_scan_barycenter", use_scan_barycenter))
105  {
106  mapper_->setParamUseScanBarycenter(use_scan_barycenter);
107  }
108 
109  double minimum_travel_distance = 0.5;
110  if(nh.getParam("minimum_travel_distance", minimum_travel_distance))
111  {
112  mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
113  }
114 
115  double minimum_travel_heading;
116  if(nh.getParam("minimum_travel_heading", minimum_travel_heading))
117  {
118  mapper_->setParamMinimumTravelHeading(minimum_travel_heading);
119  }
120 
121  int scan_buffer_size;
122  if(nh.getParam("scan_buffer_size", scan_buffer_size))
123  {
124  mapper_->setParamScanBufferSize(scan_buffer_size);
125  }
126 
127  double scan_buffer_maximum_scan_distance;
128  if(nh.getParam("scan_buffer_maximum_scan_distance",
129  scan_buffer_maximum_scan_distance))
130  {
131  mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);
132  }
133 
134  double link_match_minimum_response_fine;
135  if(nh.getParam("link_match_minimum_response_fine",
136  link_match_minimum_response_fine))
137  {
138  mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);
139  }
140 
141  double link_scan_maximum_distance;
142  if(nh.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
143  {
144  mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);
145  }
146 
147  double loop_search_maximum_distance;
148  if(nh.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
149  {
150  mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);
151  }
152 
153  bool do_loop_closing;
154  if(nh.getParam("do_loop_closing", do_loop_closing))
155  {
156  mapper_->setParamDoLoopClosing(do_loop_closing);
157  }
158 
159  int loop_match_minimum_chain_size;
160  if(nh.getParam("loop_match_minimum_chain_size",
161  loop_match_minimum_chain_size))
162  {
163  mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);
164  }
165 
166  double loop_match_maximum_variance_coarse;
167  if(nh.getParam("loop_match_maximum_variance_coarse",
168  loop_match_maximum_variance_coarse))
169  {
170  mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);
171  }
172 
173  double loop_match_minimum_response_coarse;
174  if(nh.getParam("loop_match_minimum_response_coarse",
175  loop_match_minimum_response_coarse))
176  {
177  mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);
178  }
179 
180  double loop_match_minimum_response_fine;
181  if(nh.getParam("loop_match_minimum_response_fine",
182  loop_match_minimum_response_fine))
183  {
184  mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);
185  }
186 
187  // Setting Correlation Parameters
188  double correlation_search_space_dimension;
189  if(nh.getParam("correlation_search_space_dimension",
190  correlation_search_space_dimension))
191  {
192  mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);
193  }
194 
195  double correlation_search_space_resolution;
196  if(nh.getParam("correlation_search_space_resolution",
197  correlation_search_space_resolution))
198  {
199  mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);
200  }
201 
202  double correlation_search_space_smear_deviation;
203  if(nh.getParam("correlation_search_space_smear_deviation",
204  correlation_search_space_smear_deviation))
205  {
206  mapper_->setParamCorrelationSearchSpaceSmearDeviation(
207  correlation_search_space_smear_deviation);
208  }
209 
210  // Setting Correlation Parameters, Loop Closure Parameters
211  double loop_search_space_dimension;
212  if(nh.getParam("loop_search_space_dimension", loop_search_space_dimension))
213  {
214  mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);
215  }
216 
217  double loop_search_space_resolution;
218  if(nh.getParam("loop_search_space_resolution", loop_search_space_resolution))
219  {
220  mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);
221  }
222 
223  double loop_search_space_smear_deviation;
224  if(nh.getParam("loop_search_space_smear_deviation",
225  loop_search_space_smear_deviation))
226  {
227  mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);
228  }
229 
230  // Setting Scan Matcher Parameters
231  double distance_variance_penalty;
232  if(nh.getParam("distance_variance_penalty", distance_variance_penalty))
233  {
234  mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);
235  }
236 
237  double angle_variance_penalty;
238  if(nh.getParam("angle_variance_penalty", angle_variance_penalty))
239  {
240  mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
241  }
242 
243  double fine_search_angle_offset;
244  if(nh.getParam("fine_search_angle_offset", fine_search_angle_offset))
245  {
246  mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
247  }
248 
249  double coarse_search_angle_offset;
250  if(nh.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
251  {
252  mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);
253  }
254 
255  double coarse_angle_resolution;
256  if(nh.getParam("coarse_angle_resolution", coarse_angle_resolution))
257  {
258  mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);
259  }
260 
261  double minimum_angle_penalty;
262  if(nh.getParam("minimum_angle_penalty", minimum_angle_penalty))
263  {
264  mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);
265  }
266 
267  double minimum_distance_penalty;
268  if(nh.getParam("minimum_distance_penalty", minimum_distance_penalty))
269  {
270  mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);
271  }
272 
273  bool use_response_expansion;
274  if(nh.getParam("use_response_expansion", use_response_expansion))
275  {
276  mapper_->setParamUseResponseExpansion(use_response_expansion);
277  }
278  return;
279 }
280 
281 /*****************************************************************************/
283 /*****************************************************************************/
284 {
285  mapper_->Reset();
286  return;
287 }
288 
289 } // end namespace
void configure(const ros::NodeHandle &nh)
Definition: slam_mapper.cpp:94
tf2::Transform toTfPose(const karto::Pose2 &pose) const
Definition: slam_mapper.cpp:71
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
karto::OccupancyGrid * getOccupancyGrid(const double &resolution)
Definition: slam_mapper.cpp:62
bool getParam(const std::string &key, std::string &s) const
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
void setMapper(karto::Mapper *mapper)
Definition: slam_mapper.cpp:48
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:6038
double getYaw(const A &a)
std::unique_ptr< karto::Mapper > mapper_
Definition: slam_mapper.hpp:62
karto::Pose2 toKartoPose(const tf2::Transform &pose) const
Definition: slam_mapper.cpp:83
void SetHeading(kt_double heading)
Definition: Karto.h:2160
void SetX(kt_double x)
Definition: Karto.h:2106
karto::Mapper * getMapper()
Definition: slam_mapper.cpp:41
void SetY(kt_double y)
Definition: Karto.h:2124


slam_toolbox
Author(s): Steve Macenski
autogenerated on Wed Mar 3 2021 03:49:20