slam_toolbox_localization.cpp
Go to the documentation of this file.
1 /*
2  * slam_toolbox
3  * Copyright Work Modifications (c) 2019, Steve Macenski
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
20 
21 namespace slam_toolbox
22 {
23 
24 /*****************************************************************************/
26 : SlamToolbox(nh)
27 /*****************************************************************************/
28 {
30  localization_pose_sub_ = nh.subscribe("/initialpose", 1,
33  "clear_localization_buffer",
35 
36  std::string filename;
37  geometry_msgs::Pose2D pose;
38  bool dock = false;
39  if (shouldStartWithPoseGraph(filename, pose, dock))
40  {
41  slam_toolbox_msgs::DeserializePoseGraph::Request req;
42  slam_toolbox_msgs::DeserializePoseGraph::Response resp;
43  req.initial_pose = pose;
44  req.filename = filename;
45  req.match_type =
46  slam_toolbox_msgs::DeserializePoseGraph::Request::LOCALIZE_AT_POSE;
47  if (dock)
48  {
49  ROS_ERROR("LocalizationSlamToolbox: Starting localization "
50  "at first node (dock) is correctly not supported.");
51  }
52 
54  }
55 
56  // in localization mode, we cannot allow for interactive mode
58 
59  // in localization mode, disable map saver
60  map_saver_.reset();
61  return;
62 }
63 
64 /*****************************************************************************/
66  std_srvs::Empty::Request& req,
67  std_srvs::Empty::Response& resp)
68 /*****************************************************************************/
69 {
70  ROS_INFO("LocalizationSlamToolbox: Clearing localization buffer.");
71  smapper_->clearLocalizationBuffer();
72  return true;
73 }
74 
75 /*****************************************************************************/
77  slam_toolbox_msgs::SerializePoseGraph::Request& req,
78  slam_toolbox_msgs::SerializePoseGraph::Response& resp)
79 /*****************************************************************************/
80 {
81  ROS_FATAL("LocalizationSlamToolbox: Cannot call serialize map "
82  "in localization mode!");
83  return false;
84 }
85 
86 /*****************************************************************************/
88  slam_toolbox_msgs::DeserializePoseGraph::Request& req,
89  slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
90 /*****************************************************************************/
91 {
92  if (req.match_type != procType::LOCALIZE_AT_POSE)
93  {
94  ROS_ERROR("Requested a non-localization deserialization "
95  "in localization mode.");
96  return false;
97  }
99 }
100 
101 /*****************************************************************************/
103  const sensor_msgs::LaserScan::ConstPtr& scan)
104 /*****************************************************************************/
105 {
106  // no odom info
107  Pose2 pose;
108  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
109  {
110  return;
111  }
112 
113  // ensure the laser can be used
114  LaserRangeFinder* laser = getLaser(scan);
115 
116  if(!laser)
117  {
118  ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser"
119  " device for %s; discarding scan", scan->header.frame_id.c_str());
120  return;
121  }
122 
123  if (shouldProcessScan(scan, pose))
124  {
125  addScan(laser, scan, pose);
126  }
127 
128  return;
129 }
130 
131 /*****************************************************************************/
133  LaserRangeFinder* laser,
134  const sensor_msgs::LaserScan::ConstPtr& scan,
135  Pose2& karto_pose)
136 /*****************************************************************************/
137 {
138  boost::mutex::scoped_lock l(pose_mutex_);
139 
141  {
143  }
144 
145  LocalizedRangeScan* range_scan = getLocalizedRangeScan(
146  laser, scan, karto_pose);
147 
148  // Add the localized range scan to the smapper
149  boost::mutex::scoped_lock lock(smapper_mutex_);
150  bool processed = false, update_reprocessing_transform = false;
152  {
153  if (!process_near_pose_)
154  {
155  ROS_ERROR("Process near region called without a "
156  "valid region request. Ignoring scan.");
157  return nullptr;
158  }
159 
160  // set our position to the requested pose and process
161  range_scan->SetOdometricPose(*process_near_pose_);
162  range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
163  process_near_pose_.reset(nullptr);
164  processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan);
165 
166  // reset to localization mode
168  update_reprocessing_transform = true;
169  }
171  {
172  processed = smapper_->getMapper()->ProcessLocalization(range_scan);
173  update_reprocessing_transform = false;
174  }
175  else
176  {
177  ROS_FATAL("LocalizationSlamToolbox: "
178  "No valid processor type set! Exiting.");
179  exit(-1);
180  }
181 
182  // if successfully processed, create odom to map transformation
183  if(!processed)
184  {
185  delete range_scan;
186  range_scan = nullptr;
187  } else {
188  // compute our new transform
189  setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
190  scan->header.stamp, update_reprocessing_transform);
191  }
192 
193  return range_scan;
194 }
195 
196 /*****************************************************************************/
198  geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
199 /*****************************************************************************/
200 {
202  {
203  ROS_ERROR("LocalizePoseCallback: Cannot process localization command "
204  "if not in localization mode.");
205  return;
206  }
207 
208  boost::mutex::scoped_lock l(pose_mutex_);
209  if (process_near_pose_)
210  {
211  process_near_pose_.reset(new Pose2(msg->pose.pose.position.x,
212  msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation)));
213  }
214  else
215  {
216  process_near_pose_ = std::make_unique<Pose2>(msg->pose.pose.position.x,
217  msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation));
218  }
219 
220  first_measurement_ = true;
221 
222  smapper_->clearLocalizationBuffer();
223 
224  ROS_INFO("LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f",
225  msg->pose.pose.position.x, msg->pose.pose.position.y,
226  tf2::getYaw(msg->pose.pose.orientation));
227  return;
228 }
229 
230 } // end namespace
filename
#define ROS_FATAL(...)
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp) override final
bool clearLocalizationBuffer(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
std::unique_ptr< map_saver::MapSaver > map_saver_
tf2::Stamped< tf2::Transform > setTransformFromPoses(const karto::Pose2 &pose, const karto::Pose2 &karto_pose, const ros::Time &t, const bool &update_reprocessing_transform)
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::unique_ptr< karto::Pose2 > process_near_pose_
karto::LocalizedRangeScan * getLocalizedRangeScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::unique_ptr< mapper_utils::SMapper > smapper_
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
#define ROS_WARN_THROTTLE(period,...)
#define ROS_INFO(...)
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request &req, slam_toolbox_msgs::SerializePoseGraph::Response &resp) override final
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan) override final
void localizePoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
virtual LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose) override final
double getYaw(const A &a)
bool shouldStartWithPoseGraph(std::string &filename, geometry_msgs::Pose2D &pose, bool &start_at_dock)
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
#define ROS_ERROR(...)


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