コード例 #1
0
bool
SlamKarto::updateMap()
{
    boost::mutex::scoped_lock(map_mutex_);

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

    if(!occ_grid)
        return false;

    if(!got_map_) {
        map_.map.info.resolution = resolution_;
        map_.map.info.origin.position.x = 0.0;
        map_.map.info.origin.position.y = 0.0;
        map_.map.info.origin.position.z = 0.0;
        map_.map.info.origin.orientation.x = 0.0;
        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 ||
            map_.map.info.height != (unsigned int) height ||
            map_.map.info.origin.position.x != offset.GetX() ||
            map_.map.info.origin.position.y != offset.GetY())
    {
        map_.map.info.origin.position.x = offset.GetX();
        map_.map.info.origin.position.y = offset.GetY();
        map_.map.info.width = width;
        map_.map.info.height = height;
        map_.map.data.resize(map_.map.info.width * map_.map.info.height);
    }

    for (kt_int32s y=0; y<height; y++)
    {
        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));

            switch (value)
            {
            case karto::GridStates_Unknown:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
                break;
            case karto::GridStates_Occupied:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
                break;
            case karto::GridStates_Free:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
                break;
            default:
                ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
                break;
            }
        }
    }

    // Set the header information on the map
    map_.map.header.stamp = ros::Time::now();
    map_.map.header.frame_id = map_frame_;

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

    delete occ_grid;

    return true;
}
コード例 #2
0
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
                   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;

    if (lasers_inverted_[scan->header.frame_id]) {
        for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
                it != scan->ranges.rend();
                ++it)
        {
            readings.push_back(*it);
        }
    } else {
        for(std::vector<float>::const_iterator it = scan->ranges.begin();
                it != scan->ranges.end();
                ++it)
        {
            readings.push_back(*it);
        }
    }

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

    // 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
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
            tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
                              tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
                              scan->header.stamp, base_frame_),odom_to_map);
        }
        catch(tf::TransformException e)
        {
            ROS_ERROR("Transform from base_link to odom failed\n");
            odom_to_map.setIdentity();
        }

        map_to_odom_mutex_.lock();
        map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
                                     tf::Point(      odom_to_map.getOrigin() ) ).inverse();
        map_to_odom_mutex_.unlock();


        // Add the localized range scan to the dataset (for memory management)
        dataset_->Add(range_scan);
    }
    else
        delete range_scan;

    return processed;
}
コード例 #3
0
/**
 * @brief SlamKarto::addScan
 * @param laser         LaserRangeFinder ref base
 * @param scan           传感器数据     ref  laser
 * @param karto_pose     odom_pose       回调
 * @return
 */
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
                   const sensor_msgs::LaserScan::ConstPtr& scan,
                   karto::Pose2& karto_pose)
{
    if(!getOdomPose(karto_pose, scan->header.stamp)) //   将odom与base保持坐标系对齐. 返回 odom  --> karto_pose
        return false;

    // Create a vector of doubles for karto
    std::vector<kt_double> readings;
    //readings       原始scan laser -> reading
    if (lasers_inverted_[scan->header.frame_id])
    {
        for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
            it != scan->ranges.rend();
            ++it)
        {
            readings.push_back(*it);  //laser range messages
        }
    }
    else
    {
        for(std::vector<float>::const_iterator it = scan->ranges.begin();
            it != scan->ranges.end();
            ++it)
        {
            readings.push_back(*it);
            debugPrint_<<"   "<< *it;
        }

    }
    debugPrint_<<"   " <<endl;
    if(debug_flag_)
        debugPrint_<<"reading.size()  "<<readings.size()<<endl;

    // create localized range scan   创建rangeScan
    karto::LocalizedRangeScan* range_scan =
            new karto::LocalizedRangeScan(laser->GetName(), readings);//

    range_scan->SetOdometricPose(karto_pose);// Sets the odometric pose of this scan
    range_scan->SetCorrectedPose(karto_pose);// Moves the scan by moving the robot pose to the given location.

    if(debug_print_flag_)
        debugPrint_<<"1.1 addScan-karto_pose-: "<<karto_pose.GetX()<<"   "<<karto_pose.GetY()<<"  "<< karto_pose.GetHeading() <<"  "<<endl;

    // Add the localized range scan to the mapper
    bool processed;          //range_scan  contain reading and predict pose of robot
    if((processed = mapper_->Process(range_scan))) // pose 为base相对odom的pose
    {
        //std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
        karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
        if(debug_print_flag_)
            debugPrint_<<"1.2 addScan-corrected_pose-: "<<corrected_pose.GetX()<<"   "<<corrected_pose.GetY()<<"  "<< corrected_pose.GetHeading() <<"  "<<endl;

        // Compute the map->odom transform
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
            tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform( tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
                                                                                tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
                                                                 scan->header.stamp, base_frame_),odom_to_map);
        }
        catch(tf::TransformException e)
        {
            ROS_ERROR("Transform from base_link to odom failed\n");
            odom_to_map.setIdentity();
        }

        map_to_odom_mutex_.lock();
        map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
                                     tf::Point(      odom_to_map.getOrigin() ) ).inverse();
        map_to_odom_mutex_.unlock();

        //debugPrint_<< " x y z "<< odom_to_map.getOrigin().getX() <<"   "<< odom_to_map.getOrigin().getY()<<"  "<<odom_to_map.getOrigin().getZ()<<endl;
        if(debug_print_flag_)
            debugPrint_<<"1.3 addScan-odom_to_map: "<<odom_to_map.getOrigin().getX() <<"   "<< odom_to_map.getOrigin().getY()<<"  "<<endl;
        // Add the localized range scan to the dataset (for memory management)
        dataset_->Add(range_scan);
    }
    else
        delete range_scan;

    return processed;
}