C++ TransformListener::transformQuaternion方法代码示例

//.........这里部分代码省略.........
        // HACK
        // Modify the delta in the action data so the filter gets
        // updated correctly
        odata.delta = delta;

        // Use the action data to update the filter
        odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);

        // Pose at last filter update
        //this->pf_odom_pose = pose;
    }

    bool resampled = false;
    // If the robot has moved, update the filter
    if(lasers_update_[laser_index])
    {
        AMCLLaserData ldata;
        ldata.sensor = lasers_[laser_index];
        ldata.range_count = laser_scan->ranges.size();

        // To account for lasers that are mounted upside-down, we determine the
        // min, max, and increment angles of the laser in the base frame.
        //
        // Construct min and max angles of laser, in the base_link frame.
        tf::Quaternion q;
        q.setRPY(0.0, 0.0, laser_scan->angle_min);
        tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                          laser_scan->header.frame_id);
        q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
        tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                          laser_scan->header.frame_id);
        try
        {
            tf_->transformQuaternion(base_frame_id_, min_q, min_q);
            tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
        }
        catch(tf::TransformException& e)
        {
            ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                     e.what());
            return;
        }

        double angle_min = tf::getYaw(min_q);
        double angle_increment = tf::getYaw(inc_q) - angle_min;

        // wrapping angle to [-pi .. pi]
        angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

        ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);

        // Apply range min/max thresholds, if the user supplied them
        if(laser_max_range_ > 0.0)
            ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
        else
            ldata.range_max = laser_scan->range_max;
        double range_min;
        if(laser_min_range_ > 0.0)
            range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
        else
            range_min = laser_scan->range_min;
        // The AMCLLaserData destructor will free this memory
        ldata.ranges = new double[ldata.range_count][2];
        ROS_ASSERT(ldata.ranges);
        for(int i=0; i<ldata.range_count; i++)
        {

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];
}