从tf获取机器人位置

最新推荐文章于 2023-02-24 21:41:30 发布

zhangfenger

最新推荐文章于 2023-02-24 21:41:30 发布

阅读量1.2k

点赞数

分类专栏： slam

本文链接：https://blog.csdn.net/zhangfenger/article/details/91994860

版权

slam 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

    tf::Stamped<tf::Pose> ident;
    tf::Stamped<tf::Transform> robot_pose;
    ident.setIdentity();
    ident.frame_id_ = "base_link";
    ident.stamp_ = scan->header.stamp; //ros::Time::now();
    try
    {
        tf_.transformPose("map", ident, robot_pose);
//        float dx = std::abs(robot_pose.getOrigin().x() - last_pose_.x);
//        float dy = std::abs(robot_pose.getOrigin().y() - last_pose_.y);
//        float dthe = std::abs(tf::getYaw(robot_pose.getRotation()) - last_pose_.theta);
//        tf::Quaternion q = tf::createQuaternionFromYaw(last_pose_.theta);

        myout<<ndt_x<<" " <<robot_pose.getOrigin().x()<<" "<<ndt_y<<" "<<robot_pose.getOrigin().y()<<" "<< pose_z<<" "<<robot_pose.getOrigin().z() <<std::endl;
        pose_z = pose_z + 0.00001;
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute robot pose from tf, aborting initialization (%s)", e.what());
    }

geometry_msgs::Pose current_pose_ros;
tf::StampedTransform transform;
try {
        tf_.lookupTransform("/map","/base_link", ros::Time(0), transform);
    } catch (std::exception e ) {
        ROS_ERROR("move base cannot get robot pose!");
        return;
}
geometry_msgs::TransformStamped transform_pose;
tf::transformStampedTFToMsg(transform, transform_pose);
current_pose_ros.position.x = transform.getOrigin().x();
current_pose_ros.position.y = transform.getOrigin().y();
current_pose_ros.position.z = 0.0;
current_pose_ros.orientation=transform_pose.transform.rotation;