ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Ok, I think I figured it out. In the slam_gmapping.cpp file of the ROS wrapper go to line 592ff. There you see this code:

    ROS_DEBUG("Trajectory tree:");
    for(GMapping::GridSlamProcessor::TNode* n = best.node;
        n;
        n = n->parent)
    {
        ROS_DEBUG(" %.3f %.3f %.3f",
                  n->pose.x,
                  n->pose.y,
                  n->pose.theta);
        if(!n->reading)
        {
            ROS_DEBUG("Reading is NULL");
            continue;
        }
        matcher.invalidateActiveArea();
        matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
        matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
    }

Just after the line matcher.registerScan(smap, n->pose, &((*n->reading)[0])); add a single break;. That way only the last node in the trajectory tree that tracks your robots position is considered when building the map and not the whole directory. That should do the trick.

You could also add a counter and do the break after n loops, that way only the last n readings build the map.

Let me know if it worked or if you stumbled into any problem with this approach!

Ok, I think I figured it out. In the slam_gmapping.cpp file of the ROS wrapper go to line 592ff. There you see this code:

    ROS_DEBUG("Trajectory tree:");
    for(GMapping::GridSlamProcessor::TNode* n = best.node;
        n;
        n = n->parent)
    {
        ROS_DEBUG(" %.3f %.3f %.3f",
                  n->pose.x,
                  n->pose.y,
                  n->pose.theta);
        if(!n->reading)
        {
            ROS_DEBUG("Reading is NULL");
            continue;
        }
        matcher.invalidateActiveArea();
        matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
        matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
    }

Just after the line matcher.registerScan(smap, n->pose, &((*n->reading)[0])); add a single break;. That way only the last node in the trajectory tree that tracks your robots position is considered when building the map and not the whole directory. trajectory. That should do the trick.

You could also add a counter and do the break after n loops, that way only the last n readings build the map.

Let me know if it worked or if you stumbled into any problem with this approach!