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

Expose and use the proposed open_karto parameter, minimum time interval #11

Closed
wants to merge 2 commits into from
Closed

Expose and use the proposed open_karto parameter, minimum time interval #11

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3768989
Also set the time of the LocalizedRangeScan
spmaniato Aug 16, 2016
7727aaa
Add new ROS parameter, minimum_time_interval
spmaniato Aug 16, 2016
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
41 changes: 23 additions & 18 deletions src/slam_karto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,11 +164,15 @@ SlamKarto::SlamKarto() :
bool use_scan_matching;
if(private_nh_.getParam("use_scan_matching", use_scan_matching))
mapper_->setParamUseScanMatching(use_scan_matching);

bool use_scan_barycenter;
if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter))
mapper_->setParamUseScanBarycenter(use_scan_barycenter);

double minimum_time_interval;
if(private_nh_.getParam("minimum_time_interval", minimum_time_interval))
mapper_->setParamMinimumTimeInterval(minimum_time_interval);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if you would, @spmaniato , please add {}s to the if/else's in this file and do a full lint.


double minimum_travel_distance;
if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance))
mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
Expand Down Expand Up @@ -386,7 +390,7 @@ SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
// Create a laser range finder device and copy in data from the first
// scan
std::string name = scan->header.frame_id;
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
Expand Down Expand Up @@ -427,7 +431,7 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
}
double yaw = tf::getYaw(odom_pose.getRotation());

karto_pose =
karto_pose =
karto::Pose2(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
Expand Down Expand Up @@ -477,7 +481,7 @@ SlamKarto::publishGraphVisualization()

m.action = visualization_msgs::Marker::ADD;
uint id = 0;
for (uint i=0; i<graph.size()/2; i++)
for (uint i=0; i<graph.size()/2; i++)
{
m.id = id;
m.pose.position.x = graph[2*i];
Expand All @@ -504,7 +508,7 @@ SlamKarto::publishGraphVisualization()
}

m.action = visualization_msgs::Marker::DELETE;
for (; id < marker_count_; id++)
for (; id < marker_count_; id++)
{
m.id = id;
marray.markers.push_back(visualization_msgs::Marker(m));
Expand Down Expand Up @@ -537,14 +541,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
karto::Pose2 odom_pose;
if(addScan(laser, scan, odom_pose))
{
ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());

publishGraphVisualization();

if(!got_map_ ||
if(!got_map_ ||
(scan->header.stamp - last_map_update) > map_update_interval_)
{
if(updateMap())
Expand All @@ -562,7 +566,7 @@ SlamKarto::updateMap()
{
boost::mutex::scoped_lock(map_mutex_);

karto::OccupancyGrid* occ_grid =
karto::OccupancyGrid* occ_grid =
karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);

if(!occ_grid)
Expand All @@ -577,14 +581,14 @@ SlamKarto::updateMap()
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
}

// Translate to ROS format
kt_int32s width = occ_grid->GetWidth();
kt_int32s height = occ_grid->GetHeight();
karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();

if(map_.map.info.width != (unsigned int) width ||
if(map_.map.info.width != (unsigned int) width ||
map_.map.info.height != (unsigned int) height ||
map_.map.info.origin.position.x != offset.GetX() ||
map_.map.info.origin.position.y != offset.GetY())
Expand All @@ -598,7 +602,7 @@ SlamKarto::updateMap()

for (kt_int32s y=0; y<height; y++)
{
for (kt_int32s x=0; x<width; x++)
for (kt_int32s x=0; x<width; x++)
{
// Getting the value at position x,y
kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
Expand All @@ -620,7 +624,7 @@ SlamKarto::updateMap()
}
}
}

// Set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = map_frame_;
Expand All @@ -635,12 +639,12 @@ SlamKarto::updateMap()

bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
if(!getOdomPose(karto_pose, scan->header.stamp))
return false;

// Create a vector of doubles for karto
std::vector<kt_double> readings;

Expand All @@ -659,19 +663,20 @@ SlamKarto::addScan(karto::LaserRangeFinder* laser,
readings.push_back(*it);
}
}

// create localized range scan
karto::LocalizedRangeScan* range_scan =
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);
range_scan->SetOdometricPose(karto_pose);
range_scan->SetCorrectedPose(karto_pose);
range_scan->SetTime(scan->header.stamp.toSec());

// Add the localized range scan to the mapper
bool processed;
if((processed = mapper_->Process(range_scan)))
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;

karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();

// Compute the map->odom transform
Expand Down Expand Up @@ -703,7 +708,7 @@ SlamKarto::addScan(karto::LaserRangeFinder* laser,
return processed;
}

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