ros2小小白学习之路——坐标变换

最新推荐文章于 2024-03-18 01:09:19 发布

邱髓仙素

最新推荐文章于 2024-03-18 01:09:19 发布

阅读量918

点赞数

文章标签：机器人

本文链接：https://blog.csdn.net/weixin_62313539/article/details/131657331

版权

1.坐标信息

1.geometry_msgs/TransformStamped:传输坐标系相关位置信息

其中：

frame_id : 被参考的坐标系

chile_frame_id : 另外一个坐标系

translation ：两坐标系的偏移

rotation ：角度的变换

2.geometry_msgs/PointStamped:传输某个坐标系内坐标点的信息

2.静态坐标

发布：发布坐标系相对关系

订阅：订阅到坐标系相关信息，传入坐标点，利用tf进行坐标转换

发布方：

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    需求：发布两个坐标系的相对关系
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 初始化ros节点
    ros::init(argc,argv,"static_pub");
    // 创建句柄
    ros::NodeHandle nh;
    // 创建静态坐标转换广播
    tf2_ros::StaticTransformBroadcaster pub;
    // 创建坐标系信息
    geometry_msgs::TransformStamped ts;
    // 设置头信息
    ts.header.stamp = ros::Time::now();
    // 被参考的坐标系
    ts.header.frame_id = "base_id";
    // 另一个坐标系（此处为雷达）
    ts.child_frame_id = "laser";
    // 坐标系之间的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    // 需要根据欧拉角转换
    tf2::Quaternion qtn;
    // 向该对象设置欧拉角
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    pub.sendTransform(ts);
    ros::spin();
    return 0;
}

可以使用rviz进行可视化

订阅方：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/*

订阅方：订阅发布的坐标系相关关系，传入一个坐标，进行tf转换

*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 创建订阅对象，订阅坐标系的相对关系
    //创建一个buffer 缓冲
    tf2_ros::Buffer buffer;
    // 创建监听对象（监听对象可以将订阅的数据存入buffer
    tf2_ros::TransformListener listener(buffer);
    // 组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    // TF内置转换
    ros::Rate rate(10);
    while (ros::ok())
    {
        // 
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"base_id");
        // 输出
        ROS_INFO("转换后的坐标：（%.2f,%.2f%.2f),参考的坐标系为:%s",
        ps_out.point.x,
        ps_out.point.y,
        ps_out.point.z,
        ps_out.header.frame_id.c_str()
        );
        rate.sleep();
        ros::spinOnce();
        /* code */
    }
    
    return 0;
}

补充：

rosrun tf2_ros static_transform_publisher 偏移x,y,z 偏航z 俯仰y 翻滚x 父坐标系子坐标系

3.动态坐标

两坐标系相对位置是变化的

消息发布应该是动态的

案例：乌龟坐标系

发布者

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    分布方：需要订阅乌龟的位置信息并转换坐标并发布
    要订阅位置信息：
    话题 rostopic list/info rosmsg info 话题(/turtle1/pose)
    消息/turtlesim/Pose
*/

void doPose(const turtlesim::Pose::ConstPtr& pose){
// 获取位置信息，转换成坐标系相对关系并发布
// 创建被发布对象
static tf2_ros::TransformBroadcaster pub;
// 组织被发布数据
geometry_msgs::TransformStamped ts;
ts.header.frame_id = "world";
ts.header.stamp = ros::Time::now();
ts.child_frame_id = "turtle1";
// 坐标系偏移量
ts.transform.translation.x = pose -> x;
ts.transform.translation.y = pose -> y;
ts.transform.translation.z = 0;
// 坐标四元数
// 乌龟有偏航角度，但是无翻滚和俯仰 欧拉角：0 0 theta
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose -> theta);

ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();

pub.sendTransform(ts);
// 发布
}

int main(int argc, char *argv[])
{
    // 设置编码，初始化，Nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 创建订阅对象，订阅/turtle/pose
    // 回调函数处理订阅信息
    ros::spin();
    return 0;
}

测试：

1.rostopic echo /tf

2.rviz

订阅方

修改时间戳，参考系等等即可

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/*

订阅方：订阅发布的坐标系相关关系，传入一个坐标，进行tf转换

*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_sub");
    ros::NodeHandle nh;
    // 创建订阅对象，订阅坐标系的相对关系
    //创建一个buffer 缓冲
    tf2_ros::Buffer buffer;
    // 创建监听对象（监听对象可以将订阅的数据存入buffer
    tf2_ros::TransformListener listener(buffer);
    // 组织一个坐标点数据
    geometry_msgs::PointStamped ps;
    // 参考坐标系
    ps.header.frame_id = "turtle1";
    // 时间戳
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    // 添加休眠
    // ros::Duration(2).sleep();
    // TF内置转换
    ros::Rate rate(10);
    while (ros::ok())
    {
        geometry_msgs::PointStamped ps_out;
        // 调用buffer转换函数,数据在buffer里，参数1：被转换的坐标点；参数2：目的坐标系
        try
        {
            ps_out = buffer.transform(ps,"world");
            // 输出
            ROS_INFO("转换后的坐标：（%.2f,%.2f%.2f),参考的坐标系为:%s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
            );

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s",e.what());

        }
        
        rate.sleep();
        ros::spinOnce();
        /* code */
    }
    
    return 0;
}

4.小乌龟跟随案例

位置换算

1.地图标注

2.声明自己的坐标

3.根据各自的坐标实现坐标变换，计算1相对于2的位置

4.计算2在1的坐标系上的相对位置

案例分析：

乌龟A与B需要分布相对于世界坐标系的坐标信息，订阅到该信息，转换位置信息获取到A相对于B坐标系的信息他，最后生成速度信息，并控制B运动。

发布方（注意动态参数部分）

生成乌龟：使用服务 /spawn

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

/*
    服务端

*/
int main(int argc, char *argv[])
{
    // 设置编码，初始化，生成服务客户端
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"turtle_create");
    ros::NodeHandle nh;
    // 生成服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 1.0;
    spawn.request.theta = 3.14;

    bool flag = client.call(spawn);
    if(flag){
        ROS_INFO("%s is created",spawn.response.name.c_str());
    }else{
        ROS_INFO("Fail!!!");
    }

    ros::spin();
    
    return 0;
}

位置订阅

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
/*
    订阅到位置信息并转换为世界地图的位置
    并发布
    乌龟1与乌龟2只有名称不同，所以可以传参启动此文件
*/

// 声明变量接受乌龟的名字
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    // 创建广播
    static tf2_ros::TransformBroadcaster broadcaster;
    // 创建坐标点信息
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    // 需要动态传入
    ts.child_frame_id = turtle_name;
    // 偏移
    ts.transform.translation.x = pose -> x;
    ts.transform.translation.y = pose -> y;
    ts.transform.translation.z = 0;
    // 四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.w = qtn.getW();

    broadcaster.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 设置编码，初始化
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"turtle_pub");
    // 命名
    if(argc != 2){
        ROS_ERROR("please enter the correct parameter");
        return 1;
    }else{
        turtle_name = argv[1];
        ROS_INFO("The location of %s is sent",turtle_name.c_str());
    }
    ros::NodeHandle nh;
    // 创建订阅对象 需要动态传入
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    ros::spin();
    return 0;

}

订阅方(将turtle1的坐标信息转换为相对于turtle2的，并生成速度信息并发布)

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/Twist.h"
/*
需要订阅turtle1与turtle2相对于世界坐标系的坐标信息，并转换成turtle1相对于turtle2的坐标信息
生成速度消息
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tutrle_control");
    ros::NodeHandle nh;
    // 创建一个buffer缓冲
    tf2_ros::Buffer buffer;
    // 创建监听对象
    tf2_ros::TransformListener listener(buffer);
    // 创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    ros::Rate rate(10);
    while(ros::ok()){
        try
        {
            // 获取turtle1与turtle2的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            
            ROS_INFO("turtle1相对于turtle2的信息:父级:%s,子级：%s,偏移量（%.2f,%.2f)",
                    tfs.header.frame_id.c_str(),
                    tfs.child_frame_id.c_str(),
                    tfs.transform.translation.x,
                    tfs.transform.translation.y);
            // 根据坐标信息生成速度信息
            geometry_msgs::Twist twist;
            // 速度计算 直线距离
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2)+pow(tfs.transform.translation.y,2));
            // 角度 夹角 z = 系数* 反正切（对边，邻边）
            twist.angular.z = 2 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            pub.publish(twist);

        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
    }
    ros::spinOnce();
    return 0;
}

使用launch启动多个节点

<launch>
    <!-- 启动乌龟 -->
    <node pkg="turtlesim" type = "turtlesim_node" name="turtle1" output="screen" />

    <!-- 生成乌龟 -->
    <node pkg="follow" type = "turtle_create" name="turtle2" output="screen" />

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type = "turtle_teleop_key" name="key" output="screen" />

    <!-- 设置两个乌龟相对于世界坐标关系的发布 -->
    <!-- 
        1.节点只写一个
        2.节点启动两次
        3.动态传参
     -->

     <node pkg="follow" type="follow_pub" name="pub1" args="turtle1" output="screen" />
     <node pkg="follow" type="follow_pub" name="pub2" args="turtle2" output="screen" />

     <!-- 订阅方 -->
     <node pkg="follow" type="follow_sub" name="turtle2_control" output="screen" />


</launch>

补充：控制第二只乌龟运动：

rostopic list找到对应控制的话题

rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist tab自动补齐来控制运动