目录

0、Ubuntu18.04 ROS安装：一、基础语言二、开发工具三、基本概念四、ROS基础知识4.1 小海龟例子4.2 常用的命令行工具4.2.1 话题相关命令4.2.2 服务相关命令4.2.3 话题记录 4.3 创建工作空间与功能包4.4 发布者Publisher的编程实现4.5 订阅者Subscriber的编程实现4.6 话题消息的定义与使用4.7 客户端client的编程实现4.8 服务端server的编程4.9 服务数据的定义与使用4.10 参数的使用与编程方法4.11 ROS中的坐标系管理系统4.12 tf坐标系广播与监听4.13 launch启动文件的使用方法4.14常用可视化工具的使用4.15 资料学习

0、Ubuntu18.04 ROS安装：

链接：/weixin_45702256/article/details/109329644

一、基础语言

for循环：

c++:

#include<iostream>using namespace std;int main(){int a=5;for(a;a<10;a++){count<"a="<<a<<endl;}return 0;}

python:

for a in range(5,10):if a<10:print 'a= ',aa +=1else:break

while循环：

python

a = 5while a < 10:print 'a = ' , aa += 1

面向对象：

C++

#include <iostream>class A{public:int i;void test(){std::cout << i <<std::endl;}};int main(){A a;a.i = 10;a.test();return 0;}

python：

class A:i = 10def test(self)print self.ia = A()a.test()

python：

class A:i = 10def test(self):print self.ia = A()a.test()

a: c++代码首先要编译：c++_for.cpp

g++ c++_for.cpp -o c++_for./c++_for

二、开发工具

三、基本概念

查看节点列表:rosnode list

发布话题消息:rostopic pub -r 10 /话题名

发布服务请求:rosservice call /服务文件 “变量:val”

话题记录: rosbag record -a -O fileName

话题复现: rosbag play fileName

四、ROS基础知识

4.1 小海龟例子

1. **启动ROS Master**$ roscore2. **启动小海龟仿真器**$ rosrun turtlesim turtlesim_node3. **启动海龟控制节点** $ rosrun turtlesim turtle_teleop_key

命令：

#查看节点$ rqt_graph

4.2 常用的命令行工具

4.2.1 话题相关命令

#1显示所有节点相关信息的指令rosnode# 2 列出系统所有节点rosnode list# 3 查看某一节点的具体信息rosnode info /节点（turtlesim)

图片对应上述命令3,

# 4 rostopic命令工具rostopic# 5 打印系统当前的话题列表rostopic list# 6 通过指令给话题发布指令让海龟动起来#rostopic pub 话题名 话题消息类别 消息内容# /turtle1/cmd_vel 话题名 geometry_msgs/Twist数据内容，消息结构# "linear:.."消息数据结构的具体数据 linear线速度，angular角速度rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:x: 0.0y: 0.0z: 0.0angular:x: 0.0y: 0.0z: 0.0" # 6 pub只运行一次，所以需要在pub后加上 -r(频率） 10（一秒发布10次）rostopic pub -r 10 /turtle1/cmd_vel .....

#7 显示数据结构rosmsg show

4.2.2 服务相关命令

# 1 rosservice +要干的事的类型rosservice list# 2 再调用新的海龟 theta角度rosservice call /spawn "x: 0.0y: 0.0theta: 0.0name: ''" # 3 运行后rostopic list 将包含turtle2的内容

4.2.3 话题记录

用于数据的保存和复现

# 1 话题记录rosbag record -a -O cmd_record# -a 将所有数据保存 -O保存成压缩包 cmd_压缩包名字# 2 话题复现rosbag play cmd_record

4.3 创建工作空间与功能包

创建工作空间

建立install空间:catkin_make install 输入catkin_make 出现make -j4 -l4 没有问题

解析：<package_name>:功能包的名字

test_pkg 测试功能包 后面的依赖为用到的库(roscpp rospy std_msgs)c++、py、标准消息

这些包要建立在/home/wyh/catkin_ws/src/文件夹下

# 0 建立相关文件包catkin_create_pkg test_pkg roscpp rospy std_msgs# 1 c++程序catkin_create_pkg roscpp rosp

说明：src 文件夹下放置代码.py c++文件

include放置头文件

cmakelists.txt 编译环境

4.4 发布者Publisher的编程实现

话题模型

小海龟代码练习，建立文件功能包

catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim Created file learning_topic/package.xml

c++代码：

/*** 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist*/#include <ros/ros.h>#include <geometry_msgs/Twist.h>int main(int argc, char **argv){// ROS节点初始化ros::init(argc, argv, "velocity_publisher");//定义节点名字，不能重复// 创建节点句柄，管理节点资源ros::NodeHandle n;// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);// 设置循环的频率ros::Rate loop_rate(10);int count = 0;while (ros::ok()){// 初始化geometry_msgs::Twist类型的消息geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0.5;vel_msg.angular.z = 0.2;// 发布消息 ROS——INFO相当于输出turtle_vel_pub.publish(vel_msg);ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);// 按照循环频率延时loop_rate.sleep();}return 0;}

add_executable(velocity_publisher src/velocity_publisher.cpp)target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

编译内容放置：将.cpp编译成前面命名的可视化文件，target_link_libraries跟所需要的库做链接

编译并运行：

python代码：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twistimport rospyfrom geometry_msgs.msg import Twistdef velocity_publisher():# ROS节点初始化rospy.init_node('velocity_publisher', anonymous=True)# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)#设置循环的频率rate = rospy.Rate(10) while not rospy.is_shutdown():# 初始化geometry_msgs::Twist类型的消息vel_msg = Twist()vel_msg.linear.x = 0.5vel_msg.angular.z = 0.2# 发布消息turtle_vel_pub.publish(vel_msg)rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z)# 按照循环频率延时rate.sleep()if __name__ == '__main__':try:velocity_publisher()except rospy.ROSInterruptException:pass

4.5 订阅者Subscriber的编程实现

c++代码：

/*** 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose*/#include <ros/ros.h>#include "turtlesim/Pose.h"// 接收到订阅的消息后，会进入消息回调函数void poseCallback(const turtlesim::Pose::ConstPtr& msg){// 将接收到的消息打印出来ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);}int main(int argc, char **argv){// 初始化ROS节点ros::init(argc, argv, "pose_subscriber");// 创建节点句柄ros::NodeHandle n;// 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallbackros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);// 循环等待回调函数ros::spin();return 0;}

编译：

add_executable(pose_subscriber src/pose_subscriber.cpp)target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

python代码：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Poseimport rospyfrom turtlesim.msg import Posedef poseCallback(msg):rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)def pose_subscriber():# ROS节点初始化rospy.init_node('pose_subscriber', anonymous=True)# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallbackrospy.Subscriber("/turtle1/pose", Pose, poseCallback)# 循环等待回调函数rospy.spin()if __name__ == '__main__':pose_subscriber()

4.6 话题消息的定义与使用

过程：1

2 packag.xml文件

3 .cmakelist文件：

message_generation

add_message_files(FILES Person.msg)generation_messages(DEPENDENCIES std_msgs)

message_runtime

c++实现

person_publisher.cpp

/*** 该例程将发布/person_info话题，自定义消息类型learning_topic::Person*/#include <ros/ros.h>#include "learning_topic/Person.h"int main(int argc, char **argv){// ROS节点初始化ros::init(argc, argv, "person_publisher");// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);// 设置循环的频率ros::Rate loop_rate(1);int count = 0;while (ros::ok()){// 初始化learning_topic::Person类型的消息learning_topic::Person person_msg;person_msg.name = "Tom";person_msg.age = 18;person_msg.sex = learning_topic::Person::male;// 发布消息person_info_pub.publish(person_msg);ROS_INFO("Publish Person Info: name:%s age:%d sex:%d", person_msg.name.c_str(), person_msg.age, person_msg.sex);// 按照循环频率延时loop_rate.sleep();}return 0;}

person_subscriber.cpp

/*** 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person*/#include <ros/ros.h>#include "learning_topic/Person.h"// 接收到订阅的消息后，会进入消息回调函数void personInfoCallback(const learning_topic::Person::ConstPtr& msg){// 将接收到的消息打印出来ROS_INFO("Subcribe Person Info: name:%s age:%d sex:%d", msg->name.c_str(), msg->age, msg->sex);}int main(int argc, char **argv){// 初始化ROS节点ros::init(argc, argv, "person_subscriber");// 创建节点句柄ros::NodeHandle n;// 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallbackros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);// 循环等待回调函数ros::spin();return 0;}

配置代码编译规则

添加依赖项：

add_executable(person_publisher src/person_publisher.cpp)target_link_libraries(person_publisher ${catkin_LIBRARIES})add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)add_executable(person_subscriber src/person_subscriber.cpp)target_link_libraries(person_subscriber ${catkin_LIBRARIES})add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

python实现：

person_publisher.py

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将发布/person_info话题，自定义消息类型learning_topic::Personimport rospyfrom learning_topic.msg import Persondef velocity_publisher():# ROS节点初始化rospy.init_node('person_publisher', anonymous=True)# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)#设置循环的频率rate = rospy.Rate(10) while not rospy.is_shutdown():# 初始化learning_topic::Person类型的消息person_msg = Person()person_msg.name = "Tom";person_msg.age = 18;person_msg.sex = Person.male;# 发布消息person_info_pub.publish(person_msg)rospy.loginfo("Publsh person message[%s, %d, %d]", person_msg.name, person_msg.age, person_msg.sex)# 按照循环频率延时rate.sleep()if __name__ == '__main__':try:velocity_publisher()except rospy.ROSInterruptException:pass

person_subscriber.py

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Personimport rospyfrom learning_topic.msg import Persondef personInfoCallback(msg):rospy.loginfo("Subcribe Person Info: name:%s age:%d sex:%d", msg.name, msg.age, msg.sex)def person_subscriber():# ROS节点初始化rospy.init_node('person_subscriber', anonymous=True)# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallbackrospy.Subscriber("/person_info", Person, personInfoCallback)# 循环等待回调函数rospy.spin()if __name__ == '__main__':person_subscriber()

4.7 客户端client的编程实现

话题模型

创建客户端代码

c++：

/*** 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn*/#include <ros/ros.h>#include <turtlesim/Spawn.h>int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv, "turtle_spawn");// 创建节点句柄ros::NodeHandle node;// 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的serviceros::service::waitForService("/spawn");//阻塞型函数ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");// 初始化turtlesim::Spawn的请求数据turtlesim::Spawn srv;srv.request.x = 2.0;srv.request.y = 2.0;srv.request.name = "turtle2";// 请求服务调用ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", srv.request.x, srv.request.y, srv.request.name.c_str());add_turtle.call(srv); //阻塞型函数// 显示服务调用结果ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());return 0;};

编译代码：

add_executable(turtle_spawn src/turtle_spawm.cpp)target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

编译运行客户端

python代码实现：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawnimport sysimport rospyfrom turtlesim.srv import Spawndef turtle_spawn():# ROS节点初始化rospy.init_node('turtle_spawn')# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/spawn')try:add_turtle = rospy.ServiceProxy('/spawn', Spawn)# 请求服务调用，输入请求数据response = add_turtle(2.0, 2.0, 0.0, "turtle2")return response.nameexcept rospy.ServiceException, e:print "Service call failed: %s"%eif __name__ == "__main__":#服务调用并显示调用结果print "Spwan turtle successfully [name:%s]" %(turtle_spawn())

4.8 服务端server的编程

创建服务器代码

c++：turtle_command_server.cpp

/*** 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger*/#include <ros/ros.h>#include <geometry_msgs/Twist.h>#include <std_srvs/Trigger.h>ros::Publisher turtle_vel_pub;bool pubCommand = false;// service回调函数，输入参数req，输出参数resbool commandCallback(std_srvs::Trigger::Request &req,std_srvs::Trigger::Response &res){pubCommand = !pubCommand;// 显示请求数据ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");// 设置反馈数据res.success = true;res.message = "Change turtle command state!";return true;}int main(int argc, char **argv){// ROS节点初始化ros::init(argc, argv, "turtle_command_server");// 创建节点句柄ros::NodeHandle n;// 创建一个名为/turtle_command的server，注册回调函数commandCallbackros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);// 循环等待回调函数ROS_INFO("Ready to receive turtle command.");// 设置循环的频率ros::Rate loop_rate(10);while(ros::ok()){// 查看一次回调函数队列ros::spinOnce();// 如果标志为true，则发布速度指令if(pubCommand){geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0.5;vel_msg.angular.z = 0.2;turtle_vel_pub.publish(vel_msg);}//按照循环频率延时loop_rate.sleep();}return 0;}

编译：cmakelists

add_executable(turtle_command_server src/turtle_command_server.cpp)target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

编译并运行服务器

python实现：

turtle_command_server.cpp

ros在Python中没有spinonce方法，可通过多线程来实现

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Triggerimport rospyimport thread,timefrom geometry_msgs.msg import Twistfrom std_srvs.srv import Trigger, TriggerResponsepubCommand = False;turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)def command_thread():while True:if pubCommand:vel_msg = Twist()vel_msg.linear.x = 0.5vel_msg.angular.z = 0.2turtle_vel_pub.publish(vel_msg)time.sleep(0.1)def commandCallback(req):global pubCommandpubCommand = bool(1-pubCommand)# 显示请求数据rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)# 反馈数据return TriggerResponse(1, "Change turtle command state!")def turtle_command_server():# ROS节点初始化rospy.init_node('turtle_command_server')# 创建一个名为/turtle_command的server，注册回调函数commandCallbacks = rospy.Service('/turtle_command', Trigger, commandCallback)# 循环等待回调函数print "Ready to receive turtle command."thread.start_new_thread(command_thread, ())rospy.spin()if __name__ == "__main__":turtle_command_server()

4.9 服务数据的定义与使用

1 定义srv文件：/home/wyh/catkin_ws/src/learning_service/srv/Person.srv

string name uint8 ageuint8 sexuint8 unknown = 0uint8 male = 1uint8 female = 2---string result

2

代码：

<build_depend>message_generation</build_depend><exec_depend>message runtime</exec_depend>

3 在CMakeList.txt添加：

add_service_files(FILES Person.srv)generate_messages(DEPENDENCIES std_msgs)

创建服务器代码：

c++：person_server.cpp

//服务端/**/ * 该例程将执行/show_person服务，服务数据类型learning_service::Person*/#include <ros/ros.h>#include "learning_service/Person.h"// service回调函数，输入参数req，输出参数resbool personCallback(learning_service::Person::Request &req,learning_service::Person::Response &res){// 显示请求数据ROS_INFO("Person: name:%s age:%d sex:%d", req.name.c_str(), req.age, req.sex);// 设置反馈数据res.result = "OK";return true;}int main(int argc, char **argv){// ROS节点初始化ros::init(argc, argv, "person_server");// 创建节点句柄ros::NodeHandle n;// 创建一个名为/show_person的server，注册回调函数personCallbackros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);// 循环等待回调函数ROS_INFO("Ready to show person informtion.");ros::spin();return 0;}

客户端C++代码：person_client.cpp

/*** 该例程将请求/show_person服务，服务数据类型learning_service::Person*/#include <ros/ros.h>#include "learning_service/Person.h"int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv, "person_client");// 创建节点句柄ros::NodeHandle node;// 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的serviceros::service::waitForService("/show_person");ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");// 初始化learning_service::Person的请求数据learning_service::Person srv; //注意要跟srv的文件名相同srv.request.name = "Tom";srv.request.age = 20;srv.request.sex = learning_service::Person::Request::male;// 请求服务调用ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", srv.request.name.c_str(), srv.request.age, srv.request.sex);person_client.call(srv);// 显示服务调用结果ROS_INFO("Show person result : %s", srv.response.result.c_str());return 0;};

编译代码：

add_executable(person_server src/person_server.cpp)target_link_libraries(person_server ${catkin_LIBRARIES})add_dependencies(person_server ${PROJECT_NAME}_gencpp)add_executable(person_client src/person_client.cpp)target_link_libraries(person_client ${catkin_LIBRARIES})add_dependencies(person_client ${PROJECT_NAME}_gencpp)

python代码：

服务器：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将执行/show_person服务，服务数据类型learning_service::Personimport rospyfrom learning_service.srv import Person, PersonResponsedef personCallback(req):# 显示请求数据rospy.loginfo("Person: name:%s age:%d sex:%d", req.name, req.age, req.sex)# 反馈数据return PersonResponse("OK")def person_server():# ROS节点初始化rospy.init_node('person_server')# 创建一个名为/show_person的server，注册回调函数personCallbacks = rospy.Service('/show_person', Person, personCallback)# 循环等待回调函数print "Ready to show person informtion."rospy.spin()if __name__ == "__main__":person_server()

客户端：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport sysimport rospyfrom learning_service.srv import Person, PersonRequestdef person_client():# ROS节点初始化rospy.init_node('person_client')# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/show_person')try:person_client = rospy.ServiceProxy('/show_person', Person)# 请求服务调用，输入请求数据response = person_client("Tom", 20, PersonRequest.male)return response.resultexcept rospy.ServiceException, e:print "Service call failed: %s"%eif __name__ == "__main__":#服务调用并显示调用结果print "Show person result : %s" %(person_client())

4.10 参数的使用与编程方法

参数模型

创建功能包

参数命令行使用

修改参数： rosparm set 参数 要改成的参数值

修改后的查询更新 ： rosservice call /clear “{}”

编程方法：

c++实现：parameter_config.cpp

/*** 该例程设置/读取海龟例程中的参数*/#include <string>#include <ros/ros.h>#include <std_srvs/Empty.h>int main(int argc, char **argv){int red, green, blue;// ROS节点初始化ros::init(argc, argv, "parameter_config");// 创建节点句柄ros::NodeHandle node;// 读取背景颜色参数ros::param::get("/background_r", red);ros::param::get("/background_g", green);ros::param::get("/background_b", blue);ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);// 设置背景颜色参数ros::param::set("/background_r", 255);ros::param::set("/background_g", 255);ros::param::set("/background_b", 255);ROS_INFO("Set Backgroud Color[255, 255, 255]");// 读取背景颜色参数ros::param::get("/background_r", red);ros::param::get("/background_g", green);ros::param::get("/background_b", blue);ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);// 调用服务，刷新背景颜色ros::service::waitForService("/clear");ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");std_srvs::Empty srv;clear_background.call(srv);sleep(1);return 0;}

编译：

add_executable(parameter_config src/parameter_config.cpp)target_link_libraries(parameter_config ${catkin_LIBRARIES})

编译并运行

python代码：

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程设置/读取海龟例程中的参数import sysimport rospyfrom std_srvs.srv import Emptydef parameter_config():# ROS节点初始化rospy.init_node('parameter_config', anonymous=True)# 读取背景颜色参数red = rospy.get_param('/background_r')green = rospy.get_param('/background_g')blue = rospy.get_param('/background_b')rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)# 设置背景颜色参数rospy.set_param("/background_r", 255);rospy.set_param("/background_g", 255);rospy.set_param("/background_b", 255);rospy.loginfo("Set Backgroud Color[255, 255, 255]");# 读取背景颜色参数red = rospy.get_param('/background_r')green = rospy.get_param('/background_g')blue = rospy.get_param('/background_b')rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)#发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/clear')try:clear_background = rospy.ServiceProxy('/clear', Empty)# 请求服务调用，输入请求数据response = clear_background()return responseexcept rospy.ServiceException, e:print "Service call failed: %s"%eif __name__ == "__main__":parameter_config()

4.11 ROS中的坐标系管理系统

海龟案例：

a: rosrun tf view_frames

在主文件夹下生成frames.pdf

b:显示

tf tf_echo turtle1 turtle2

c: 利用rviz

4.12 tf坐标系广播与监听

创建功能包

创建TF广播器代码（C++）

turtle_tf_broadcaster.cpp

/*** 该例程产生tf数据，并计算、发布turtle2的速度指令*/#include <ros/ros.h>#include <tf/transform_broadcaster.h>#include <turtlesim/Pose.h>std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg){// 创建tf的广播器static tf::TransformBroadcaster br;// 初始化tf数据tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 广播world与海龟坐标系之间的tf数据br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));}int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv, "my_tf_broadcaster");// 输入参数作为海龟的名字if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}turtle_name = argv[1];// 订阅海龟的位姿话题ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &amp;poseCallback);// 循环等待回调函数ros::spin();return 0;};

监听器：

/*** 该例程监听tf数据，并计算、发布turtle2的速度指令*/#include <ros/ros.h>#include <tf/transform_listener.h>#include <geometry_msgs/Twist.h>#include <turtlesim/Spawn.h>int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv, "my_tf_listener");// 创建节点句柄ros::NodeHandle node;// 请求产生turtle2ros::service::waitForService("/spawn");ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");turtlesim::Spawn srv;add_turtle.call(srv);// 创建发布turtle2速度控制指令的发布者ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);// 创建tf的监听器tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){// 获取turtle1与turtle2坐标系之间的tf数据tf::StampedTransform transform;try{//查询是否有这两个坐标系，查询当前时间，如果超过3s则报错listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +pow(transform.getOrigin().y(), 2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;};

Python

广播器的编写

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport roslibroslib.load_manifest('learning_tf')import rospyimport tfimport turtlesim.msgdef handle_turtle_pose(msg, turtlename):br = tf.TransformBroadcaster()br.sendTransform((msg.x, msg.y, 0),tf.transformations.quaternion_from_euler(0, 0, msg.theta),rospy.Time.now(),turtlename,"world")if __name__ == '__main__':rospy.init_node('turtle_tf_broadcaster')turtlename = rospy.get_param('~turtle')rospy.Subscriber('/%s/pose' % turtlename,turtlesim.msg.Pose,handle_turtle_pose,turtlename)rospy.spin()

监听器的编写

#!/usr/bin/env python# -*- coding: utf-8 -*-# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport roslibroslib.load_manifest('learning_tf')import rospyimport mathimport tfimport geometry_msgs.msgimport turtlesim.srvif __name__ == '__main__':rospy.init_node('turtle_tf_listener')listener = tf.TransformListener()rospy.wait_for_service('spawn')spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)spawner(4, 2, 0, 'turtle2')turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)rate = rospy.Rate(10.0)while not rospy.is_shutdown():try:(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):continueangular = 4 * math.atan2(trans[1], trans[0])linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)cmd = geometry_msgs.msg.Twist()cmd.linear.x = linearcmd.angular.z = angularturtle_vel.publish(cmd)rate.sleep()

4.13 launch启动文件的使用方法

Launch文件 ：通过XML文件实现多节点的配置和启动（可自动启动ROS Master）

launch文件语法：

例1：

<launch><node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" /><node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> </launch>

例2：

<launch><param name="/turtle_number" value="2"/><node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"><param name="turtle_name1" value="Tom"/><param name="turtle_name2" value="Jerry"/><rosparam file="$(find learning_launch)/config/param.yaml" command="load"/></node><node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/></launch>

例3：

<launch><!-- Turtlesim Node--><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /><node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /><node pkg="learning_tf" type="turtle_tf_listener" name="listener" /></launch>

例4：

<launch><!-- Turtlesim Node--><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py"><param name="turtle" type="string" value="turtle1" /></node><node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py"><param name="turtle" type="string" value="turtle2" /> </node><node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" /></launch>

例5：

<launch><include file="$(find learning_launch)/launch/simple.launch" /><node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"><remap from="/turtle1/cmd_vel" to="/cmd_vel"/></node></launch>

4.14常用可视化工具的使用

4.14.1 rqt

4.14.2 rviz

roscorerosrun rviz rviz

4.14.3 Gazebo

roslaunch gazebo_ros

4.15 资料学习

视频学习：/video/BV1zt411G7Vn?p=21

古月居：/

古-月：/hcx25909/category_1191901.html

ROS Robots : /

ROS Wiki: /

ZhangRelay 专栏：/ZhangRelay