ROS入门21讲笔记（古月居）

ROS入门

古月居入门21讲

请支持正版课程，本博客只为记录学习内容与便于后来者配套学习：古月居网址

一、基础概念

二、命令行

建议在跑不同的程序时重新运行roscore，否则可能会因为参数服务器中有同名的参数导致意外的报错。

查看节点列表:rosnode list

查看话题列表：rostopic list

发布话题消息:rostopic pub <可选项>/话题名

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

话题记录: rosbag record -a -O fileName（-a: all, -O: output）

话题复现: rosbag play fileName

查看节点通信流：rqt_graph

启动功能包的某个节点：rosrun <功能包名> <节点名>

三、工作空间

Workspace，一个存放工程开发相关文件的文件夹：

• src : Source Space (代码空间) 存放开发的代码

• build : Build Space (编译空间) 存放编译过程的二进制文件

• devel : Development Space (开发空间) 对应开发中的工程文件

• install : Install Space (安装空间) 对应开发后给客户等的发布版本，与devel文件有一定重复

• catkin编译系统下的工作空间结构：

  

工作空间的创建

①创建

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace # 在src中初始化整个工作空间

注：

其中-p不是必要的，表示创建的目标目录前的父目录如果不存在，会自动创建

catkin_ws为自定义名称

②编译

$ cd ~/catkin_ws/
$ catkin_make # 只产生build和devel文件夹，要产生install文件夹还要执行 catkin_make install

③设置环境变量

$ source devel/setup.bash

④检查环境变量

$ echo $ROS_PACKAGE_PATH

创建功能包

在src文件夹下创建，src中无法直接运行代码，需要放到功能包中

①创建

$ cd ~/catkin_ws/src
$ catkin_create_pkg test_pkg std_msgs rospy roscpp
# template: $ catkin_create_pkg <package_name>[depend1][depend2][depend3]

②编译

$ cd ~/catkin_ws #（注意是在整个工作空间中编译，而不是在src中）
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash # 设置环境变量

编译完成后产生以下文件：

• include 存放头文件
• src 存放代码文件
• CMakeLists.txt 用GCC语法对编译规则进行编辑
• package.xml 存放关于依赖库，作者信息，功能包描述信息等
• 若编译的代码内容不为空，则会在工作空间下的devel/lib/功能包名对应文件夹中产生编译后的文件

注意！

• 同一工作空间下，不允许存在同名功能包
• 不同工作空间下，允许存在同名功能包

四、话题模型（Topic）

1.发布者Publisher的编程实现

C++版本

* 终端方法

①创建工作空间与功能包

创建流程与上面的流程大致相同，只有功能包名字和依赖不同

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

②编写Publisher程序

此处编写程序可以用文本文件进行编写后改为.cpp文件，放到功能包learning_topic目录下的src文件夹中。

.cpp文件代码如下：

/**
 * 该例程将发布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,以geometry_msgs::Twist为信息类型，发布名为/turtle1/cmd_vel的topic，！必须要与给定平台或程序的话题对应，发送缓冲队列的长度为10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    // 设置循环的频率
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        // 初始化geometry::msgs类型的消息实例vel_msgs
        geometry_msgs::Twist vel_msgs;
        vel_msgs.linear.x = 0.5;
        vel_msgs.angular.z = 0.2;

        // 发布消息
        turtle_vel_pub.publish(vel_msgs);
        ROS_INFO("Publish turtle velocity command[%0.2f m/s %0.2f rads/s]",vel_msgs.linear.x,vel_msgs.angular.z);
        // 按照循环延时
        loop_rate.sleep();
    }
    return 0;
}

代码思路如下：

• 初始化ROS节点
• 向ROS Master注册节点信息，包括发布的话题和话题中的消息类型
• 创建消息数据
• 按照一定频率循环发布消息

③配置Publisher代码编译规则

编译语法示例：

add_executable(节点名 src/文件名)
target_link_libraries(节点名  ${catkin_LIBRARIES})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/test_pkg_node.cpp)

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

在功能包目录learning_topic文件夹下的CmakeLsit.txt文件内，找到Build相关模块，添加以下语句：

add_executable(velocity_publisher src/velocity_publisher.cpp) # 将src/velocity_publisher.cpp编译为名为velocity_publisher的可执行文件
target_link_libraries(velocity_publisher ${catkin_LIBRARIES}) # 将编译后的文件与ROS的库做链接

④节点编译

切换路径到工作空间catkin_ws文件夹下，输入：

$ catkin_make
$ source devel/setup.bash # 添加环境变量

为了避免每次都要添加环境变量或者忘记，可以直接在home/weaki(Username)/，即~/目录下，按Ctrl+H显示隐藏文件，找到.bashrc，在文件的最后一行添加 :

source /home/<usrname>/<workspace_name>/devel/setup.bash

这里即：source /home/weaki/catkin_ws/devel/setup.bash

重新启动终端后生效。

• 编译后产生的可执行文件会存在工作空间devel文件夹下的lib中

⑤运行ROS Master

$ roscore

⑥运行节点

先运行海龟仿真器：rosrun turtlesim turtlesim_node

然后运行节点：

Template: rosrun <功能包名> <节点名>

$ rosrun learning_topic velocity_publisher

* Vscode方法

①创建工作空间

创建需要利用终端

②利用vscode打开工作空间

在工作空间目录，这里即~/catkin_ws下执行code .命令（注意.前有空格），即可打开vscode并自动识别工作空间（其中vscode需要安装相关的ROS插件，catkin_tools、CMake Tools、ROS等），会直接识别catkin环境，并且自动生成.vscode文件夹，里面保含c_cpp_properties.json、settings.json 两个文件。

• c_cpp_properties.json主要是includePath参数，当有自定义头文件时，需要在其内添加。
• setting.json主要是ROS使用python编程，python相关配置，以及其它配置。这里不用做出改动。

③创建功能包

在左侧资源管理器找到我们创建的工作区中的src文件夹，右键src选择create catkin package：

弹出的第一个框（package name）填写你的功能包名称，这个是自定义，这里命名为learning_topic；
弹出的第二个框（dependencies）填写你用到的功能包名称，这里填roscpp rospy std_msgs geometry_msgs turtlesim；

注* 如果功能包是由CMake方法创建的而不是vsode创建，那么这时想要使用vsode完成后续的操作，需要先 在src文件夹下的CMakeLists.txt中，添加set(CMAKE_EXPORT_COMPILE_COMMANDS ON)代码以用于生成compile_commands.json文件，该文件其中包含了用于编译每个文件的命令，这对于确保代码编辑器和分析工具正确理解项目结构非常有用。将这行代码添加到文件的顶部（在项目声明project()之后）是一个好习惯，因为它确保了无论项目如何变化，这个设置都会被应用。这不仅对当前的开发环境有用，也有助于确保其他使用相同代码库的开发者能够利用到相同的配置。

示意如下：

cmake_minimum_required(VERSION 3.0.2)
project(my_project)

# Enable the export of compile commands
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# Other CMake configuration like find_package(), include_directories() etc.

add_executable(my_executable src/main.cpp)
# other targets and configurations

添加完成后记得重新编译工作空间以生成compile_commands.json文件，catkin_make后会生成在build下

④创建配置.json文件

​ 1、tasks.json：按下ctrl + shfit + p输入指令tasks: configure task并点击，然后会下拉出许多选项，选择catkin_make: build 会自动生成tasks.json文件。这里需要对文件进行如下修改：

{
	"version": "2.0.0",
	"tasks": [
		{
			"type": "catkin_make",
			"args": [
				"--directory",
				"/home/weaki/catkin_ws",
				"-DCMAKE_BUILD_TYPE=RelWithDebInfo",
			    "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
			],
			"problemMatcher": [
				"$catkin-gcc"
			],
			"group": "build",
			"label": "catkin_make: build"
		}
	]
}

• 这里需要在添加 "args"中添加了"-DCMAKE_EXPORT_COMPILE_COMMANDS=ON"

• 并且需要在c_cpp_properties.json的configurations中添加 "compileCommands": "${workspaceFolder}/build/compile_commands.json"

• 这里修改是解决编译cpp可能会报错找不到头文件的问题，具体原因我还没有搞懂，懂了后续更新。

2、修改c_cpp_properties.json：

{
  "configurations": [
    {
      "browse": {
        "databaseFilename": "${default}",
        "limitSymbolsToIncludedHeaders": false
      },
      "includePath": [
        "/opt/ros/noetic/include/**",
        "/home/weaki/catkin_ws/src/learning_topic/include/**",
        "/usr/include/**"
      ],
      "name": "ROS",
      "intelliSenseMode": "gcc-x64",
      "compilerPath": "/usr/bin/gcc",
      "cStandard": "gnu11",
      "cppStandard": "c++14",
      "compileCommands": "${workspaceFolder}/build/compile_commands.json"
    }
  ],
  "version": 4
}

⑤配置Publisher代码编译规则

这里与终端方法一样

⑥节点编译

快捷键ctrl + shfit +b编译出可执行文件

编译完成后也可以选择用命令行方法来运行

⑦运行ROS Master

执行ctrl + shfit + p，输入ROS：Start启动ROS Master。

⑧运行节点

• 执行ctrl + shfit + p，输入ROS：Run a executable，然后输入turtlesim，然后输入turtlsim_node。目的启动小海龟节点。

• 执行ctrl + shfit + p，输入ROS：Run a executable，然后输入功能包名learning_topic,然后输入编译出的可执行文件名，这里为velocity_publisher。

• 就可以在终端看到小海龟画圈了。

  ---

如果要通过vscode方法生成多个功能包

• 首先在生成后，需要在已有的c_cpp_properties.json文件中的includePath中仿照已有功能包的路径形式加入新功能包的路径，这样可以解决新功能包下的源码头文件报错，如：

  "includePath": [
          "/home/weaki/catkin_ws/devel/include/**",
          "/opt/ros/noetic/include/**",
          "/home/weaki/catkin_ws/src/learning_topic/include/**",
          "/home/weaki/catkin_ws/src/learning_service/include/**", //新加的
  

• 按照前面的方法修改编译规则，编译文件后，会自动在已有的compile_commands.json文件中加入新功能包的相关配置。产生例如：

  "xxx.cpp" not found in "${workspaceFolder}/build/compile_commands.json". 'includePath' from c_cpp_properties.json in folder 'catkin_ws' will be used for this file instead.
  

  的错误时，编译文件后会自动消除。

Python版本

Python版本的代码一般放在功能包文件夹下的script文件夹中，与C++的src文件夹做区分。（Python文件不需要编译）

python文件需要右键查看属性中的权限选项卡，勾选允许作为程序执行文件

或者执行chmod +x <文件路径>

参考代码如下:

# 该例程将发布turtle1/cmd_vel话题，消息类型为geometry::Twist

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
    # ros节点初始化
    rospy.init_node('velocity_publisher',anonymous=True)
    # 创建一个Publisher,发布名为/turtle1/cmd_vel的话题，消息类型为geometry_msgs.msg::Twist,发送缓冲队列长度为10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel',Twist,queue_size=10)
    #设置循环的频率
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        # 初始化geometry_msgs.msg::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2
        # 发布信息
        turtle_vel_pub.publish(vel_msg)
        # 输出显示
        rospy.loginfo("Publish turtle velocity command[%0.2f m/s %0.2f rads/s]",vel_msg.linear.x,vel_msg.angular.z)
        # 按照循环频率延时
        rate.sleep()

if __name__ == "__main__":
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

要运行python脚本，有两种方式：

• 直接利用python解释器运行：

  python3 /home/weaki/catkin_ws/src/learning_topic/scripts/velocity_publisher.py

• 直接在ros环境下运行：

  该方法需要先在python脚本首行添加Shebang行，以指定运行脚本的解释器，否则文件会直接被当做shell脚本。以Python3的环境为例，在首行应添加：#!/usr/bin/env python3

​ 然后就可以顺利运行rosrun learning_topic velocity_publisher.py了。

2.订阅者Subscriber的编程实现

C++版本

记得配置编译规则：

/**
 * 该例程将订阅/turtle/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（注意是1的），10为缓冲队列，注册回调函数poseCallback
    ros::Subscriber pos_sub = n.subscribe("/turtle1/pose",10,poseCallback);
    //循环等待话题并进入回调函数    
    ros::spin();

    return 0;
}

思路：

• 初始化ROS节点

• 订阅需要的话题

• 循环等待话题消息，接收到消息后进入回调函数

• 在回调函数中处理消息

  （注意：在回调函数中处理的时间不能过长，不能太复杂，否则可能会接收不到处理过程中的一些消息）

Python版本

记得要将其改为可执行文件：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将订阅/turtle1/pose话题，消息类型tertlesim::Pose

import rospy
from turtlesim.msg import Pose

def 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，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

3.话题消息的定义与使用

创建msg文件并定义

在功能包所在目录下创建msg文件夹用于管理消息文件，通过touch <消息文件名称>.msg创建文件。

如：touch Person.msg

内容例如：（不属于任何一种语言类型，编译根据python或C++进行动态编译）

//定义一个人的类型的消息Person.msg
//一般在msg文件夹下
string name 
uint8 sex
uint8 age
//宏定义    
uint8 unknown = 0
uint8 male = 1
uint8 female = 2

在package.xml中添加功能包依赖

该文件在功能包目录下

编译依赖 功能包message_generation动态产生message

执行依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

加在文件接近末尾的位置。

在CMakeLists.txt添加编译选项

该文件在功能包目录下

find_package( ...... message_generation)

add_message_files(FILES Person.msg) //定义消息接口
generate_messages(DEPENDENCIES std_msgs) //接口依赖库

catkin_package( ...... message_runtime) //运行依赖

加入位置如下所示：

编译生成语言相关文件

回到工作空间根目录下进行catkin_make编译生成可执行文件，将在工作空间根目录devel中的include文件生成.h头文件。使用时需引用头文件自己定义并编译的.h。

编写发布者与订阅者

C++

Publisher:

/**
 * 该例程将发布/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，队列长度10
    ros::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;
}

Subscriber:

/**
 * 该例程将订阅/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，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

再在功能包下的CMakeLists.txt文件中添加编译规则，添加动态生成的程序进行连接的依赖：

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_messsages_cpp)

再次在工作空间根目录下进行编译。（在此编译前，需要先编译生成msg的.h头文件）

• 关闭roscore（ros master）不影响已经建立联系的Publisher和Subscriber，ros master相当于媒人，为节点间建立通讯后便不需要了，除非要访问它的参数服务器。关闭后没法再加入第三个节点与之前建立连接的节点进行通讯。

Python

Publisher:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

def velocity_publisher():
	# ROS节点初始化
	rospy.init_node('person_publisher', anonymous=True)

	# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
	person_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

Subscriber:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

def 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，注册回调函数personInfoCallback
    rospy.Subscriber("/person_info", Person, personInfoCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    person_subscriber()

五、服务模型（Client/Server）

1.客户端Client的编程实现

首先另外创建功能包learning_service:

//在工作空间的src目录下
cd ~/catkin_ws/src
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

代码思路：

• 初始化ROS节点
• 创建Client实例
• 发布服务请求数据
• 等待Server处理之后的应答结果

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的service
 	ros::service::waitForService("/spawn"); //查询系统里是否有/spawn服务，存在才能请求，阻塞型函数
     //创建名为add_turtle的客户端，请求名为/spawn、数据类型为turtle::Spawn的服务
 	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
     // 初始化turtlesim::Spawn的请求数据
 	turtlesim::Spawn srv;//定义请求数据结构为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());//c_str()返回当前字符串的首字符地址
 	return 0;
 };

Python

先执行 chmod +x <py文件路径>将其改为可执行文件

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
	# ROS节点初始化
    rospy.init_node('turtle_spawn')
	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/spawn')
    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn) # Spwan为消息类型
		# 请求服务调用，输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")#x,y,theta,name
        return response.name
    except rospy.ServiceException as e:
        print ("Service call failed: %s"%e)
if __name__ == "__main__":
	#服务调用并显示调用结果
    print ("Spwan turtle successfully [name:%s]" %(turtle_spawn()))

2.服务端Server的编程实现

demo: 给海龟发指令，接受request决定是不是要给海龟发指令。包含server和topic发布。

触发信号Trigger 可用**rossrv show std_srvs/Trigger**查看数据结构

结果如下：

weaki@weaki-Legion-Y7000P:~$ rossrv show std_srvs/Trigger
---
bool success
string message

分隔符---上方对应的是服务的接收的请求部分，下方为应答部分，Trigger服务请求部分没有定义任何变量。

思路：

• 初始化ROS节点
• 创建Server实例
• 循环等待服务请求，进入回调函数（已注册）
• 在回调函数中完成服务功能的处理，并反馈应答数据

C++

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h> //topic头文件
#include <std_srvs/Trigger.h> //server头文件

ros::Publisher turtle_vel_pub; //全局publisher
bool pubCommand = false; //标志位默认停止false
// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
         			std_srvs::Trigger::Response &res)
{
	pubCommand = !pubCommand;//标志位取反 开关
    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");
	// 设置反馈数据 数据结构来自于内置库的Trigger
	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，注册回调函数commandCallback。收到request后，立刻进入回调函数
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);
	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10。发送速度指令
	turtle_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.txt）：

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

Python

注意，ROS在Python中没有spinOnce()方法，可通过多线程来实现

先执行 chmod +x <py文件路径>将其改为可执行文件

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
import rospy
# import thread,time # 基于python2的
import threading,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
def command_thread():	#线程，判断标志位；python只有spin没有spinonce
	while True:
		if pubCommand:
			vel_msg = Twist()
			vel_msg.linear.x = 0.5
			vel_msg.angular.z = 0.2
			turtle_vel_pub.publish(vel_msg)
		time.sleep(0.1)

def commandCallback(req):
	global pubCommand
	pubCommand = 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，注册回调函数commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)
	# 循环等待回调函数
    print ("Ready to receive turtle command.")
	#基于python2的
    # thread.start_new_thread(command_thread, ())  # 新的线程，其循环与rospy.spin循环一起执行
    threading.Thread(target=command_thread).start()  # 使用 threading 模块创建线程
    rospy.spin() #循环直到收到数据，进入回调函数修改标志位 
if __name__ == "__main__":
    turtle_command_server()

3.服务数据的定义与使用

本节与话题消息的定义与使用高度相似，若有文件或配置不知道在哪里设置，可参考话题消息的定义与使用

创建srv文件并定义

在功能包目录下创建srv文件夹，srv中创建Person.srv文件并写入：

string name 
uint8 age
uint8 sex

uint8 unknown = 0
uint8 male = 1
uint8 female = 2
---   //以上是request数据，以下是response数据
string result

在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

在CMakeLists.txt文件中添加编译选项

find_package( .... message_generation)#加功能包

add_service_files(FILES Person.srv)#根据哪一个srv文件创建头文件
generate_messages(DEPENDENCIES std_msgs)#根据文件定义产生头文件

catkin_package( .... message_runtime)#添加编译依赖

编译生成相关语言文件

在工作空间根目录下catkin_make，成功后会在工作空间根目录devel中的include文件生成.h头文件，并且会有三个文件，一个是总体的Person.h，另外两个分别对应请求与答复: PersonResquest.h与PersonResponse.h。

编写服务端与客户端

C++

若使用vsode方法发现引入自定义头文件时报错，注意检查c_cpp_properties.json文件中的includePath是否包含该工作空间下的/devel/include/**路径。

Client:

/**
 * 该例程将请求/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;
    // 等待，发现/show_person服务后，创建一个服务客户端，连接名为/show_person的service
	ros::service::waitForService("/show_person");
	ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");
    // 初始化learning_service::Person的请求数据
	learning_service::Person 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;
};

Server:

/**
 * 该例程将执行/show_person服务，服务数据类型learning_service::Person
 */
#include <ros/ros.h>
#include "learning_service/Person.h"

// service回调函数，输入参数req，输出参数res
bool 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，注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
    // 循环等待请求，接收请求后调用回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();
    return 0;
}

编译文件：

再在功能包目录下的CMakeLists.txt文件中修改编译规则：

• 设置需要编译的代码和生成的可执行文件
• 设置链接库
• 添加依赖项

add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp) #动态生成的cpp文件

add_executable(person_client src/person_client.cpp)![请添加图片描述](https://img-blog.csdnimg.cn/direct/6a1be0ebc9cb4a01b50b5df0ab82e3cd.png)

target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

可以检查devel的lib文件夹中对应功能包下有没有生成的可执行文件以确认是否编译成功。

运行程序：

两个程序谁先执行都可以，因为服务端会等待请求，客户端也会等待服务。

Python

Client:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import sys
import rospy
from learning_service.srv import Person, PersonRequest

def person_client():
	# ROS节点初始化
    rospy.init_node('person_client')

	# 发现/show_person服务后，创建一个服务客户端，连接名为/show_person的service
    rospy.wait_for_service('/show_person')
    try:
        person_client = rospy.ServiceProxy('/show_person', Person)

		# 请求服务调用，输入请求数据
        response = person_client("Tom", 20, PersonRequest.male)
        return response.result
    except rospy.ServiceException as e:
        print ("Service call failed: %s" %e)

if __name__ == "__main__":
	#服务调用并显示调用结果
    print ("Show person result : %s" %(person_client()))

Server:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将执行/show_person服务，服务数据类型learning_service::Person

import rospy
from learning_service.srv import Person, PersonResponse

def 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，注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)

	# 循环等待回调函数
    print ("Ready to show person informtion.")
    rospy.spin()

if __name__ == "__main__":
    person_server()

4.参数的使用与编程方法

参数模型

• 各个节点可以全局访问参数服务器。参数文件存储在.yaml文件中

参数命令行的使用

以海龟仿真器为例：

:~$ rosparam 
rosparam is a command-line tool for getting, setting, and deleting parameters from the ROS Parameter Server.

Commands:
	rosparam set	set parameter
	rosparam get	get parameter
	rosparam load	load parameters from file
	rosparam dump	dump parameters to file
	rosparam delete	delete parameter
	rosparam list	list parameter names
:~$ rosparam list
/rosdistro # ros发行版本
/roslaunch/uris/host_172_20_184_152__32849
/rosversion # ros版本号
/run_id # 线程id
/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r
:~$ rosparam get /turtlesim/background_b
255
:~$ rosparam get /rosversion 
'1.16.0

  '
:~$ rosparam get /rosdistro 
'noetic

  '
:~$ rosparam set /turtlesim/background_b 100
:~$ rosservice call /clear "{}" # 修改参数后需要请求服务才能生效，这里是海龟仿真器对应的服务请求
:~$ rosparam dump param.yaml # 保存参数文件，保存在命令行当前路径下
:~$ rosparam load param.yaml # 可对yaml文件进行修改并加载，同样，生效需要请求服务
:~$ rosservice call /clear "{}"
:~$ rosparam delete /turtlesim/background_g
:~$ rosservice call /clear "{}"

程序实现参数操作

创建功能包：

cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

思路：

• 初始化ROS节点
• get函数获取参数
• set函数设置参数

对于获取和设置参数的方式不止这里的set与get，更多的请参考链接：http://wiki.ros.org/Parameter%20Server

C++

/**
 * 该例程设置/读取海龟例程中的参数
 */
#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;
    // 读取背景颜色参数 get("变量名"，参数值存储到哪个变量中)
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);
	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);
	// 设置背景颜色参数
	ros::param::set("/turtlesim/background_r", 255);
	ros::param::set("/turtlesim/background_g", 255);
	ros::param::set("/turtlesim/background_b", 255);
	ROS_INFO("Set Backgroud Color[255, 255, 255]");
    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/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;
}

CMakeLists.txt中修改编译规则：

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

运行：

Python

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程设置/读取海龟例程中的参数

import sys
import rospy
from std_srvs.srv import Empty

def parameter_config():
	# ROS节点初始化
    rospy.init_node('parameter_config', anonymous=True)

	# 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

	# 设置背景颜色参数
    rospy.set_param("/turtlesim/background_r", 255);
    rospy.set_param("/turtlesim/background_g", 255);
    rospy.set_param("/turtlesim/background_b", 255);

    rospy.loginfo("Set Backgroud Color[255, 255, 255]");

	# 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/clear')
    try:
        clear_background = rospy.ServiceProxy('/clear', Empty)

		# 请求服务调用，输入请求数据
        response = clear_background()
        return response
    except rospy.ServiceException as e:
        print ("Service call failed: %s" %e)

if __name__ == "__main__":
    parameter_config()

六.ROS中的坐标系管理系统

基础知识储备

坐标变换工具TF

用于查询两坐标系之间的变换关系。通过广播和监听实现。

可以记录并返回10秒内的机器人各关节坐标关系信息，以TF Tree的形式存储

海龟例程

rosrun tf view_frames是查看坐标关系的重要工具之一

sudo apt-get install ros-noetic-turtle-tf # ros-版本-功能包
roslaunch turtle_tf turtle_tf_demo.launch # .launch启动脚本文件中的诸多节点
rosrun turtlesim turtle_teleop_key # 开启键盘控制节点
rosrun tf view_frames # tf功能包提供的查看系统中所有tf关系

---

问题①：由于turtle库是基于python2的，在python3中，执行.launch文件时可能会遇到Shebang行不对导致找不到解释器。解决办法：

cd /opt/ros/noetic/lib/turtle_tf # 转到turtle_tf脚本所在的目录
ls # 列出目录内容以找到相关的Python脚本文件
sudo nano turtle_tf_broadcaster.py # 使用文本编辑器修改所有该目录下py文件的shebang行
...

#!/usr/bin/env python改为#!/usr/bin/env python3即可。

问题②：在Ubuntu 20.04上运行代码lanx@lanx:~$ rosrun tf view_frames查看tf树的时候报错如下：

Listening to /tf for 5.0 seconds
Done Listening
b'dot - graphviz version 2.43.0 (0)\n'
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/tf/view_frames", line 119, in <module>
    generate(dot_graph)
  File "/opt/ros/noetic/lib/tf/view_frames", line 89, in generate
    m = r.search(vstr)
TypeError: cannot use a string pattern on a bytes-like object

解决办法：

在命令框中输入：

sudo gedit /opt/ros/noetic/lib/tf/view_frames

然后搜索 m = r.search(vstr)（89行），在这行前面添加 vstr=str(vstr)即可。

原来为：

vstr=str(vstr)

修改后：

vstr=str(vstr)
m = r.search(vstr)

---

执行rosrun tf view_frames后会在终端当前目录下产生frames.pdf文件可以查看TF树的关系。

World坐标系：全局坐标系，不动。在海龟仿真器中为左下角的点为World坐标系原点。

其他TF工具

• rosrun tf tf_echo <坐标系1> <坐标系2>

rosrun tf tf_echo turtle1 turtle2 # 查询坐标关系
# Translation：平移
# Rotation: 旋转
#	四元数
#	欧拉角RPY(弧度制/角度值) [ , , ]绕[x,y,z]旋转的弧度/角度

• rosrun rviz rviz -d 'rospack find [包名]' /[相对路径]/[配置文件名].rviz

rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz

• rosrun rviz rviz: 使用rosrun来启动rviz包中的rviz可视化工具。
• -d参数指定rviz启动时使用的配置文件。
• rospack find turtle_tf是一个命令，用来找到turtle_tf包的安装路径。
• 然后，该路径被用来指定turtle_rviz.rviz配置文件的完整路径，通常位于该包的rviz目录下。

进入RVIZ仿真后，将fixed frame改为World,并add TF坐标。

七.TF坐标系广播与监听

TF基本的数据类型

（Quaternion, Vector, Point, Pose, Transform）

   Type         |         tf
-------------------------------------
Quaternion      |      tf::Quaternion
Vector          |      tf::Vector3
Point           |      tf::Point
Pose            |      tf::Pose
Transform       |      tf::Transform

①创建功能包：

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

②编写广播器与监听器

TF广播器

广播坐标系之间的关系。

思路：

• 定义TF广播器(TransformBroadcaster)

• 创建坐标变换值

• 发布坐标变换(sendTransform)

C++

/**
 * 该例程产生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; //4x4矩阵 T
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); //平移参数
	tf::Quaternion q; //旋转
	q.setRPY(0, 0, msg->theta); //姿态变化，通过欧拉角的方式设置四元数
	transform.setRotation(q);
	// 广播world与海龟坐标系之间的tf数据 StampedTransform(变换矩阵，时间戳，坐标系1，坐标系2)
	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];
    /*
       argc 表示传递给程序的参数数量（包括程序名本身）
       argv 是参数值的数组，argv[0] 是程序的名称，argv[1] 是第一个传递给程序的参数，依此类推。
       如果用户没有提供参数，或者提供了多余一个参数（这使得argc不等于2），程序会通过ROS_ERROR打印一个错误		  消息：“need turtle name as argument”，意思是“需要海龟的名字作为参数”，然后返回-1，表示异常退出。
    */
	// 订阅海龟的位姿话题
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    // 循环等待回调函数
	ros::spin();
	return 0;
};

添加编译规则：

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

Python

记得更改文件执行权限

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程产生tf数据，并计算、发布turtle2的速度指令

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def 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()

TF监听器

获取任意两个坐标系之间关系

思路：

• 定义TF监听器 TransformListener
• 查找坐标变换 waitForTransform、lookupTransform

C++

/**
 * 该例程监听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;
	// 请求产生turtle2
	ros::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
		{
             // waitForTransform(坐标系1，坐坐标系2，查询（当前）时间，等待时间)如果存在关系则程序往下走（代码核心）
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
             // lookupTransform(坐标系1，坐标系2，查询时间，结果保存于)
			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());
        // 欧式距离除以2得到线速度，运动时间为2秒
		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;
};

添加编译规则：

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

Python

记得更改文件权限

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程创建第二只海龟并创建监听器获取两只海龟的坐标关系，并且使得海龟2跟随第一只海龟运动

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __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):
            continue

        angular = 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 = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

③运行例程

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1 #重映射，重新命名
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
$ rosrun learning_tf turtle_tf_listener
$ rosrun turtlesim turtle_teleop_key

这里因为同一节点运行两次，因此为了避免重名造成不必要的问题，这里进行重映射，__name:=turtle1_tf_broadcaster，[这里的名字取代掉程序中的这个部分](#replace here):ros::init(argc, argv, "my_tf_broadcaster");该部分为节点初始化。重映射后面的/turtle1是输入参数，传入argv[1]中。argv[0]为程序名，这里为重映射后的程序名。

注意，对于Python程序，运行带有重映射的命令时需要稍作修改：

$ rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1
$ rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2

八.launch启动文件的使用方法

XML常用语法：

<launch> launch文件中的根元素采用<launch>标签定义</launch>
<node> 启动节点
   <node pkg="package-name" type="executable-name" name="node-name" />
         pkg：节点所在功能包名称
         type：节点的可执行文件名称
         name：节点运行时的名称 会取代文件中初始化的节点名，同文件多命名以实现该程序的多次利用
    其他可选参数：
         output：节点是否要打印日志信息
         respawn：如果节点挂掉是否要进行重启
         required：某个节点是否必须要启动
         ns：namespace，避免命名冲突
         args：输入参数
    </node>

<param> 设置ROS运行中的一个参数，存储在参数服务器中
    <param name="output_frame" value="odom"/>
    name：参数名
    value：参数值
    </param>
    
<rosparam>加载参数文件中的多个参数
    <rosparam file="params.yaml" command="load" ns="params" />
    </rosparam>
    
<arg> launch文件内部的局部变量，仅限于launch文件使用
    <arg name="arg-name" default="arg-value" />
    name：参数名
    value：参数值
    </arg>
    调用
    <!--
    <param name="foo" value=$(arg arg-name)" />
    <node name="node" pkg="package" type="type" args="$(arg arg-name)" />  
       -->

<remap>重映射ROS计算图资源的命名
    <remap from="/turtlebot/cmd_vel" to="/cmd_vel" />
    from :原命名
    to：映射之后的命名
        </param>
    
<include>包含其他launch文件，类似于C语言中的头文件包含
    <include flle="$(dirname)/other.launch" />
    file：包含的其他launch文件路径
    </include>

注意，映射完后原资源就不复存在了

更多标签参见：https://wiki.ros.org/roslaunch/XML

示例

如果利用vscode创建launch文件时系统无法识别，可以直接新建txt文件再重命名

• 创建功能包：

$ catkin_create_pkg learning_launch # 不需要其他依赖，因为功能都来自各种包含的节点

• 在该功能包下创建launch文件夹方便维护，launch文件全放这里面

• 在工作空间根目录进行编译catkin_make来告诉系统加入了新的功能包

• 运行launch文件：roslaunch 功能包 文件名.launch

① simple.launch

用于开启前面的两个node节点，并将日志输出到终端

<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
</launch>
<!--开启功能包下的可执行文件的某某节点并于终端输出日志-->

② turtlesim_parameter_config.launch

• 用于配置参数
• 先在功能包目录下，创建config文件夹并创建param.yaml配置文件，文件内容如下（其中group为一个命名空间，名称可自定义）：

A: 123
B: "hello"

group:
  C: 456
  D: "hello"

<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="lo![外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传](https://img-home.csdnimg.cn/images/20230724024159.png?origin_url=ROS%E5%85%A5%E9%97%A8.assets%2FScreenshot%20from%202024-03-13%2014-27-57.png&pos_id=img-tfkoYWae-1719289657410)ad"/>
	</node>
    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>
</launch>
<!-- $(find learning_launch)系统搜索功能包 -->

运行后，再执行$ rosparam list可以查看到参数服务器中的参数如下，与代码对照学习：

PARAMETERS
 * /rosdistro: noetic
 * /rosversion: 1.16.0
 * /turtle_number: 2 # 不在节点turtlesim_node下
 * /turtlesim_node/A: 123
 * /turtlesim_node/B: hello
 * /turtlesim_node/group/C: 456 # 在节点下的group命名空间中
 * /turtlesim_node/group/D: hello
 * /turtlesim_node/turtle_name1: Tom
 * /turtlesim_node/turtle_name2: Jerry

NODES
  /
    turtle_teleop_key (turtlesim/turtle_teleop_key)
    turtlesim_node (turtlesim/turtlesim_node)

auto-starting new master

③ start_tf_demo_c++.launch

用于启动海龟跟随功能（C++版）

<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>

④ start_tf_demo_py.launch

用于启动海龟跟随功能（Python版）

<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>

⑤ turtlesim_remap.launch

测试重映射以及include

<launch>
	<include file="$(find learning_launch)/launch/simple.launch" /> <!--启动这个launch文件所有内容-->
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
	</node>
</launch>

九.总结与进阶

导航建图定位：

机械臂运动控制：

各种ROS包适用于快速开发原型，想要实际应用需要进一步研究原理和代码实现。

学习资源整理：

• ROS Wiki有对ROS中功能包和对应算法，开发者等详细信息。
• ROS Con是相关的会议，能够了解ROS相关的前沿研究
• ROS Robots等可用于机器人选型，有一些已有型号的驱动等配套

• ROS相关书籍，学习应用方法

• 机器人学等相关书籍，适合学习原理（推荐第一行左数第二本，难度适中，全面）

• 搜索引擎

十. 学完21讲该学什么

• 多研究已有项目的源代与例程

十一. ROS在机器人中是如何运行的

Turtlebot官网：https://emanual.robotis.com/docs/en/platform/turtlebot3/quick-start/

鸣谢

本文引用了如下作者的部分内容
CSDN @ 我绕过山腰雨声敲敲 @ lingruisheng