[ROS] TF 坐标转换,小乌龟跟随案例(调试成功,课件代码)

[ROS] TF 坐标转换,小乌龟跟随案例(调试成功,课件代码)




1. 完整代码(cpp)

0. 配置

package.xml

添加 TF 依赖

    <build_depend>tf</build_depend>
    <build_export_depend>tf</build_export_depend>
    <exec_depend>tf</exec_depend>

cmakelist.txt

# 1. 找到 TF 库文件包
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
        roscpp
        rosmsg
        rospy
        tf
        )

# 2. 生成可执行文件
add_executable(${PROJECT_NAME}_demo01_turtle1
        src/demo01_turtle1.cpp)


add_executable(${PROJECT_NAME}_demo01_turtle2
        src/demo01_turtle2.cpp)


# 3. 为可执行文件链接库文件
target_link_libraries(${PROJECT_NAME}_demo01_turtle1
        ${catkin_LIBRARIES}
        )

target_link_libraries(${PROJECT_NAME}_demo01_turtle2
        ${catkin_LIBRARIES}
        )

1. 项目结构

在这里插入图片描述

2. demo01.launch 文件

<launch>

    <!--
     1. 界面上需要显示两只小乌龟
     2. 小乌龟1是怎么运动的:通过键盘操作控制移动
     3. 小乌龟2是怎么运动的:通过publisher发送topic消息控制移动的
     4. 小乌龟2目标位置的确定
     -->

      <!-- 一个界面, 一个乌龟 -->
     <node pkg="turtlesim" type="turtlesim_node" name="turtlesim"></node>

     <!-- 键盘操控小乌龟 -->
     <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key"></node>

     <!-- 小乌龟1对应的节点,用来发布实时位置 -->
     <node pkg="demo_tf" type="demo_tf_demo01_turtle1" name="turtle1" output="screen"></node>

     <!-- 显示小乌龟2,用来接收乌龟1位置,并通过 TF 得到实时的与乌龟1的相对位置信息,再计算距离,角度 -->
     <node pkg="demo_tf" type="demo_tf_demo01_turtle2" name="turtle2" output="screen"></node>


</launch>

3. demo01_turtle1.cpp

//获取turtle1的实时位置
//将实时位置上传个TF工具


#include <iostream>
#include <ros/ros.h>
#include <turtlesim/Pose.h>

#include <tf/transform_broadcaster.h>


using namespace std;


void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster);
//void poseCallback(const turtlesim::Pose::ConstPtr &msg);  // test without TF


// tf使用步骤:
// 1. 映入依赖 tf
// 2. 添加头文件
// 3. 同broadcaster.sendTransform来上传

int main(int argc, char **argv) {
    // 初始化节点
    string nodeName = "demo01_turtle1_node";
    ros::init(argc, argv, nodeName);

    ros::NodeHandle node;

    //将实时位置上传个TF工具(broadcaster)
    tf::TransformBroadcaster broadcaster;

    //获取turtle1的实时位置
    string pose_topic_name = "/turtle1/pose";
    const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
            pose_topic_name,
            1000,
            boost::bind(poseCallback, _1, broadcaster)
            );

//    // test without TF
//    const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
//            pose_topic_name,
//            1000,
//            poseCallback
//    );

    //阻塞
    ros::spin();

    return 0;
}


/***-------------------- callback function area --------------------***/
void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster) {
//void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
    //获取turtle1的实时位置 给定的坐标是相对于显示面板(世界坐标系)
    float x = msg->x;
    float y = msg->y;
    float theta = msg->theta;

    ROS_INFO_STREAM("turtle_1: " << x << " " << y << " " << theta);

    //实时上传
    // 位置和姿态信息
    tf::Transform input;
    //设置位置 (x, y, z)
    input.setOrigin(tf::Vector3(x, y, 0));
    //设置姿态(以四元素建立姿态描述) (欧拉角(roll, pitch, yaw)) 四元素(x, y, z, w) https://quaternions.online/
    tf::Quaternion quat;
    quat.setRPY(0, 0, theta);
    input.setRotation(quat);

    tf::StampedTransform transform(input, ros::Time::now(), "world", "turtle1");
    broadcaster.sendTransform(transform);
}


4. demo02_turtle2.cpp


// 小乌龟2

#include <iostream>

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

using namespace std;


void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster);


int main(int argc, char **argv) {
   // 初始化节点
   string nodeName = "demo01_turtle2";
   ros::init(argc, argv, nodeName);

   ros::NodeHandle node;


   ros::AsyncSpinner spinner(1);
   spinner.start();


    产生小乌龟2 
   //将小乌龟2显示到界面上去
   //通过 service client通讯创建小乌龟
   string service_name = "/spawn";
   ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>(service_name);

   //等待启动
   client.waitForExistence();

   turtlesim::Spawn service;
   service.request.x = 1.0;
   service.request.y = 1.0;
   service.request.theta = 0;
   service.request.name = "turtle2";
   client.call(service);


    获取小乌龟的位置 
   //将实时位置上传个TF工具(broadcaster)
   tf::TransformBroadcaster broadcaster;

   //获取turtle1的实时位置
   string pose_topic_name = "/turtle2/pose";
   const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
           pose_topic_name,
           1000,
           boost::bind(poseCallback, _1, broadcaster)
   );

//    // test without TF
//    const ros::Subscriber &subscriber = node.subscribe(
//            pose_topic_name,
//            1000,
//            poseCallback
//    );


    发布速度 
   // publisher控制小乌龟动
   string vel_topic_name = "/turtle2/cmd_vel";
   ros::Publisher vel_publisher = node.advertise<geometry_msgs::Twist>(vel_topic_name, 1000);

   //tf的监听工具,可以帮助我们实时的拿到相对位置信息
   tf::TransformListener listener;

   ros::Rate rate(10);
   while (ros::ok()) {
       //实时查看turtle1 在 turtle2的坐标系中的位置

       // target_frame: 坐标系,参考坐标系
       // source_frame: 求解的坐标系
       // 想知道 source_frame在 target_frame的位置信息
       // time: 0获得最近我和你的相对位置

       tf::StampedTransform transform;

       try {
           listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform);
       } catch (exception e) {
           //不执行
           continue;
       }

       //获得 位置
       tf::Vector3 &vector3 = transform.getOrigin();
       double x = vector3.x();
       double y = vector3.y();
       double z = vector3.z();

       ROS_INFO_STREAM("xyz(" << x << ", " << y << ", " << z << ")");

       double distance = sqrt(pow(x, 2) + pow(y, 2));
       double angular = atan2(y, x);

       // 之前的pid是做匀速控制的pid

       // time值不好确定
       // kp系数 t又不确定 kp/t 当成一种系数 k (常量)

       // vel = distance / t
       // vel = k * distance  - 常量

       // distance / t;
       // angular / t;

       geometry_msgs::Twist twist;
       twist.linear.x = 0.6 * distance;
       twist.angular.z = 6 * angular;
       vel_publisher.publish(twist);

       //获得 姿态
       // const tf::Quaternion &quaternion = transform.getRotation();


       //知道位置后,距离就好计算了

       rate.sleep();
   }


   //阻塞
   ros::waitForShutdown();
//    ros::spin();

   return 0;
}


/***-------------------- callback funciton area --------------------***/

void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster) {
//void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
   //获取turtle2的实时位置 给定的坐标是相对于显示面板(世界坐标系)
   float x = msg->x;
   float y = msg->y;
   float theta = msg->theta;

   ROS_INFO_STREAM("turtle_2: " << x << " " << y << " " << theta);

   //实时上传
   // 位置和姿态信息
   tf::Transform input;
   //设置位置 (x, y, z)
   input.setOrigin(tf::Vector3(x, y, 0));
   //设置姿态(以四元素建立姿态描述) (欧拉角(roll, pitch, yaw)) 四元素(x, y, z, w) https://quaternions.online/
   tf::Quaternion quat;
   quat.setRPY(0, 0, theta);
   input.setRotation(quat);

   tf::StampedTransform transform(input, ros::Time::now(), "world", "turtle2");
   broadcaster.sendTransform(transform);
}


2. 调试验证

在这里插入图片描述


3.完整代码(python)

1. 项目结构

在这里插入图片描述

2. py01.launch

<launch>

    <!--
    python版本的
     1. 界面上需要显示两只小乌龟
     2. 小乌龟1是怎么运动的:通过键盘操作控制移动
     3. 小乌龟2是怎么运动的:通过publisher发送topic消息控制移动的
     4. 小乌龟2目标位置的确定
     -->

      <!-- 一个界面, 一个乌龟 -->
     <node pkg="turtlesim" type="turtlesim_node" name="turtlesim"></node>

     <!-- 键盘操控小乌龟 -->
     <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key"></node>

     <!-- 小乌龟1对应的节点 -->
     <node pkg="demo_tf" type="demo01_turtle1.py" name="turtle1" output="screen"></node>

     <!-- 显示小乌龟2 -->
     <node pkg="demo_tf" type="demo01_turtle2.py" name="turtle2" output="screen"></node>


    <!-- rviz可视化调试节点 -->
    <!-- 路径必须写绝对路径 -->
    <node pkg="rviz"
          type="rviz"
          name="rviz"
          args="-d $(find demo_tf)/rviz/demo01.rviz">
    </node>

</launch>

3. demo01_turtle1.py

#! /usr/bin/env python
# coding: utf-8

import rospy
from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler


def pose_callback(msg):
    if not isinstance(msg, Pose):
        return

    x = msg.x
    y = msg.y
    # 小乌龟Z轴转动的角度, roll: x   pitch: y    yaw:z
    theta = msg.theta

    # 实时发布位置信息到TF工具
    # 位置
    translation = (x, y, 0)
    # 姿态 tf工具是用四元素来描述姿态信息 将 rpy欧拉角描述方式转换为 四元素描述方式
    rotation = quaternion_from_euler(0, 0, theta)

    broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle1", "world")


if __name__ == '__main__':
    # 创建node
    node_name = "demo01_turtle1"
    rospy.init_node(node_name)

    # 实时发布小乌龟的坐标位置信息
    broadcaster = TransformBroadcaster()

    # 订阅小乌龟1的位置信息
    pose_topic_name = "/turtle1/pose"
    rospy.Subscriber(pose_topic_name, Pose, pose_callback)

    rospy.spin()


4. demo01_turtle2.py

#! /usr/bin/env python
# coding: utf-8

import rospy

from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler

from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
from geometry_msgs.msg import Twist

from tf.listener import TransformListener
from math import pow, sqrt, atan2


def pose_callback(msg):
    if not isinstance(msg, Pose):
        return

    x = msg.x
    y = msg.y
    # 小乌龟Z轴转动的角度, roll: x   pitch: y    yaw:z
    theta = msg.theta

    # 实时发布位置信息到TF工具
    # 位置
    translation = (x, y, 0)
    # 姿态 tf工具是用四元素来描述姿态信息 将 rpy欧拉角描述方式转换为 四元素描述方式
    rotation = quaternion_from_euler(0, 0, theta)

    broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle2", "world")


if __name__ == '__main__':
    # 创建node
    node_name = "demo01_turtle2"
    rospy.init_node(node_name)

    # 产生小乌龟2
    service_name = "/spawn"
    client = rospy.ServiceProxy(service_name, Spawn)
    client.wait_for_service()

    request = SpawnRequest()
    request.x = 1.0
    request.y = 1.0
    request.theta = 0
    request.name = "turtle2"
    client.call(request)

    # 实时发布小乌龟的坐标位置信息
    broadcaster = TransformBroadcaster()

    # 订阅小乌龟2的位置信息
    pose_topic_name = "/turtle2/pose"
    rospy.Subscriber(pose_topic_name, Pose, pose_callback)

    # 发布速度,控制位置
    vel_topic_name = "/turtle2/cmd_vel"
    publisher = rospy.Publisher(vel_topic_name, Twist, queue_size=1000)

    # 获取相对位置信息的listener
    listener = TransformListener()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 实时查看turtle1 在 turtle2的坐标系中的位置
        #
        # target_frame: 坐标系,参考坐标系
        # source_frame: 求解的坐标系
        # 想知道 source_frame在 target_frame的位置信息
        # time: 0获得最近我和你的相对位置

        # 使用 try - catch 防止启动时没有数据导致的异常
        try:
            transform = listener.lookupTransform("turtle2", "turtle1", rospy.Time())
        except:
            rate.sleep()
            continue

        # 获取位置
        x, y, z = transform[0]
        # # 获取姿态 (四元素)
        # quat = transform[1]

        distance = sqrt(pow(x, 2) + pow(y, 2))
        angular = atan2(y, x)

        twist = Twist()
        twist.linear.x = 0.6 * distance
        twist.angular.z = 6 * angular
        publisher.publish(twist)

        rate.sleep()

    rospy.spin()



4. 调试验证

在这里插入图片描述


*. 参考


*. 问题解决


*. rough 知识点

在这里插入图片描述
在这里插入图片描述

创建小乌龟1,使用 turtlesim + 键盘控制节点,通过 launch 文件启动
创建小乌龟2,使用 api 写入 turtlesim,通过 turtlesim 的 service 通信 /spawn

在这里插入图片描述

可以看到 turtulesim 节点中有两个小乌龟订阅 turtle1 跟 bbb

在这里插入图片描述
在这里插入图片描述

小乌龟2运动,publisher控制小乌龟运动
在这里插入图片描述

小乌龟2目标位置确定
在这里插入图片描述
在这里插入图片描述
turtle1 实时上传
创建源码文件,实时获取 turtle1 当前位置,使用 topic 通信

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle1 集成 TF 功能
添加依赖,package.xml,cmakelist 添加 tf 包

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

4元素

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 获取位置实时上传(与 turtle1 代码几乎相同)
产看关系信息:rosrun rqt_tf_tree rqt_tf_tree

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 中位置在主线程中接收,对于回调函数
主线程卡了3秒,接收不到
在这里插入图片描述
在这里插入图片描述
创建一个异步 spinner 用于在子线程接收数据

在这里插入图片描述

报错,主线程中的 ros::spin 与 spinner 冲突

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 查看相对位置信息
实时查看 turtle1 在 turtle2 的坐标系中的位置

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 代码做频率操作,降低频率

在这里插入图片描述
在这里插入图片描述

turtle2 速度计算与发布

在这里插入图片描述

  • 7
    点赞
  • 18
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值