Exemplo n.º 1
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;
}
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  // Check whether we know about this laser yet
  if(lasers_.find(scan->header.frame_id) == lasers_.end())
  {
    // New laser; need to create a Karto device for it.

    // Get the laser's pose, relative to base.
    tf::Stamped<tf::Pose> ident;
    tf::Stamped<tf::Transform> laser_pose;
    ident.setIdentity();
    ident.frame_id_ = scan->header.frame_id;
    ident.stamp_ = scan->header.stamp;
    try
    {
      tf_.transformPose(base_frame_, ident, laser_pose);
    }
    catch(tf::TransformException e)
    {
      ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
	       e.what());
      return NULL;
    }

    double yaw = tf::getYaw(laser_pose.getRotation());

    ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
	     scan->header.frame_id.c_str(),
	     laser_pose.getOrigin().x(),
	     laser_pose.getOrigin().y(),
	     yaw);
    // To account for lasers that are mounted upside-down,
    // we create a point 1m above the laser and transform it into the laser frame
    // if the point's z-value is <=0, it is upside-down

    tf::Vector3 v;
    v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
    tf::Stamped<tf::Vector3> up(v, scan->header.stamp, base_frame_);

    try
    {
      tf_.transformPoint(scan->header.frame_id, up, up);
      ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
    }
    catch (tf::TransformException& e)
    {
      ROS_WARN("Unable to determine orientation of laser: %s", e.what());
      return NULL;
    }

    bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0;
    if (inverse)
      ROS_INFO("laser is mounted upside-down");


    // 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::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
    laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
				      laser_pose.getOrigin().y(),
				      yaw));
    laser->SetMinimumRange(scan->range_min);
    laser->SetMaximumRange(scan->range_max);
    laser->SetMinimumAngle(scan->angle_min);
    laser->SetMaximumAngle(scan->angle_max);
    laser->SetAngularResolution(scan->angle_increment);
    // TODO: expose this, and many other parameters
    //laser_->SetRangeThreshold(12.0);

    // Store this laser device for later
    lasers_[scan->header.frame_id] = laser;

    // Add it to the dataset, which seems to be necessary
    dataset_->Add(laser);
  }

  return lasers_[scan->header.frame_id];
}
Exemplo n.º 3
0
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    // Check whether we know about this laser yet
    if(lasers_.find(scan->header.frame_id) == lasers_.end())
    {
        // New laser; need to create a Karto device for it.

        // Get the laser's pose, relative to base.
        tf::Stamped<tf::Pose> ident;
        tf::Stamped<tf::Transform> laser_pose;
        ident.setIdentity();
        ident.frame_id_ = scan->header.frame_id;
        ident.stamp_ = scan->header.stamp;
        try
        {
            tf_.transformPose(base_frame_, ident, laser_pose);
        }
        catch(tf::TransformException e)
        {
            ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
                     e.what());
            return NULL;
        }

        double yaw = tf::getYaw(laser_pose.getRotation());

        ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",
                 scan->header.frame_id.c_str(),
                 laser_pose.getOrigin().x(),
                 laser_pose.getOrigin().y(),
                 yaw);
        // To account for lasers that are mounted upside-down, we determine the
        // min, max, and increment angles of the laser in the base frame.
        tf::Quaternion q;
        q.setRPY(0.0, 0.0, scan->angle_min);
        tf::Stamped<tf::Quaternion> min_q(q, scan->header.stamp,
                                          scan->header.frame_id);
        q.setRPY(0.0, 0.0, scan->angle_max);
        tf::Stamped<tf::Quaternion> max_q(q, scan->header.stamp,
                                          scan->header.frame_id);
        try
        {
            tf_.transformQuaternion(base_frame_, min_q, min_q);
            tf_.transformQuaternion(base_frame_, max_q, max_q);
        }
        catch(tf::TransformException& e)
        {
            ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                     e.what());
            return false;
        }

        double angle_min = tf::getYaw(min_q);
        double angle_max = tf::getYaw(max_q);
        bool inverse =  lasers_inverted_[scan->header.frame_id] = angle_max < angle_min;
        if (inverse)
            ROS_INFO("laser is mounted upside-down");


        // 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::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
        laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
                                          laser_pose.getOrigin().y(),
                                          yaw));
        laser->SetMinimumRange(scan->range_min);
        laser->SetMaximumRange(scan->range_max);
        laser->SetMinimumAngle(scan->angle_min);
        laser->SetMaximumAngle(scan->angle_max);
        laser->SetAngularResolution(scan->angle_increment);
        // TODO: expose this, and many other parameters
        //laser_->SetRangeThreshold(12.0);

        // Store this laser device for later
        lasers_[scan->header.frame_id] = laser;

        // Add it to the dataset, which seems to be necessary
        dataset_->Add(laser);
    }

    return lasers_[scan->header.frame_id];
}
/**
 * @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;
}