Skip to content

Commit 0f76690

Browse files
jsongCMUjsongCMU
committed
Laser frame no longer needs to be same as base frame (#2)
1 parent ebee42b commit 0f76690

File tree

10 files changed

+67
-25
lines changed

10 files changed

+67
-25
lines changed

‎slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ class LifelongSlamToolbox : public SlamToolbox
4242

4343
protected:
4444
virtual void laserCallback(
45-
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
45+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
4646
virtual bool deserializePoseGraphCallback(
4747
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
4848
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

‎slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ class AsynchronousSlamToolbox : public SlamToolbox
3333

3434
protected:
3535
virtual void laserCallback(
36-
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
36+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
3737
virtual bool deserializePoseGraphCallback(
3838
slam_toolbox_msgs::DeserializePoseGraph::Request& req,
3939
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

‎slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ class SlamToolbox
7070
void setROSInterfaces(ros::NodeHandle& node);
7171

7272
// callbacks
73-
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) = 0;
73+
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) = 0;
7474
bool mapCallback(nav_msgs::GetMap::Request& req,
7575
nav_msgs::GetMap::Response& res);
7676
virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request& req,
@@ -123,7 +123,7 @@ class SlamToolbox
123123
std::unique_ptr<karto::Dataset> dataset_;
124124
std::map<std::string, laser_utils::LaserMetadata> lasers_;
125125
std::map<std::string, tf2::Transform> m_map_to_odoms_;
126-
std::map<std::string, std::string> m_base_id_to_odom_id_;
126+
std::map<std::string, std::string> m_base_id_to_odom_id_, m_laser_id_to_base_id_;
127127

128128
// helpers
129129
std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>> laser_assistants_;
@@ -134,7 +134,7 @@ class SlamToolbox
134134

135135
// Internal state
136136
std::vector<std::unique_ptr<boost::thread> > threads_;
137-
boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_;
137+
boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_, laser_id_map_mutex_;
138138
PausedState state_;
139139
nav_msgs::GetMap::Response map_;
140140
ProcessType processor_type_;

‎slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ class LocalizationSlamToolbox : public SlamToolbox
3535

3636
protected:
3737
virtual void laserCallback(
38-
const sensor_msgs::LaserScan::ConstPtr& scan) override final;
38+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
3939
void localizePoseCallback(
4040
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
4141

‎slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ class SynchronousSlamToolbox : public SlamToolbox
3333
void run();
3434

3535
protected:
36-
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) override final;
36+
virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final;
3737
bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request& req, slam_toolbox_msgs::ClearQueue::Response& resp);
3838
virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req,
3939
slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final;

‎slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh)
6666

6767
/*****************************************************************************/
6868
void LifelongSlamToolbox::laserCallback(
69-
const sensor_msgs::LaserScan::ConstPtr& scan)
69+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
7070
/*****************************************************************************/
7171
{
7272
// no odom info on any pose helper

‎slam_toolbox/src/slam_toolbox_async.cpp‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,21 +32,31 @@ AsynchronousSlamToolbox::AsynchronousSlamToolbox(ros::NodeHandle& nh)
3232

3333
/*****************************************************************************/
3434
void AsynchronousSlamToolbox::laserCallback(
35-
const sensor_msgs::LaserScan::ConstPtr& scan)
35+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
3636
/*****************************************************************************/
3737
{
3838
// no odom info on any pose helper
3939
karto::Pose2 pose;
4040
bool found_odom = false;
4141
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
4242
{
43-
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
43+
// try compute odometry
44+
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
4445
if(found_odom)
4546
break;
4647
}
4748
if(!found_odom)
4849
return;
4950

51+
// Add new sensor to laser ID map, and to laser assistants
52+
{
53+
boost::mutex::scoped_lock l(laser_id_map_mutex_);
54+
if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end())
55+
{
56+
m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id;
57+
laser_assistants_[scan->header.frame_id] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frame_id);
58+
}
59+
}
5060
// ensure the laser can be used
5161
karto::LaserRangeFinder* laser = getLaser(scan);
5262

‎slam_toolbox/src/slam_toolbox_common.cpp‎

Lines changed: 34 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -46,16 +46,15 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
4646
ROS_FATAL("[RoboSAR:slam_toolbox_common:SlamToolbox] base_frames_.size() != odom_frames_.size()");
4747
assert(base_frames_.size() == laser_topics_.size());
4848
assert(base_frames_.size() == odom_frames_.size());
49-
// Create map to compute odom frame given base frame
49+
// Setup map: base frame to odom frame
5050
for(size_t idx = 0; idx < base_frames_.size(); idx++)
5151
{
5252
m_base_id_to_odom_id_[base_frames_[idx]] = odom_frames_[idx];
5353
}
54-
// Set up pose helpers and laser assistants for each robot
54+
// Set up pose helpers for each robot
5555
for(size_t idx = 0; idx < base_frames_.size(); idx++)
5656
{
5757
pose_helpers_.push_back(std::make_unique<pose_utils::GetPoseHelper>(tf_.get(), base_frames_[idx], odom_frames_[idx]));
58-
laser_assistants_[base_frames_[idx]] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frames_[idx]); // Assumes base frame = laser frame
5958
}
6059
scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
6160
map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
@@ -90,7 +89,6 @@ SlamToolbox::~SlamToolbox()
9089
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
9190
{
9291
pose_helpers_[idx].reset();
93-
// laser_assistants_.reset();
9492
}
9593
for(std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++)
9694
{
@@ -189,7 +187,7 @@ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node)
189187
ROS_INFO("Subscribing to scan: %s", laser_topics_[idx].c_str());
190188
scan_filter_subs_.push_back(std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, laser_topics_[idx], 5));
191189
scan_filters_.push_back(std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_subs_.back(), *tf_, odom_frames_[idx], 5, node));
192-
scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
190+
scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1, base_frames_[idx]));
193191
}
194192
}
195193

@@ -331,12 +329,19 @@ karto::LaserRangeFinder* SlamToolbox::getLaser(const
331329
sensor_msgs::LaserScan::ConstPtr& scan)
332330
/*****************************************************************************/
333331
{
332+
// Use laser scan ID to get base frame ID
334333
const std::string& frame = scan->header.frame_id;
335334
if(lasers_.find(frame) == lasers_.end())
336335
{
337336
try
338337
{
339-
lasers_[frame] = laser_assistants_[frame]->toLaserMetadata(*scan);
338+
std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::const_iterator it = laser_assistants_.find(frame);
339+
if(it == laser_assistants_.end())
340+
{
341+
ROS_ERROR("Failed to get requested laser assistant, aborting initialization (%s)", frame.c_str());
342+
return nullptr;
343+
}
344+
lasers_[frame] = it->second->toLaserMetadata(*scan);
340345
dataset_->Add(lasers_[frame].getLaser(), true);
341346
}
342347
catch (tf2::TransformException& e)
@@ -385,16 +390,27 @@ tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
385390
const bool& update_reprocessing_transform)
386391
/*****************************************************************************/
387392
{
388-
// Use base frame to look up odom frame
389-
std::string odom_frame = m_base_id_to_odom_id_[header.frame_id];
393+
tf2::Stamped<tf2::Transform> odom_to_map;
394+
// Use laser frame to look up base and odom frame
395+
std::string base_frame;
396+
{
397+
boost::mutex::scoped_lock l(laser_id_map_mutex_);
398+
std::map<std::string, std::string>::const_iterator it = m_laser_id_to_base_id_.find(header.frame_id);
399+
if(it == m_laser_id_to_base_id_.end())
400+
{
401+
ROS_ERROR("Requested laser frame ID not in map: %s", header.frame_id.c_str());
402+
return odom_to_map;
403+
}
404+
base_frame = it->second;
405+
}
406+
const std::string& odom_frame = m_base_id_to_odom_id_[base_frame];
390407
// Compute the map->odom transform
391408
const ros::Time& t = header.stamp;
392-
tf2::Stamped<tf2::Transform> odom_to_map;
393409
tf2::Quaternion q(0.,0.,0.,1.0);
394410
q.setRPY(0., 0., corrected_pose.GetHeading());
395411
tf2::Stamped<tf2::Transform> base_to_map(
396412
tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
397-
corrected_pose.GetY(), 0.0)).inverse(), t, header.frame_id); // Assumes base frame = laser frame
413+
corrected_pose.GetY(), 0.0)).inverse(), t, base_frame);
398414
try
399415
{
400416
geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
@@ -735,8 +751,14 @@ void SlamToolbox::loadSerializedPoseGraph(
735751
ROS_INFO("Got scan!");
736752
try
737753
{
738-
lasers_[scan->header.frame_id] =
739-
laser_assistants_[scan->header.frame_id]->toLaserMetadata(*scan);
754+
const std::string& frame = scan->header.frame_id;
755+
std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::const_iterator it = laser_assistants_.find(frame);
756+
if(it == laser_assistants_.end())
757+
{
758+
ROS_ERROR("Failed to get requested laser assistant, aborting continue mapping (%s)", frame.c_str());
759+
exit(-1);
760+
}
761+
lasers_[frame] = it->second->toLaserMetadata(*scan);
740762
break;
741763
}
742764
catch (tf2::TransformException& e)

‎slam_toolbox/src/slam_toolbox_localization.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ bool LocalizationSlamToolbox::deserializePoseGraphCallback(
101101

102102
/*****************************************************************************/
103103
void LocalizationSlamToolbox::laserCallback(
104-
const sensor_msgs::LaserScan::ConstPtr& scan)
104+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
105105
/*****************************************************************************/
106106
{
107107
// no odom info on any pose helper

‎slam_toolbox/src/slam_toolbox_sync.cpp‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,21 +65,31 @@ void SynchronousSlamToolbox::run()
6565

6666
/*****************************************************************************/
6767
void SynchronousSlamToolbox::laserCallback(
68-
const sensor_msgs::LaserScan::ConstPtr& scan)
68+
const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id)
6969
/*****************************************************************************/
7070
{
7171
// no odom info on any pose helper
7272
karto::Pose2 pose;
7373
bool found_odom = false;
7474
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
7575
{
76-
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
76+
// try compute odometry
77+
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id);
7778
if(found_odom)
7879
break;
7980
}
8081
if(!found_odom)
8182
return;
8283

84+
// Add new sensor to laser ID map, and to laser assistants
85+
{
86+
boost::mutex::scoped_lock l(laser_id_map_mutex_);
87+
if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end())
88+
{
89+
m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id;
90+
laser_assistants_[scan->header.frame_id] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frame_id);
91+
}
92+
}
8393
// ensure the laser can be used
8494
karto::LaserRangeFinder* laser = getLaser(scan);
8595

0 commit comments

Comments
 (0)