Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

consistent handling of tf_prefix for all frames #43

Open
wants to merge 1 commit into
base: hydro-devel
Choose a base branch
from
Open

consistent handling of tf_prefix for all frames #43

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 22 additions & 17 deletions gmapping/src/slam_gmapping.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
*
*
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
Expand Down Expand Up @@ -35,7 +35,7 @@ written to a file using e.g.
@section topic ROS topics

Subscribes to (name/type):
- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner
- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner
- @b "/tf": odometry from the robot


Expand Down Expand Up @@ -168,9 +168,9 @@ void SlamGMapping::init()

got_first_scan_ = false;
got_map_ = false;





// Parameters used by our GMapping wrapper
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
Expand All @@ -181,13 +181,18 @@ void SlamGMapping::init()
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";

// update the frame with the proper tf_prefix
base_frame_ = tf_.resolve( base_frame_ );
odom_frame_ = tf_.resolve( odom_frame_ );
map_frame_ = tf_.resolve( map_frame_ );

private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);

double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);

// Parameters used by GMapping itself
maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper()
if(!private_nh_.getParam("minimumScore", minimum_score_))
Expand Down Expand Up @@ -246,7 +251,7 @@ void SlamGMapping::init()
lasamplerange_ = 0.005;
if(!private_nh_.getParam("lasamplestep", lasamplestep_))
lasamplestep_ = 0.005;

if(!private_nh_.getParam("tf_delay", tf_delay_))
tf_delay_ = transform_publish_period_;

Expand Down Expand Up @@ -274,10 +279,10 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);

rosbag::Bag bag;
bag.open(bag_fname, rosbag::bagmode::Read);

std::vector<std::string> topics;
topics.push_back(std::string("/tf"));
topics.push_back(scan_topic);
Expand Down Expand Up @@ -310,7 +315,7 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
s_queue.pop();
}
// ignoring un-timestamped tf data
// ignoring un-timestamped tf data
}

// Only process a scan if it has tf data
Expand Down Expand Up @@ -426,7 +431,7 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
e.what());
return false;
}

// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
if (fabs(fabs(up.z()) - 1) > 0.001)
{
Expand Down Expand Up @@ -539,7 +544,7 @@ SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoin
{
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;

if(scan.ranges.size() != gsp_laser_beam_count_)
return false;

Expand All @@ -558,7 +563,7 @@ SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoin
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
}
} else
} else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)
{
Expand Down Expand Up @@ -689,13 +694,13 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
}

GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;
center.y=(ymin_ + ymax_) / 2.0;

GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
delta_);

ROS_DEBUG("Trajectory tree:");
Expand Down Expand Up @@ -726,7 +731,7 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;

ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
xmin_, ymin_, xmax_, ymax_);

Expand Down Expand Up @@ -762,13 +767,13 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)

//make sure to set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
map_.map.header.frame_id = map_frame_;

sst_.publish(map_.map);
sstm_.publish(map_.map.info);
}

bool
bool
SlamGMapping::mapCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res)
{
Expand Down