所用的学习链接：
【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P204-
【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】
一、目标
产生两只乌龟 A 和 B ，B 会自动运行至A的位置。
键盘只控制 A 的运动，而 B 跟随 A 运动。
乌龟跟随实现的核心，是乌龟 A 和 B 都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。
- 启动乌龟显示节点
- 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
- 编写两只乌龟发布坐标信息的节点
- 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
二、准备工作
1.创建环境
① 创建工作空间
# mkdir -p 空间名称/src # 必须得有 src
mkdir -p 7.24_turtle/src
cd 7.24_turtle
catkin_make
# 启动
code .② 创建功能包 tf1_turtle
右键src ---> create catkin package→自定义package name（例 tf1_turtle）→dependencies：roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim如果有包缺失的话可以调用以下代码加载：
# sudo apt-get install ros-<ros版本>-包名
# 包名中的 下划线（_）要换成杠（-）
# sudo apt-get install ros-noetic-包名ctrl+shift+b 编译运行，选择 catkin_make：build，看是否报错，没报错则依赖包没有出现问题。
③ 创建文件夹
在 tf1_turtle 目录下新建文件夹 launch(存放launch文件)、scripts(存放python文件)
④ 创建 t1_turtle2.cpp 文件
在 src 文件夹下新建文件 t1_turtle2.cpp，用于生成一只新的小乌龟
// 1.头文件
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
目标：向服务器发送请求，生成一只新的乌龟
话题：/spawn
消息：turtlesim/Spawn
1.头文件
2.初始化 ROS 节点
3.创建节点句柄
4.创建客户端对象
5.组织数据并发送
6.处理响应
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"service_call");
// 3.创建节点句柄
ros::NodeHandle nh;
// 4.创建客户端对象
ros::ServiceClient client= nh.serviceClient<turtlesim::Spawn>("/spawn");
// 5.组织数据并发送
// 5-1.组织请求数据
// 创建乌龟
turtlesim::Spawn spawn;
// 乌龟坐标
spawn.request.x=1.0;
spawn.request.y=4.0;
// 转向 90 度
spawn.request.theta=1.57;
// 乌龟名字
spawn.request.name="turtle2";
// 5-2.发送请求
// 判断服务器状态
// ros::service::waitForService("/spawn");
client.waitForExistence();
// flag 接收响应状态，响应结果也会被设置进 spawn 对象
bool flag=client.call(spawn);
// 6.处理响应
if(flag){
ROS_INFO("乌龟生成成功，新乌龟较：%s",spawn.response.name.c_str());
}
else{
ROS_INFO("请求失败！");
}
return 0;
}⑤ 修改 CMakeLists.txt 文件
修改 tf1_turtle 目录下的 CMakeLists.txt 文件
# 原句子
# 1.add_executable(${PROJECT_NAME}_node src/tf1_turtle_node.cpp)
# 2.add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# 3. target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
# 修改为
# 1.
add_executable(t1_turtle2 src/t1_turtle2.cpp)
# 2.
add_dependencies(t1_turtle2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# 3.
target_link_libraries(t1_turtle2
${catkin_LIBRARIES}
)⑥ 创建 t1_turtle1.launch 文件
在 launch 文件夹下新建文件 t1_turtle1.launch
<launch>
<!-- 启动乌龟GUI节点 -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>
<!-- 键盘控制节点 -->
<!--
也可以不加这一行，启动后新建终端，运行
rosrun turtlesim turtle_teleop_key
来启动键盘
-->
<!-- 发布的是关于第一只乌龟的运动消息，因此只能控制第一只 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"/>
<!-- 生成新乌龟的节点 -->
<node pkg="tf1_turtle" type="t1_turtle2" name="turtle2" output="screen"/>
</launch>这里我因为 type 写错，出现过 RLException: Unable to launch 的错误，注意检查
⑦ 给予文件执行权限
在 7.24_turtle / src / tf1_turtle / src 目录下打开终端，运行
# 使文件成为非root用户也可执行的文件
chmod +x *.cppctrl+shift+b 编译运行，选择 catkin_make：build，看是否报错，没报错则依赖包没有出现问题。
2. 测试运行
roslaunch tf1_turtle t1_turtle1.launch弹出一个窗口，上面有两只小乌龟。
选中运行 launch 文件的终端窗口为当前窗口，上下左右可以控制小乌龟运动。
三、 消息发布和订阅
（1）新建 t2_pub_turtle.cpp（发布）
#include "ros/ros.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方：订阅乌龟的位姿信息，转换成相对于窗体的坐标系并发布
话题：/turtle/pose
消息：/turtlesim/Pose
1.头文件
2.设置编码、初始化 ros 节点
3.创建订阅对象，订阅 /turtlex/pose
4.回调函数处理订阅的 pose 信息
将 pose 信息转换成 TransFormStamped
发布
5.spin
*/
// 声明变量接收传递的参数
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
// 获取位姿信息，转换成坐标系相对关系（核心），并发布
// a.创建TF发布对象
static tf2_ros::TransformBroadcaster pub;
// b.组织被发布的数据
geometry_msgs::TransformStamped ts;
ts.header.frame_id = "world";
ts.header.stamp = ros::Time::now();
// 关键动态参数
// ts.child_frame_id="turtle1";
ts.child_frame_id = turtle_name;
// 坐标系偏移量设置
ts.transform.translation.x = pose->x;
ts.transform.translation.y = pose->y;
ts.transform.translation.z = 0;
// 坐标系四元数
/*
位姿信息中没有四元数，但有偏航角度。
乌龟为 2D,无翻滚和俯仰角度
所以乌龟欧拉角：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();
// c.发布
pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
// 2.设置编码、初始化 ROS节点
setlocale(LC_ALL, "");
ros::init(argc, argv, "dynamic_pub");
ros::NodeHandle nh;
// 解析通过launch传入的arg参数
if (argc != 2)
{
ROS_ERROR("请传入一个参数");
return 1;
}
else
{
turtle_name = argv[1];
}
// 3.创建订阅对象，订阅/turtlex/pose
// 关键动态参数
// ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
ros::Subscriber sub = nh.subscribe(turtle_name + "/pose", 100, doPose);
// 4.回调函数处理订阅的 pose 信息
ros::spin();
return 0;
}（2）新建 t3_control_turtle2.cpp（订阅）
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
订阅方：计算 turtle1 和 turtle2 的 相对关系
计算 角速度和线速度并发布
1.头文件
2.编码、初始化、NodeHandle
3.创建订阅对象
4.编写解析逻辑
5.spinOnce()
*/
int main(int argc, char *argv[])
{
// 2.编码、初始化、NodeHandle
setlocale(LC_ALL, "");
ros::init(argc, argv, "tfs_sub");
ros::NodeHandle nh;
// 3.创建订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// A.创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);
// 4.编写解析逻辑
ros::Rate rate(10);
while (ros::ok())
{
// 核心
try
{
// 1.计算 turtle1 和 turtle2 的 相对关系
/*
1 相对于 2 的坐标系关系
参数1：目标坐标系 B
参数2：源坐标系 A
参数：ros::Time(0) 取间隔最短的两个坐标系关系帧计算相对关系
返回值：geometry_msgs::TransformStamped 源相对于坐标系的相对关系
*/
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
// ROS_INFO("son1 相对于 son2 的信息：父级%s,子级：%s 偏移量(%.2f,%.2f,%.2f)",
// son1ToSon2.header.frame_id.c_str(), //turtle2
// son1ToSon2.child_frame_id.c_str(), //turtle1
// son1ToSon2.transform.translation.x,
// son1ToSon2.transform.translation.y,
// son1ToSon2.transform.translation.z
// );
// B.根据相对计算并组织速度消息
geometry_msgs::Twist twist;
// 线速度，不会横着y，不会飞z，只有x前后
// x = 系数 * 开方( y ^ 2 + x ^ 2 )
twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x, 2) + pow(son1ToSon2.transform.translation.y, 2));
// 角速度，没有打滚x,没有抬头低头y,只有左右转z
// z = 系数 * 反正切( 对边 ， 临边 )
twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y, son1ToSon2.transform.translation.x);
// C.发布s
pub.publish(twist);
}
catch (const std::exception &e)
{
ROS_INFO("错误提示:%s", e.what());
}
rate.sleep();
ros::spinOnce();
}
// 5.spinOnce()
return 0;
}（3）修改 CMakeLists.txt
修改 tf1_turtle 目录下的 CMakeLists.txt 文件
（4）修改 t1_turtle1.launch 文件
<launch>
<!-- 1.启动乌龟GUI节点 -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
<!-- 键盘控制节点 -->
<!--
也可以不加这一行，启动后新建终端，运行
rosrun turtlesim turtle_teleop_key
来启动键盘
-->
<!-- 发布的是关于第一只乌龟的运动消息，因此只能控制第一只 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
<!-- 2.生成新乌龟的节点 -->
<node pkg="tf1_turtle" type="t1_turtle2" name="turtle2" output="screen" />
<!-- 3.启动2个乌龟相对于世界的坐标关系的发布
因为代码内容相似，因此用一个节点。
对该节点启动2次，分别传参 turtle1 和 turtle2 -->
<node pkg="tf1_turtle" type="t2_pub_turtle" name="pub1" args="turtle1" output="screen" />
<node pkg="tf1_turtle" type="t2_pub_turtle" name="pub2" args="turtle2" output="screen" />
<!-- 4.订阅坐标信息，转换成 乌龟A 相对于 乌龟B 的坐标信息，再生成控制 乌龟B 的速度信息 -->
<node pkg="tf1_turtle" type="t3_control_turtle2" name="control" output="screen" />
</launch>四、运行乌龟跟随
roslaunch tf1_turtle t1_turtle1.launch操控键盘，第二只小乌龟将一直跟随第一只移动。距离越远，第二只小乌龟的加速越大
7054
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
