slam_toolbox_sync.cpp
Go to the documentation of this file.
1 /*
2  * slam_toolbox
3  * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
4  * Copyright Work Modifications (c) 2019, Steve Macenski
5  *
6  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
7  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
8  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
9  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
10  *
11  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
12  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
13  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
14  * CONDITIONS.
15  *
16  */
17 
18 /* Author: Steven Macenski */
19 
21 
22 namespace slam_toolbox
23 {
24 
25 /*****************************************************************************/
27 : SlamToolbox(nh)
28 /*****************************************************************************/
29 {
30  ssClear_ = nh.advertiseService("clear_queue",
32 
33  threads_.push_back(std::make_unique<boost::thread>(
34  boost::bind(&SynchronousSlamToolbox::run, this)));
35 
37 }
38 
39 /*****************************************************************************/
41 /*****************************************************************************/
42 {
43  ros::Rate r(100);
44  while(ros::ok())
45  {
46  if (!q_.empty() && !isPaused(PROCESSING))
47  {
48  PosedScan scan_w_pose = q_.front();
49  q_.pop();
50 
51  if (q_.size() > 10)
52  {
53  ROS_WARN_THROTTLE(10., "Queue size has grown to: %i. "
54  "Recommend stopping until message is gone if online mapping.",
55  (int)q_.size());
56  }
57 
58  addScan(getLaser(scan_w_pose.scan), scan_w_pose);
59  continue;
60  }
61 
62  r.sleep();
63  }
64 }
65 
66 /*****************************************************************************/
68  const sensor_msgs::LaserScan::ConstPtr& scan)
69 /*****************************************************************************/
70 {
71  // no odom info
72  karto::Pose2 pose;
73  if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
74  {
75  return;
76  }
77 
78  // ensure the laser can be used
79  karto::LaserRangeFinder* laser = getLaser(scan);
80 
81  if(!laser)
82  {
83  ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser"
84  " device for %s; discarding scan", scan->header.frame_id.c_str());
85  return;
86  }
87 
88  // if sync and valid, add to queue
89  if (shouldProcessScan(scan, pose))
90  {
91  q_.push(PosedScan(scan, pose));
92  }
93 
94  return;
95 }
96 
97 /*****************************************************************************/
99  slam_toolbox_msgs::ClearQueue::Request& req,
100  slam_toolbox_msgs::ClearQueue::Response& resp)
101 /*****************************************************************************/
102 {
103  ROS_INFO("SynchronousSlamToolbox: Clearing all queued scans to add to map.");
104  while(!q_.empty())
105  {
106  q_.pop();
107  }
108  resp.status = true;
109  return true;
110 }
111 
112 /*****************************************************************************/
114  slam_toolbox_msgs::DeserializePoseGraph::Request& req,
115  slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
116 /*****************************************************************************/
117 {
118  if (req.match_type == procType::LOCALIZE_AT_POSE)
119  {
120  ROS_ERROR("Requested a localization deserialization "
121  "in non-localization mode.");
122  return false;
123  }
125 }
126 
127 } // end namespace
std::vector< std::unique_ptr< boost::thread > > threads_
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp) override final
bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request &req, slam_toolbox_msgs::ClearQueue::Response &resp)
bool isPaused(const PausedApplication &app)
karto::LaserRangeFinder * getLaser(const sensor_msgs::LaserScan::ConstPtr &scan)
bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr &scan, const karto::Pose2 &pose)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request &req, slam_toolbox_msgs::DeserializePoseGraph::Response &resp)
#define ROS_WARN_THROTTLE(period,...)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
virtual karto::LocalizedRangeScan * addScan(karto::LaserRangeFinder *laser, const sensor_msgs::LaserScan::ConstPtr &scan, karto::Pose2 &karto_pose)
bool sleep()
void loadPoseGraphByParams(ros::NodeHandle &nh)
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr &scan) override final
std::unique_ptr< pose_utils::GetPoseHelper > pose_helper_
r
#define ROS_ERROR(...)


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