ros编程入门

简介

功能包（package）:ROS软件中的基本单元，包含节点源码、配置文件、数据定义等。一个功能包里可以有多个节点，在节点中进行发布和订阅话题
功能包下的src文件夹编写的cpp用来实现发布和订阅功能，一个cpp文件可以制作一个节点
功能包下的script文件夹编写的python代码用来实现发布和订阅功能，一个python文件可以制作一个节点
节点可以发布或订阅话题，也可以提供或者使用服务(cpp或者py等编写)
节点是可执行程序，通过ros::init(argc, argv, “节点名字”);产生一个节点并命名
一个话题由多个消息组成，话题的类型取决于消息类型（结构体）
在大部分例程中将cpp文件名和可执行文件名，以及节点名命名成了一致。py文件一样
launch文件的目的是启动节点即可执行文件
一个消息类型相当于一个结构体
话题是一对多，服务是一对一，客户端给服务端传参数值
创建代理a=rospy.ServiceProxy(服务名，服务类型)；a(传参1，传参2)
服务端经过相关计算将结果给分割线下的成员变量。客户端则可以通过分割线下的成员变量获取计算结果
通过代理的返回值获取：b=a(传参1，传参2); b.成员变量
服务名相当于话题名，服务类型相当于话题类型，服务器相当于订阅器，客户端相当于发布器。
服务端的回调函数中的
return 服务类型Response()
将客户端发来的信息计算处理后将
括号内的数
送给该服务类型中相应服务名的的分割线下的成员变量
c++编写时，定义一个服务类型的结构体变量a 则a.Request.成员变量代表分割线上的成员变量—-客户端传的
c++编写时，定义一个服务类型的结构体变量a 则a.Response.成员变量代表分割线下的成员变量—-服务端回应的
python编写tf动态坐标发布只需要定义一个tf广播器，广播器调用发送函数sendTransform（传入参数相对位置和姿态）即可。姿态传入的四元数可以用tf.transformations.quaternion_from_euler函数将欧拉角转换为四元数
c++编写tf动态坐标发布只需要定义一个tf广播器，广播器调用发送函数sendTransform（传入参数相对位置和姿态）即可。姿态传入的四元数可以用tf::createQuaternionFromRPY将描述姿态的欧拉角转换为四元数（三个参数是分别绕x轴，y轴，z轴）

创建功能包

切换到工作空间的src下
1
cd catkin_ws/src
catkin_create_pkg创建功能包：参数为功能包名
1
catkin_create_pkg bingda_practices

发布者节点c++

打开Visual Studio Code找到我们刚建立的功能包，右键选择New Folder在功能包下新建一个文件夹取名src用来存放代码，src下新建一个talker.cpp文件，代码如下

代码详解

#include "ros/ros.h"
//要发布Sting类型的消息
#include "std_msgs/String.h"
 
int main(int argc, char **argv)
{//初始化一个节点名字为talker来发布话题名为chatter的话题
ros::init(argc, argv, "talker");
//创建句柄
ros::NodeHandle n;
//创建发布器chatter_pub，发布消息类型String,发布话题名字为chatter,最大缓存1000个
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
//指定循环频率10hz
ros::Rate loop_rate(10);
//定义变量count来计算发布话题个数
  int count = 0;
//当crtl+c、同名节点运行、ros整个进程结束、kill杀死时ros::ok()为false
while (ros::ok())
{//创建一个消息类型为String(结构体)(成员变量data)的变量
std_msgs::String msg;
//创建string类型的变量ss
std::stringstream ss;
//变量ss中传入数据hello world count(变量会变)
ss<<" hello world "<<count;
//把变量ss中的内容给消息msg的成员变量data中 
msg.data = ss.str();
//打印msg的内容以字符串的形式
ROS_INFO("%s",msg.data.c_str());
//发布器发布话题，发布内容为消息msg
chatter_pub.publish(msg);
ros::spinOnce();
//休眠
loop_rate.sleep();
count++;
}

return 0;
}

代码编写完成后编辑CMakeLists.txt文件在find_package(catkin REQUIRED)括号内加入 roscpp std_msgs
将include_directories内的两行注释取消

在文件末尾加上以下两句，设置编译规则

1 2	add_executable(talker src/talker.cpp) target_link_libraries(talker ${catkin_LIBRARIES})

修改完成后，调用终端编译工作空间。编译完成后在工作空间的devel->lib路径下对应的功能包里就会存放有编译生成的可执行文件。

订阅者节点c++

在功能包下的src文件下新建一个listener.cpp文件

代码详解

#include "ros/ros.h"
#include "std_msgs/String.h"
//回调函数，传入参数为订阅的话题消息
void chatterCallback(const std_msgs::String::ConstPtr& msg)

{//打印chatter话题里的消息内容，以字符串形式打印
ROS_INFO("I heard [%s]",msg->data.c_str());
}
int main(int argc, char **argv)
{//初始化一个节点名字为listener来订阅话题名为chatter的话题
ros::init(argc, argv, "listener");
//创建句柄
ros::NodeHandle n;
//创建订阅器sub，接收话题的名字为chatter,最大缓存1000个，收到话题后执行回调函数chatterCallback
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
//创建自动循环
ros::spin();
return 0;
}

在CMakeLists.txt文件末尾添加

1 2	add_executable(listener src/listener.cpp) target_link_libraries(listener ${catkin_LIBRARIES})

调用终端编译工作空间，订阅者节点编写完成

发布者节点py

打开Visual Studio Code在功能包下新建一个文件夹取名script用来存放python代码，script下新建一个talker.py文件

代码详解

#!/usr/bin/env python
//导入rospy
import rospy
//导入std_msgs.msg里的String类型
from std_msgs.msg import String
//定义talker()函数
def talker():
//创建发布器pub发布话题名为chatter，消息类型String,缓存为10
pub = rospy.Publisher("chatter",String,queue_size=10)
//初始化生成节点talker
rospy.init_node("talker",anonymous=False)
//发布频率10hz
rate =rospy.Rate(10)
//创建循环，当这个节点没有关闭就一直循环
while not rospy.is_shutdown():
//创建字符串赋值为hello world rospy.get_time()--当前时间
hello_str = " hello world %s" % rospy.get_time()
//打印字符串的内容
rospy.loginfo(hello_str)
//发布器发布话题里的消息hello_str     
pub.publish(hello_str)
//休眠
rate.sleep()
//运行main函数
if __name__ == '__main__':
try:
//main中运行的函数talker()
talker()
except rospy.ROSInternalException:
pass

因为是python文件编写完成后直接运行即可
rosrun bingda_practices talker.py如遇报错提示执行权限不够，talker.py增加执行权限即可，右键talker.py点击Properties->Permission->Execute勾选

订阅者节点py

在script下新建一个listener.py文件

代码详解

#!/usr/bin/env python
//导入rospy
import rospy
//导入std_msgs.msg里的String类型
from std_msgs.msg import String
//定义回调函数callback，参数为data--这个参数随意。收到话题时，话题里的消息会传给回调函数的参数
def callback(data):
//打印回调进程ID和消息里的数据
rospy.loginfo(rospy.get_caller_id() + " I heard %s",data.data)
//定义listenr()函数
def listener():
//初始化生成节点listener
rospy.init_node("listener",anonymous=True)
//创建订阅器订阅话题名为chatter，消息类型String,回调函数callback
rospy.Subscriber("chatter",String,callback)
//循环
rospy.spin()
//运行main函数
if __name__ == '__main__':
//main中运行的函数listener()
listener()

rosrun bingda_practices listener.py给listener.py文件增加可执行权限，执行talker.py后再执行listener.py即可订阅talker发布的消息

启动单个节点

打开Visual Studio Code在功能包下新建一个文件夹取名launch用来存放launch文件，launch下新建一个talker.launch文件
代码详解

pkg指定的是节点所在的功能包

type指定的是c++语言编写的可执行文件talker，也可以指定为python编写的talker.py文件

name是launch里对节点重命名为talker，也可以是其他名称

output输出方式为在屏幕输出
1
2
3
<launch>
<node pkg = "bingda_practices" type = "talker" name = "talker" output = "screen"/>
</launch>
launch下新建一个listener.launch文件
1
2
3
<launch>
<node pkg = "bingda_practices" type = "listener.py" name = "listener" output = "screen"/>
</launch>

启动多个节点

新建一个test.launch文件，同时启动talker和listener节点
代码详解
$(find launch文件所在的功能包名)/launch/要启动的launch文件名

<launch>
<include file = "$(find bingda_practices)/launch/talker.launch"/>
<include file = "$(find bingda_practices)/launch/listener.launch"/>
</launch>

这个launch调用了bingda_practices/launch/下的talker.launch和listener.launch文件

自定义消息类型

打开Visual Studio Code在功能包下新建一个文件夹取名msg用来存放消息文件，msg下新建一个student类型的消息，Student.msg文件。在Student.msg文件中填充以下几个变量
1
2
3
4
string first_name
string last_name
uint8 age
uint32 score
修改配置文件CMakeLists.txt
find_package中加入message_generation
add_message_files中加入FILES换行Student.msg也就是自定义消息文件
generate_messages函数注释
在catkin_package中加上CATKIN_DEPENDS message_runtime
修改配置文件package.xml

加入构建依赖项
1
2
<build_depend>message_runtime</build_depend>
<build_export_depend>message_runtime</build_export_depend>
保存修改，编译工作空间。

查看Student消息类型的结构体
rosmsg show 包名/消息类型
1
rosmsg show bingda_practices/Student

使用自定义消息类型发布接收话题py

一.发布话题

在功能包的script下新建一个msg_pub.py文件

代码详解

#!/usr/bin/env python
# license removed for brevity
import rospy
//从功能包里的msg文件夹里导入Student类型
from bingda_practices.msg import Student
//定义话题发布函数
def msg_pub():
//定义发布器，发布的话题名为Student_info，消息类型Student，缓存为10
pub = rospy.Publisher('Student_info',Student,queue_size=10)
//初始化生成节点，名字为msg_pub
rospy.init_node('msg_pub',anonymous=False)
//设置发布频率10hz
rate = rospy.Rate(10)
//当节点正常运行则一直进行下面循环
while not rospy.is_shutdown():
//定义Student类型的变量student
student = Student()
//给Student类型的变量赋值
student.first_name = "BingDa"
student.last_name = "Robot"
student.age = 2
student.score = 100
//打印字符串pub a student info
rospy.loginfo('pub a student info')
//使用发布器发布Student类型的变量student
pub.publish(student)
//休眠
rate.sleep()
//主函数
if __name__ == '__main__':
try:
//使用话题发布函数
msg_pub()
except rospy.ROSInterruptException:
pass

运行发布节点

1 2	roscore rosrun bingda_practices msg_pub.py

二.订阅话题

在功能包的script下新建一个msg_sub.py文件

代码详解

#!/usr/bin/env python
import rospy
//从功能包里的msg文件夹里导入Student类型
from bingda_practices.msg import Student
//订阅器收到的话题消息传给回调函数的参数data
def callback(data):
//打印收到的Student消息类型的各成员变量
rospy.loginfo("Student Name is %s %s;Age is %d ;Socer is %d.", data.first_name,data.last_name ,data.age,data.score)
//定义话题订阅函数
def msg_sub():
//初始化生成节点，名字为msg_sub
rospy.init_node('msg_sub', anonymous=False)
//定义订阅器，订阅的话题名为Student_info，消息类型Student，回调函数为callback
rospy.Subscriber("student_info", Student, callback)
//循环
rospy.spin()
//mian函数
if __name__ == '__main__':
//使用话题订阅函数
msg_sub()

运行订阅节点

1 2	roscore rosrun bingda_practices msg_sub.py

使用自定义消息类型发布接收话题c++

一.发布话题

在功能包的src下新建一个msg_pub.cpp文件

代码详解

#include "ros/ros.h"
//包含自定义消息类型的头文件
#include "bingda_practices/Student.h"

#include <sstream>

int main(int argc, char **argv)
{//初始化一个节点名字为msg_pub来发布话题名为student_info的话题
ros::init(argc, argv, "msg_pub");
//创建句柄
ros::NodeHandle n;
//创建发布器chatter_pub，发布消息类型为bingda_practices包里的Student类型,发布话题名字为student_info,最大缓存1000个
ros::Publisher chatter_pub = n.advertise<bingda_practices::Student>("student_info", 1000);
//指定循环频率10hz
ros::Rate loop_rate(10);
//定义变量count来计算发布话题个数
uint8_t count = 0;
//当crtl+c、同名节点运行、ros整个进程结束、kill杀死时ros::ok()为false
while (ros::ok())
{
//创建一个消息类型为Student(结构体)的变量
bingda_practices::Student student;
//给Student类型的变量中的成员变量赋值
student.first_name = "BingDa";
student.last_name = "Robot";
student.age = count;
student.score = 100;
//发布器发布话题，发布内容为消息变量student
chatter_pub.publish(student);

ros::spinOnce();
//休眠
loop_rate.sleep();
++count;
}
return 0;
}

在这个msg_pub.cpp文件中包含了我们自定义消息的头文件bingda_practices/Student.h而Visual Studio Code中没有包含工作空间的路径，需要先包含工作空间下的头文件目录
1
2
//c_cpp_properties.json文件里的includePath里添加
"/home/bingda/catkin_ws/devel/include**"

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

1
2
3

add_executable(msg_pub src/msg_pub.cpp)
target_link_libraries(msg_pub ${catkin_LIBRARIES})
add_dependencies(msg_pub bingda_practices_generate_messages_cpp)

完成工作空间编译后，运行节点并输出话题内容

二.订阅话题

在功能包的src下新建一个msg_sub.cpp文件

代码详解

#include "ros/ros.h"
//要接受Student类型的消息
#include "bingda_practices/Student.h"
//回调函数，传入参数为订阅的Student类型的话题消息
void chatterCallback(const bingda_practices::Student::ConstPtr& msg)
{
  //打印student_info话题里的消息内容，以字符串形式打印
ROS_INFO("Student Name is %s %s;Age is %d ;Socer is %d.", msg->first_name.c_str(),msg->last_name.c_str() ,msg->age,msg->score);
}

int main(int argc, char **argv)
{
  //初始化一个节点名字为msg_sub来订阅话题名为student_info的话题
ros::init(argc, argv, "msg_sub");
//创建句柄
ros::NodeHandle n;
//创建订阅器sub，接收话题的名字为student_info,最大缓存1000个，收到话题后执行回调函数chatterCallback
ros::Subscriber sub = n.subscribe("student_info", 1000, chatterCallback);
//创建自动循环
ros::spin();
return 0;
}

编辑CMakeLists.txt文件，在文件末尾加上以下三句，设置编译规则

1
2
3

add_executable(msg_sub src/msg_sub.cpp)
target_link_libraries(msg_sub ${catkin_LIBRARIES})
add_dependencies(msg_sub bingda_practices_generate_messages_cpp)

完成工作空间编译后，运行节点订阅msg_pub节点发布的话题

自定义服务类型

打开Visual Studio Code在功能包下新建一个文件夹取名srv用来存放服务文件，srv下新建一个AddTwoInts.srv文件。假如要实现的功能是客户端发送两个数字，服务端响应这两个数字的和。AddTwoInts.srv文件中填充以下几个变量

分隔符以上的部分是客户端的请求，分别是a，b两个数。分隔符以下的部分是服务端应答的ab之和sum

int64 a
int64 b
---
int64 sum

修改配置文件CMakeLists.txt
add_service_files中加入FILES换行AddTwoInts.srv也就是自定义服务类型，删除其他服务类型
保存修改，编译工作空间后查看验证服务文件

rossrv show 功能包名/服务类型
1
rossrv show bingda_practices/AddTwoInts

使用自定义服务类型编写服务端客户端py

一.服务端

编写一个python的节点使用这个服务，在功能包的script下新建add_two_ints_server.py文件

代码详解

#!/usr/bin/env python

import rospy
//包含服务类型和服务类型的响应
from bingda_practices.srv import AddTwoInts,AddTwoIntsResponse
//定义回调函数
//当收到服务类型为AddTwoInts，服务名为add_two_ints的信息时
//信息会传递给回调函数的参数
def handle_add_two_ints(req):
//打印客户端发来的信息
print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
//将客户端发来的信息相加送给服务名为add_two_ints服务类型为AddTwoInts的分割线下的成员变量
//return 服务类型Response()；
return AddTwoIntsResponse(req.a + req.b)
//定义一个服务函数handle_add_two_ints
def add_two_ints_server():
//初始化节点名字为add_two_ints_server
rospy.init_node('add_two_ints_server')
//定义一个服务器，服务名为add_two_ints，服务类型为add_two_ints，回调函数为handle_add_two_ints
s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
//打印信息
print("Ready to add two ints.")
//循环
rospy.spin()
//main函数执行服务函数
if __name__ == "__main__":
add_two_ints_server()

保存运行，启动服务器节点发布服务
1
rosrun bingda_practices add_two_ints_server.py
调用服务
1
rosservice call /add_two_inits 3 5
二.客户端

编写一个python的客户端节点，在功能包的script下新建add_two_ints_client.py文件

代码详解

#!/usr/bin/env python
//参数判断包，如果传入的是参数，则给sys.argv[]数组
import sys
import rospy
//从bingda_practices的srv文件夹导入自定义服务类型AddTwoInts
from bingda_practices.srv import *
//定义一个客户端函数add_two_ints_client来给服务类型为AddTwoInts的分割线上的成员变量赋值
def add_two_ints_client(x, y):
//等待服务名为add_two_ints的服务（阻塞式）
rospy.wait_for_service('add_two_ints')
//若服务可用
try:
//创建一个服务代理名add_two_ints,服务名为add_two_ints,服务类型为AddTwoInts
add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
//x，y赋值给服务类型为AddTwoInts的分割线上的成员变量
resp1 = add_two_ints(x, y)
//获取服务类型为AddTwoInts的分割线下的成员变量sum
return resp1.sum
//出现异常则抛出异常
except rospy.ServiceException as e:
print("Service call failed: %s"%e)

if __name__ == "__main__":
//判断是否为三个参数，argv[0] 一般是被调用的脚本文件名或全路径，和操作系统有关，argv[1]和以后就是传入的数据了。
if len(sys.argv) == 3:
x = int(sys.argv[1])
y = int(sys.argv[2])
//不满足就退出
else:
sys.exit(1)
//打印传入的数据和从服务端计算后的响应数据
print("Requesting %s+%s"%(x, y))
print("%s + %s = %s"%(x, y, add_two_ints_client(x, y)))

保存运行，启动服务器节点发布服务
1
rosrun bingda_practices add_two_ints_server.py

保存运行，启动客户端节点调用服务

1	rosrun bingda_practices add_two_ints_client.py 3 5

使用自定义服务类型编写服务端客户端c++

一.服务端

在功能包的src下新建一个add_two_ints_server.cpp文件

代码详解

#include "ros/ros.h"
//包含服务类型头文件
#include "bingda_practices/AddTwoInts.h"
//回调函数
//当客户端传入AddTwoInts服务类型的参数给分割线上的成员变量结构体，定义该结构体为res
//AddTwoInts服务类型的分割线下的成员变量结构体定义为res
bool add(bingda_practices::AddTwoInts::Request  &req,
bingda_practices::AddTwoInts::Response &res)
{
//给服务类型的分割线下的成员变量结构体赋值
res.sum = req.a + req.b;
//打印客户端发来的信息
ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
//打印分割线下的成员变量sum
ROS_INFO("sending back response: [%ld]", (long int)res.sum);
return true;
}

int main(int argc, char **argv)
{
  //初始化生成节点名字为add_two_ints_server
ros::init(argc, argv, "add_two_ints_server");
//定义句柄
ros::NodeHandle n;
//定义一个服务器，服务名为add_two_ints，回调函数为add
ros::ServiceServer service = n.advertiseService("add_two_ints", add);
//打印信息
ROS_INFO("Ready to add two ints.");
//循环
ros::spin();

return 0;
}

修改配置文件CMakeLists.txt

在文件末尾加上以下三句，设置编译规则

1
2
3

add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server bingda_practices_generate_messages_cpp)

保存编译后即可运行服务端节点并使用rosservice call的方法调用它
1
2
rosrun bingda_practices add_two_ints_server
rosservice call /add_two_inits 3 5

二.客户端

在功能包的src下新建一个add_two_ints_client.cpp文件

代码详解

#include "ros/ros.h"
//包含服务类型头文件
#include "bingda_practices/AddTwoInts.h"

int main(int argc, char **argv)
{
  //初始化生成节点名字为add_two_ints_client
ros::init(argc, argv, "add_two_ints_client");
//判断是否为三个参数，argv[0] 一般是被调用的脚本文件名或全路径，和操作系统有关，argv[1]和以后就是传入的数据了。
if (argc != 3)
{
  //如果输入不是三个参数，输出提示信息
ROS_INFO("usage: add_two_ints_client X Y");
return 1;
}
//定义句柄
ros::NodeHandle n;
//定义一个客户端，使用服务名为add_two_ints，服务类型AddTwoInts的服务
ros::ServiceClient client = n.serviceClient<bingda_practices::AddTwoInts>("add_two_ints");
//定义一个服务类型AddTwoInts的结构体变量srv
bingda_practices::AddTwoInts srv;
//给结构体变量赋值即服务类型AddTwoInts的分割线上的成员变量
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);
//如果结构体变量srv收到回应则输出回应的分割线下的成员变量值
if (client.call(srv))
{
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}

return 0;
}

修改配置文件CMakeLists.txt

在文件末尾加上以下三句，设置编译规则

1
2
3

add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client bingda_practices_generate_messages_cpp)

简介

x方向为车头前进方向，y方向为向左的方向，z轴用右手定则
雷达坐标系为tf2，车体坐标系为tf1，tf2的坐标原点在tf1中的表示为（x:0.1m，y:0.0m，z:0.2m）称为偏移或者tf2相对于tf1的位置，图中tf2相对于tf1的姿态为0
欧拉角描述的是两个坐标系的姿态变化

ros中的坐标系

一.base_link

机器人本体上的坐标系，通常放在底盘中心或者车体中心：x 轴指向机器人前方，y 轴指向机器人左方，z 轴指向机器人上方

二.odom里程计坐标系

odom和map坐标系是不是重合的？
odom坐标系的原点就是机器人上电时所处的位置，所以在建图时odom坐标系和map坐标系是重合的，后面随着建图的进行，由于误差存在，odom坐标系和map坐标系会慢慢偏离。而导航时，机器人起始位置一般不会与建图时的起始位置完全相同，而map坐标系是始终不变的，所以在导航开始时，odom坐标系和map坐标系一般都不是重合的，odom:里程计坐标系，不是固定的坐标系，是可以运动的
odom和base_link坐标系的关系获取
odom-base_link坐标系关系是通过里程计信息计算出来的。比如轮子的编码器或者视觉里程计算法或者陀螺仪和加速度计，odom-base_link坐标系关系是一个短期的局域的精确坐标系，比较可靠，比如向前走了多少，向右转了多少度，里程计均会记录

三.map地图坐标系

map坐标系的位置
在构建地图时，机器人构建地图的起点位置就是map坐标系的位置。在导航节点，当加载地图时，map坐标系就确定了。 map坐标系一旦固定，就不会再变
map和base_link坐标系的关系获取
通过amcl定位模块和雷达：amcl使用粒子算法，通过激光点与地图匹配，首先求出map(父坐标)–>laser，laser在map下的位姿，然后根据已知的base_link–>laser，计算求出map->base_link
各坐标系的关系

在避障时既要使用map又要使用odom，因为odom误差会越来越大可以通过map修正，map是一个长期准确的坐标系

静态坐标系转换

实现静态坐标转换只需在launch文件中启动一个静态坐标转换的节点在功能包lauch文件夹下新建一个launch文件tf_transform.launch
代码详解
1
2
3
4
5
<launch>
<node pkg="tf" type="static_transform_publisher" name="static_transform"
args="0.1 0.0 0.2 0.0 0.0 0.0 /tf1 /tf2 20">
</node>
</launch>
调用tf功能包下的static_transform_publisher节点并传入参数，这几个参数分别是tf2相对于tf1的位姿：x，y，z三个方向的距离变换，航向(x)，俯仰(y)，横滚(z)三个角度的变换。0.1和0.2表示x轴方向偏移0.1米，z轴方向偏移0.2米，20表示发布间隔20毫秒。

动态坐标系转换py

在功能包的script下新建tf_transform.py文件

代码详解

#!/usr/bin/env python
import rospy
//导入tf的包
import tf
//导入数学包（单纯的转换不需要）
import math
//定义主函数tf_transform()
def tf_transform():
//初始化生成一个节点名字为tf_transform
rospy.init_node('tf_transform', anonymous=False)
//定义一个tf坐标广播器
tf_broadcaster = tf.TransformBroadcaster()
//设置节点运行频率10hz
rate = rospy.Rate(10) # 10hz
//利用angel变化产生一个动态坐标
angle = 0.0
//输出信息
rospy.loginfo('Start TF Transform')
//节点进入死循环
while not rospy.is_shutdown():
//做一个动态坐标
//位置
x = math.cos(angle)*0.3
y = math.sin(angle)*0.3
z = 0.2
//姿态
//将描述姿态的欧拉角（0，0，angle）转换为四元数（三个参数是分别绕x轴，y轴，z轴）
quat = tf.transformations.quaternion_from_euler(0,0,angle)
//tf坐标广播器发布动态坐标关系
//tf3(子)相对于tf1(父)的位置为x,y,z。姿态为四元数(上面用欧拉角转换成了四元数)
current_time = rospy.Time.now()
tf_broadcaster.sendTransform((x,y,z),quat,current_time,'tf3','tf1')
//举个例子
tf_broadcaster.sendTransform((x,y,z),(0,0,0,1),current_time,'tf4','tf1')
angle += 0.01
//休眠
rate.sleep()

if __name__ == '__main__':
//执行主函数tf_transform()
tf_transform()

动态坐标系转换c++

在功能包的在src下新建一个tf_transform.cpp文件

代码详解

#include <ros/ros.h>
//包含tf广播的头文件
#include <tf/transform_broadcaster.h>
//包含矩阵变换头文件
#include <tf/transform_datatypes.h>

int main(int argc, char** argv){
 // 定义生成初始化节点robot_tf_publisher
ros::init(argc, argv, "robot_tf_publisher");
//定义句柄
ros::NodeHandle n;
//节点运行频率10hz
ros::Rate r(10);
//定义一个tf广播发布器
tf::TransformBroadcaster broadcaster;
//做一个动态坐标
float angle = 0.0;
float x = 0.0;
float y = 0.0;
float z = 0.0;
//输出信息
ROS_INFO("Start TF Transform");
//节点进入死循环
while(n.ok()){
//位置
x = cos(angle)*0.3;
y = sin(angle)*0.3;
z = 0.2;
//发布动态坐标转换矩阵
//tf::createQuaternionFromRPY将描述姿态的欧拉角（0，0，angle）转换为四元数（三个参数是分别绕x轴，y轴，z轴）
//tf3(子)相对于tf1(父)的位置为x,y,z。姿态为四元数(上面用欧拉角转换成了四元数)
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::createQuaternionFromRPY(0.0,0.0,angle), tf::Vector3(x, y, z)),
ros::Time::now(),"tf1", "tf3"));
//举个例子
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(x, y, z*2)),
ros::Time::now(),"tf1", "tf4"));
//angle变化使得坐标系变化
angle += 0.01;
r.sleep();
}
}

配置文件

编辑package.xml文件加入构建依赖项

1 2	<build_depend>tf</build_depend> <build_export_depend>tf</build_export_depend>

编辑CMakeLists.txt文件在find_package中加入tf

在CMakeLists.txt文件末尾加上以下两句，设置编译规则

1 2	add_executable(tf_transform src/tf_transform.cpp) target_link_libraries(tf_transform ${catkin_LIBRARIES})