[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 速度计算与发布