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  boost::mutex::scoped_lock lock(smapper_mutex_);
71  ROS_INFO("LocalizationSlamToolbox: Clearing localization buffer.");
72  smapper_->clearLocalizationBuffer();
73  return true;
74 }
75 
76 /*****************************************************************************/
78  slam_toolbox_msgs::SerializePoseGraph::Request& req,
79  slam_toolbox_msgs::SerializePoseGraph::Response& resp)
80 /*****************************************************************************/
81 {
82  ROS_FATAL("LocalizationSlamToolbox: Cannot call serialize map "
83  "in localization mode!");
84  return false;
85 }
86 
87 /*****************************************************************************/
89  slam_toolbox_msgs::DeserializePoseGraph::Request& req,
90  slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
91 /*****************************************************************************/
92 {
93  if (req.match_type != procType::LOCALIZE_AT_POSE)
94  {
95  ROS_ERROR("Requested a non-localization deserialization "
96  "in localization mode.");
97  return false;
98  }
100 }
101 
102 /*****************************************************************************/
104  const sensor_msgs::LaserScan::ConstPtr& scan)
105 /*****************************************************************************/
106 {
107  // no odom info
108  Pose2 pose;
109  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
110  {
111  return;
112  }
113 
114  // ensure the laser can be used
115  LaserRangeFinder* laser = getLaser(scan);
116 
117  if(!laser)
118  {
119  ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser"
120  " device for %s; discarding scan", scan->header.frame_id.c_str());
121  return;
122  }
123 
124  if (shouldProcessScan(scan, pose))
125  {
126  addScan(laser, scan, pose);
127  }
128 
129  return;
130 }
131 
132 /*****************************************************************************/
134  LaserRangeFinder* laser,
135  const sensor_msgs::LaserScan::ConstPtr& scan,
136  Pose2& karto_pose)
137 /*****************************************************************************/
138 {
139  boost::mutex::scoped_lock l(pose_mutex_);
140 
142  {
144  }
145 
146  LocalizedRangeScan* range_scan = getLocalizedRangeScan(
147  laser, scan, karto_pose);
148 
149  // Add the localized range scan to the smapper
150  boost::mutex::scoped_lock lock(smapper_mutex_);
151  bool processed = false, update_reprocessing_transform = false;
153  {
154  if (!process_near_pose_)
155  {
156  ROS_ERROR("Process near region called without a "
157  "valid region request. Ignoring scan.");
158  return nullptr;
159  }
160 
161  // set our position to the requested pose and process
162  range_scan->SetOdometricPose(*process_near_pose_);
163  range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
164  process_near_pose_.reset(nullptr);
165  processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true);
166 
167  // reset to localization mode
169  update_reprocessing_transform = true;
170  }
172  {
173  processed = smapper_->getMapper()->ProcessLocalization(range_scan);
174  update_reprocessing_transform = false;
175  }
176  else
177  {
178  ROS_FATAL("LocalizationSlamToolbox: "
179  "No valid processor type set! Exiting.");
180  exit(-1);
181  }
182 
183  // if successfully processed, create odom to map transformation
184  if(!processed)
185  {
186  delete range_scan;
187  range_scan = nullptr;
188  } else {
189  // compute our new transform
190  setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
191  scan->header.stamp, update_reprocessing_transform);
192  }
193 
194  return range_scan;
195 }
196 
197 /*****************************************************************************/
199  geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
200 /*****************************************************************************/
201 {
203  {
204  ROS_ERROR("LocalizePoseCallback: Cannot process localization command "
205  "if not in localization mode.");
206  return;
207  }
208 
209  boost::mutex::scoped_lock l(pose_mutex_);
210  if (process_near_pose_)
211  {
212  process_near_pose_.reset(new Pose2(msg->pose.pose.position.x,
213  msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation)));
214  }
215  else
216  {
217  process_near_pose_ = std::make_unique<Pose2>(msg->pose.pose.position.x,
218  msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation));
219  }
220 
221  first_measurement_ = true;
222 
223  boost::mutex::scoped_lock lock(smapper_mutex_);
224  smapper_->clearLocalizationBuffer();
225 
226  ROS_INFO("LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f",
227  msg->pose.pose.position.x, msg->pose.pose.position.y,
228  tf2::getYaw(msg->pose.pose.orientation));
229  return;
230 }
231 
232 } // end namespace
slam_toolbox::SlamToolbox::process_near_pose_
std::unique_ptr< karto::Pose2 > process_near_pose_
Definition: slam_toolbox_common.hpp:141
slam_toolbox::SlamToolbox
Definition: slam_toolbox_common.hpp:55
slam_toolbox::LocalizationSlamToolbox::clearLocalizationBuffer
bool clearLocalizationBuffer(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Definition: slam_toolbox_localization.cpp:65
slam_toolbox::SlamToolbox::getLaser
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
Definition: slam_toolbox_common.cpp:289
slam_toolbox_localization.hpp
slam_toolbox::SlamToolbox::getLocalizedRangeScan
karto::LocalizedRangeScan * getLocalizedRangeScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
Definition: slam_toolbox_common.cpp:391
tf2::getYaw
double getYaw(const A &a)
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
toolbox_types::PROCESS_NEAR_REGION
@ PROCESS_NEAR_REGION
Definition: toolbox_types.hpp:88
slam_toolbox::LocalizationSlamToolbox::deserializePoseGraphCallback
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp) override final
Definition: slam_toolbox_localization.cpp:88
slam_toolbox::SlamToolbox::shouldProcessScan
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
Definition: slam_toolbox_common.cpp:415
slam_toolbox::SlamToolbox::smapper_
std::unique_ptr< mapper_utils::SMapper > smapper_
Definition: slam_toolbox_common.hpp:123
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
slam_toolbox::SlamToolbox::deserializePoseGraphCallback
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
Definition: slam_toolbox_common.cpp:736
slam_toolbox::SlamToolbox::map_saver_
std::unique_ptr< map_saver::MapSaver > map_saver_
Definition: slam_toolbox_common.hpp:130
slam_toolbox::LocalizationSlamToolbox::serializePoseGraphCallback
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request &req, slam_toolbox_msgs::SerializePoseGraph::Response &resp) override final
Definition: slam_toolbox_localization.cpp:77
slam_toolbox::SlamToolbox::setTransformFromPoses
tf2::Stamped< tf2::Transform > setTransformFromPoses(const karto::Pose2 &pose, const karto::Pose2 &karto_pose, const ros::Time &t, const bool &update_reprocessing_transform)
Definition: slam_toolbox_common.cpp:340
slam_toolbox::LocalizationSlamToolbox::addScan
virtual LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose) override final
Definition: slam_toolbox_localization.cpp:133
slam_toolbox::LocalizationSlamToolbox::laserCallback
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan) override final
Definition: slam_toolbox_localization.cpp:103
slam_toolbox::LocalizationSlamToolbox::LocalizationSlamToolbox
LocalizationSlamToolbox(ros::NodeHandle &nh)
Definition: slam_toolbox_localization.cpp:25
slam_toolbox::SlamToolbox::enable_interactive_mode_
bool enable_interactive_mode_
Definition: slam_toolbox_common.hpp:120
slam_toolbox::LocalizationSlamToolbox::localization_pose_sub_
ros::Subscriber localization_pose_sub_
Definition: slam_toolbox_localization.hpp:56
slam_toolbox::SlamToolbox::smapper_mutex_
boost::mutex smapper_mutex_
Definition: slam_toolbox_common.hpp:137
slam_toolbox::LocalizationSlamToolbox::localizePoseCallback
void localizePoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
Definition: slam_toolbox_localization.cpp:198
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ROS_FATAL
#define ROS_FATAL(...)
slam_toolbox::SlamToolbox::shouldStartWithPoseGraph
bool shouldStartWithPoseGraph(std::string &filename, geometry_msgs::Pose2D &pose, bool &start_at_dock)
Definition: slam_toolbox_common.cpp:250
slam_toolbox::SlamToolbox::first_measurement_
bool first_measurement_
Definition: slam_toolbox_common.hpp:120
slam_toolbox::SlamToolbox::processor_type_
ProcessType processor_type_
Definition: slam_toolbox_common.hpp:140
ROS_ERROR
#define ROS_ERROR(...)
slam_toolbox::LocalizationSlamToolbox::clear_localization_
ros::ServiceServer clear_localization_
Definition: slam_toolbox_localization.hpp:57
slam_toolbox::SlamToolbox::pose_helper_
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
Definition: slam_toolbox_common.hpp:129
slam_toolbox
Definition: slam_toolbox_lifelong.hpp:24
process_constraints.filename
filename
Definition: process_constraints.py:114
ROS_INFO
#define ROS_INFO(...)
toolbox_types::PROCESS_LOCALIZATION
@ PROCESS_LOCALIZATION
Definition: toolbox_types.hpp:89
slam_toolbox::SlamToolbox::pose_mutex_
boost::mutex pose_mutex_
Definition: slam_toolbox_common.hpp:137
ros::NodeHandle


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:56