ROS编程实现

1. 创造工作空间和功能包

工作空间（workspace）是一个存放工程开发相关文件的文件夹

src：代码空间（Source Space），用来放置功能包的代码、配置文件、launch文件

build：编译空间（Build Space），编译过程中产生的中间文件，二进制文件，不用考虑

devel：开发空间（Development Space）放置开发中编译生成的可执行文件，一些库、脚本

install：安装空间（Install Space），安装指令/程序的位置，放置开发后生成的可执行文件，和devel文件夹类似

image-20220107221102273

1.1 创建工作空间

创建工作空间

1
2
3
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

编译工作空间

1
2
cd ~/catkin_ws/
catkin_make

设置环境变量

1
source devel/setup.bash

检查环境变量

1
echo $ROS_PACKAGE_PATH   # 显示ROS功能包路径，只有前面source之后才能通过echo命令显示出来

如果想产生install文件夹，可以通过catkin_make install编译后即可产生install安装空间，存放最终编译的可执行文件

image-20220107223553725

1.2 创建功能包

创建功能包

1
2
cd ~/catkin_ws/src
catkin_create_pkg test_pkg std_msgs rospy roscpp catkin_create_pkg <package_name> [depend 1][depend 2][depend 3]

test_pkg编译过程中用到哪些库，就需要跟哪些依赖，比如std_msgs、rospy、roscpp，这些库都放到src文件夹中。

编译功能包

1
2
3
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

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

编译过后，C++头文件可以放到include文件夹中，功能包中一定有CMakeLists.txt和package.xml，这两个文件说明该文件夹为功能包。

package.xml的作用：记录该功能包相关的信息、个人信息、开源许可证、功能包依赖信息等，如果需要增加相应的依赖，可以再进行拓展。

CMakeLists.txt的作用：描述功能包的编译规则，主要的语法是Cmake语法。

2. 发布者Publisher的编程实现

发布者编程实现原理图：

image-20220108001134600

2.1 创建功能包

具体操作步骤：

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

image-20220108001818372

2.2 配置发布者代码编译规则

如何配置CMakeLists.txt中的编译规则：

  • 设置需要编译的代码和生成的可执行文件；
  • 设置链接库；
  • CMakeLists.txt中主要描述了如何编译以及编译的规则。

在~/catkin_ws/src/learning_topic文件夹下的CMakeLists.txt文件中加入如下代码：

1
2
add_executable(velocity_publisher src/velocity_publisher.cpp) ##将src/velocity_publisher.cpp编译成名为velocity_publisher的可执行文件
target_link_libraries(velocity_publisher ${catkin_LIBRARIES}) ##将可执行文件velocity_publisher和ROS相关的库做一个链接，比如C++的接口#python不用做接口，python不需要编译

image-20220108232013967

2.3 用C++/python实现发布者

在src文件夹中创建cpp文件

C++代码实现一个发布者：

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

具体实现的C++代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
/*
*该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs:Twist
*/

#include <ros/ros.h>    //包含ROS相关的头文件
#include <geometry_msgs/Twist.h>  //ROS中线速度和角速度相关消息的头文件，geometry_msgs库中已经定义好的头文件

int main(int argc, char **argv)
{
    // ROS节点初始化，
    ros::init(argc, argv, "velocity_publisher"); //velocity_publisher为节点的名字，在ROS中节点名不能重复。                             //argc和argv为输入的参数来设置初始化的属性，默认情况下属性不作配置，只有velocity_publisher这个名字
    
    // 创建节点句柄，主要用来管理ROS相关API的资源，比如发布者创建API调用需要节点句柄的调用，用来管理节点资源。
    ros::NodeHandle n;
    
    /*创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10。Publisher里面有一个缓存（队列），先把数据放入缓存中，再根据实际传输能力传输数据，当缓存空间不够或者传输速度太慢，会将最老的数据删除，只保留缓存长度的数据，再进行数据传输，出现这种情况时，会出现掉帧掉数据的情况*/
    ros==Publisher turtle_vel_pub = n.advertise<geometry_msgs==Twist>("/turtle1/cmd_vel",10);
    
    // 设置循环的频率，后续有一个while循环，loop_rate用来设置频率的
    ros::Rate loop_rate(10);
    
    int count = 0;
    while (ros::ok()) //封装数据，发不出去，延时满足频率，进行下一次循环
    {
        // 初始化geometry_msgs::Twist类型的消息，
        geometry_msgs::Twist vel_msg; //设置Twist消息的内容，首先创建Twist消息，Twist是一个类，首先创建类的对象，对象中包括线速度角速度
        vel_msg.linear.x = 0.5;  //设置线速度
        vel_msg.angular.z = 0.2;  //设置角速度
        
        // 发布消息
        turtle_vel_pub.publish(vel_msg); //调用发布者turtle_vel_pub，使用publish这个方法，publisher的输入是vel_msg
        ROS_INFO("Publish turtlesim velocity command[%0.2f m/s, %0.2f rad/s]",vel_msg.linear.x, vel_msg.angular.z);
        //ROS_INFO相当于printf，用来输出信息
        
        // 按照循环频率演示
        loop_rate.sleep();//延时10Hz
    }
    
    return 0;
}

python代码实现一个发布者：

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

具体实现的python代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist

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

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::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::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_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

2.4 编译并运行发布者

编译具体操作如下：

1
2
3
4
5
6
cd ~/catkin_ws
catkin_make   # 用该指令进行编译
source devel/setup.bash   # 设置环境变量，可以将该句程序放入.bashrc文件的最后一行
roscore
rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_publisher # 运行learning_topic文件中的velocity_publisher节点

image-20220108234447167

python文件放在scripts文件夹中，同时注意要设置python文件具有可执行文件。

image-20220109001406674

3. 订阅者Subscriber的编程实现

订阅者编程实现原理图：

image-20220109002944532

3.1 配置发布者代码编译规则

如何配置CMakeLists.txt中的编译规则：

  • 设置需要编译的代码和生成的可执行文件；
  • 设置链接库

在~/catkin_ws/src/learning_topic文件夹下的CMakeLists.txt文件中加入如下代码：

1
2
add_executable(pose_subscriber src/pose_subscriber.cpp) # 将src/pose_subscriber.cpp编译成名为pose_subscriber的可执行文件
target_link_libraries(pose_subscriber ${catkin_LIBRARIES}) # 将可执行文件pose_subscriber和ROS相关的库做一个链接，比如C++的接口

image-20220109010041310

3.2 用C++/python实现订阅者

C++代码实现一个订阅者：

  • 初始化ROS节点；
  • 订阅需要的话题；
  • 循环等待话题消息，接收到消息后进入回调函数；
  • 在回调函数中完成消息处理。

具体实现的C++代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose，这是小海龟仿真器自己定义的信息
 */
 
#include <ros/ros.h>
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数，turtlesim::Pose是消息的结构，后面的ConstPtr调用的是长指针
//当msg数据传输过来的时候，就以长指针的形式指向turtlesim::Pose这个姿态信息的数据内容
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这个类，创建一个pose_sub这个对象，订阅名为/turtle1/pose的topic，需要和turtlesim相对应，注册回调函数poseCallback
    //其中10为订阅话题的队列长度，poseCallback为回调函数，因为数据不知道什么时候发送过来，因此有回调函数，当有数据进来，就调用回调函数进行处理
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数，死循环，不断查看队列中是否有数据过来，如果有则进入poseCallback进行处理，如果没有则不断地死循环
    ros::spin();

    return 0;
}

python代码实现一个订阅者：

  • 初始化ROS节点；
  • 订阅需要的话题；
  • 循环等待话题消息，接收到消息后进入回调函数；
  • 在回调函数中完成消息处理。

具体实现的python代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::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()

python可以不用编译，直接可以运行。

3.3 编译并运行发布者

编译具体操作如下：

1
2
3
4
5
6
7
cd ~/catkin_ws
catkin_make   # 用该指令进行编译
source devel/setup.bash   # 设置环境变量，可以将该句程序放入.bashrc文件的最后一行
roscore
rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_subscriber#运行learning_topic文件中的velocity_subscriber节点,如果小海龟不移动，则订阅的小海龟的位置不变。
rosrun turtlesim turtle_teleop_key # 运行键盘遥控节点，控制小海龟移动，可以看到订阅的小海龟位置发生改变

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

4.1 自定义消息文件的配置

步骤一：定义msg文件

在learning_topic文件夹中创建msg文件夹，跟消息相关定义都存放在msg文件夹中，方便管理，创建文件person.msg，打开文件，将下列文件放入person.msg文件中。

1
2
3
4
5
6
7
string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

步骤二：在package.xml中添加功能包依赖

在learning_topic文件夹中打开package.xml文件，添加下面的语句

1
2
<build_depend>message_generation<build_depend>  
<exec_depend>message_runtime</exec_depend>

一个是编译依赖，编译依赖是一个动态产生message的功能包；一个是执行依赖，执行依赖是一个执行message的功能包。如下图：

image-20220109143133953

步骤三：在CMakeLists.txt添加编译选项

  • find_package(…… message_generation)
  • add_message_files(FILES Person.msg) 将定义的Person.msg作为接口，针对其来进行编译

​ generate_messages(DEPENDENCIES std_msgs) 编译Person.msg需要ROS已有的库或者包，需要依赖std_msgs

  • catkin_package(…… message_runtime) 创建message运行依赖

分别对应CMakeLists.txt文件中的内容为：

image-20220109143527394

image-20220109144602025

image-20220109145204729

步骤四：编译生成语言相关文件

在工作空间的根目录catkin_ws下通过catkin_make进行编译，生成Person.h头文件。

image-20220109150648353

4.2 话题消息的使用

如何根据上面4.1生成的Person.h头文件，通过程序来使用它，具体操作步骤如下：

在src文件夹中创建cpp文件

4.2.1 C++代码实现发布者

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

具体实现的C++代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
/**
 * 该例程将发布/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;
}

4.2.2 C++代码实现订阅者

  • 初始化ROS节点；
  • 订阅需要的话题；
  • 循环等待话题消息，接收到消息后进入回调函数；
  • 在回调函数中完成消息处理。

具体实现的C++代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
/**
 * 该例程将订阅/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;
}

4.2.3 编译配置文件

添加订阅者和发布者相关程序后，需要对CMakeList_List.txt文件进行配置

配置CMakeLists.txt中的编译规则：

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

在catkin_ws/src/learning_topic文件夹下的CMakeList_List.txt文件中加入以下代码：

1
2
3
4
5
6
7
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)

有一些代码是动态生成的，需要将可执行文件和动态生成的程序产生依赖关系，因此需要通过add_dependencies命令来做一些连接。要和自己自定义的消息进行连接必须要添加这样一行代码。如图所示：

image-20220109153302090

在工作空间的根目录catkin_ws下，通过catkin_make进行编译。

ROS Master是帮助节点之间创建连接，一旦两者创建连接后，关闭roscore，publisher和subscriber依然发布消息和订阅消息，此时ROS Master不再起作用。

4.2.4 python代码实现

发布者python实现：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person   # 对应的msg文件也生成了python文件，通过import形式引入进来

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

订阅者python实现：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
# 该例程将订阅/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()

5. 客户端Client的编程实现

客户端Client编程实现原理图：

image-20220109162256828

5.1创建功能包

代码实现如下：

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

image-20220109162826338

image-20220109162908264

5.2 C++/python代码实现客户端

5.2.1 C++代码实现客户端

在learning_service文件夹下的src文件中，新建turtle_spawn.cpp文件，实现流程如下：

  • 初始化ROS节点；
  • 创建一个Client实例；
  • 发布服务请求数据；
  • 等待Server处理之后的应答结果。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "turtle_spawn"); //节点名为spawn

    // 创建节点句柄
	ros::NodeHandle node;

    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros==service==waitForService("/spawn");  //循环等待spawn这样一个服务，是一个阻塞式的服务，只有服务在系统中存在了，才能请求这个服务
	ros==ServiceClient add_turtle = node.serviceClient<turtlesim==Spawn>("/spawn");//创建客户端ServiceClient
    //客户端用来给spawn服务发送请求，请求的数据类型为turtlesim::Spawn，/spawn代表服务的名字。    
    
    // 初始化turtlesim::Spawn的请求数据
	turtlesim::Spawn srv;  //定义srv
	srv.request.x = 2.0;   //海龟的x轴坐标
	srv.request.y = 2.0;   //海龟的y轴坐标，没有角速度
	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);//请求的方法用call，call也是阻塞性函数，把request发出去后一直在等待服务器给反馈
    //如果一直等待超过call的时间，会提示超时

	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

	return 0;
};

5.2.2 python代码实现客户端

在learning_service文件夹下的src文件中，新建turtle_spawn.cpp文件，实现流程如下：

  • 初始化ROS节点；
  • 创建一个Client实例；
  • 发布服务请求数据；
  • 等待Server处理之后的应答结果。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
# 该例程将请求/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)

		# 请求服务调用，输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")##x轴坐标、y轴坐标、角度值、海龟的名字
        return response.name
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
	# 服务调用并显示调用结果
    print "Spwan turtle successfully [name:%s]" %(turtle_spawn())

5.3 配置客户端代码编译规则

如何配置CMakeLists.txt中的编译规则

  • 设置需要编译的代码和生成的可执行文件
  • 设置链接库
1
2
add_executable(turtle_spawn src/turtle_spawn.cpp)  //将turtle_spawn.cpp文件编译成turtle_spawn的可执行文件
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})  //将turtle_spawn链接到ROS对应的库

5.4 编译并运行客户端

回到工作空间的根目录catkin_ws文件夹下，通过catkin_make进行编译。

image-20220109173743981

在catkin_ws/devel/lib文件夹下产生的learning_service文件夹就是编译后产生的，里面的turtle_spawn文件就是编译产生的可执行文件。

1
2
3
4
5
6
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_spawn

image-20220109174700615

image-20220109174536473

注意：要将python文件的权限选择允许作为程序执行文件，如下所示。

image-20220109175841987

6. 服务端Server的编程实现

服务端Server编程实现原理图：

image-20220109180341410

6.1 用C++/python实现一个服务器

6.1.1 用C++实现一个服务器

在learning_service文件夹下的src文件中，新建turtle_command_server.cpp文件，实现流程如下：

  • 初始化ROS节点；
  • 创建一个Server实例；
  • 循环等待服务请求，进入回调函数；
  • 在回调函数中完成服务功能的处理，并反馈应答数据。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>  //topic的头文件Twist
#include <std_srvs/Trigger.h> //服务数据类型的头文件Trigger

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");

	// 设置反馈数据
	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，实例为command_service，定义service的名字为turtle_command
    //类似订阅者，因为不知道数据什么时候过来，通过回调函数commandCallback来处理request，即一旦service收到request之后立刻跳到回调函数里面
    //在回调函数里面再做针对server的处理
    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);//进行一秒钟10Hz来做topic的发布

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

6.1.2 用python实现一个服务器

在python程序中没有spinOnce这条指令，通过python中的多线程指令来完成标志位是True还是False。

在learning_service文件夹下的scripts文件中，新建turtle_command_server.py文件，实现流程如下：

  • 初始化ROS节点；
  • 创建一个Server实例；
  • 循环等待服务请求，进入回调函数；
  • 在回调函数中完成服务功能的处理，并反馈应答数据。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger

import rospy
import thread,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():	
	while True:     # 如果是True就继续，如果是False就跳过
		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."

    thread.start_new_thread(command_thread, ())
    rospy.spin()

if __name__ == "__main__":
    turtle_command_server()

6.2 配置服务器代码编译规则

如何配置CMakeList.txt中的编译规则：

  • 设置需要编译的代码和生成的可执行文件；
  • 设置链接库。

将下面的规则加入CMakeList.txt中：

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

image-20220109192708646

6.3 编译并运行服务器

6.3.1 编译运行C++程序

编译运行流程：

1
2
3
4
5
6
7
cd ~/catkin_ws
catkin_make
source devel/setup.bash  # 如果不想每次source，则需要在.bashrc中添加一行代码：source /home/yww/catkin_ws/devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_command_server
rosservice call/turtle_command "{}" #{}里面的内容是request的内容，是空的，

输入后的效果：

image-20220109194230634

再一次最后一行命令的效果：

image-20220109194523039

image-20220109194633759

6.3.2 编译运行python程序

编译运行流程：

1
2
3
4
5
6
7
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_command_server.py
rosservice call/turtle_command "{}" #{}里面的内容是request的内容，是空的，

image-20220109200517280

7. 服务数据的定义和使用

服务模型的结构图：

image-20220109234711913

7.1 如何自定义服务数据

步骤一：定义srv文件

打开catkin_ws/src/learning_service/srv文件夹，创建Person.srv文件，打开，并输入以下代码：

1
2
3
4
5
6
7
8
9
string  name
uint8   age
uint8   sex

uint8   unknown = 0
uint8   male    = 1
uint8   female  = 2
---
string result

在---上面是request，在---下面是response。

步骤二：在package.xml中添加功能包依赖

打开catkin_ws/src/learning_service文件夹中打开package.xml文件，将下面的代码加入其中：

1
2
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

image-20220110000303130

步骤三：在CMakeLists.txt添加编译选项

打开catkin_ws/src/learning_service文件夹中打开CMakeLists.txt文件中做如下修改

1
2
3
4
5
6
find_package(... message_generation)

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

catkin_package(... message_runtime)

具体实现的效果如图：

image-20220110001104459

image-20220110001211592

image-20220110001315761

步骤四：编译生成语言相关文件

回到工作空间根目录catkin_ws下，通过catkin_make进行编译。

编译后在Person.h文件中包含了PersonRequest.h和PersonResponse.h文件，用的时候包含一个Person.h就可以

image-20220110001845113

7.2 相关代码实现

7.2.1 C++相关代码实现

Client端的C++代码编写：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
/**
 * 该例程将请求/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的service
	ros==service==waitForService("/show_person");//等待show_person服务
	ros==ServiceClient person_client = node.serviceClient<learning_service==Person>("/show_person");
    //learning_service::Person是自己定义的，数据service的对象是通道/show_person

    // 初始化learning_service::Person的请求数据
	learning_service::Person srv; //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);//通过person_client.call把请求数据发出去，然后等待server的反馈结果

	// 显示服务调用结果
	ROS_INFO("Show person result : %s", srv.response.result.c_str());// 反馈收到后，显示反馈结果

	return 0;
};

代码22行中的名字和srv的文件名一致，如下图：

image-20220110004537101

Server端的C++代码编写：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
/**
 * 该例程将执行/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;
}

7.2.2 python相关代码实现

客户端相关的代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/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')

	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的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, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
	# 服务调用并显示调用结果
    print "Show person result : %s" %(person_client())
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将执行/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()

7.3 配置服务器/客户端代码编译规则

配置CMakeLists.txt中的编译规则：

  • 设置需要编译的代码和生成的可执行文件；
  • 设置链接库；
  • 添加依赖项，依赖动态生成的cpp文件。
1
2
3
4
5
6
7
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)

image-20220110010101564

7.4 编译并运行发布者和订阅者

编译运行流程：

1
2
3
4
5
6
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun learning_service person_server
rosrun learning_service person_client

image-20220110010428506

image-20220110010845829

客户端每请求一次服务端显示一次。

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

参数模型结构图：

image-20220110215606138

ROS Master中有一个参数服务器Parameter Server，是一个全局字典，用来保存各个节点之间的配置参数，各个节点都可以访问。

8.1 参数模型的使用方法

参数模型实现：

首先创建功能包：

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_ws

rosparam命令行的使用方式：

rosparam list 列出当前有多少参数

rosparam get param_key 显示某个参数值

rosparam set param_key param_value 设置某个参数值

rosparam dump file_name 保存参数到文件

rosparam load file_name 从文件读取参数

rosparam delete param_key 删除参数

rosparam 通过rosparam可以看到对该句命令的解释

image-20220110220808769

以小海龟案例为例：

1
2
3
roscore
rosrun turtlesim turtlesim_node
rosparam list

可以显示如下：

image-20220110222413859

background_b、background_g、background_r是小海龟背景的颜色值

rosdistro是当前的ros是Melodic

rosversion是小海龟当前的版本

run_id为进程的id号

如果想得到某个参数的值，可以通过rosparam get /background_b获得：

1
rosparam get /background_b

image-20220110223202858

如果想修改某个参数的值，可以通过rosparam set /background_b修改：

1
rosparam set /background_b 100

image-20220110224131388

需要发送一个请求才能把小海龟背景颜色给改变：

1
rosservice call /clear "{}"

这个请求的服务名字为clear，发送空即可，发送请求后仿真器会查询rgb值，如果有更改，会根据新的值刷新背景颜色。

image-20220110224737789

保存ros系统的参数

1
rosparam dump yww_param.yaml

回车之后会自动保存为yww_param.yaml文件，没有任何提示。

也可以更改yww_param.yaml中的数据，然后用以下命令进行加载：

1
rosparam load yww_param.yaml

修改好之后通过获取rosparam get /background_b之后的值。

8.2 用C++/python实现参数的使用

8.2.1 用C++实现参数的使用

通过以下代码实现：

1
2
cd ~/catkin_ws/src/learning_parameter/src
touch parameter_config.cpp

其中parameter_config.cpp代码的结构为：

  • 初始化ROS节点；
  • get函数获取参数；
  • set函数设置参数。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
/**
 * 该例程设置/读取海龟例程中的参数
 */
#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;
}

8.2.2 用python实现参数的使用

其中parameter_config.py代码的结构为：

  • 初始化ROS节点；
  • get函数获取参数；
  • set函数设置参数。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
# 该例程设置/读取海龟例程中的参数

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('/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的service
    rospy.wait_for_service('/clear')
    try:
        clear_background = rospy.ServiceProxy('/clear', Empty)

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

if __name__ == "__main__":
    parameter_config()

8.3 配置代码编译规则

配置CMakeLists.txt中的编译规则：

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

将以下代码加入CMakeLists.txt中：

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

image-20220110232209217

然后在工作空间catkin_ws中用catkin_make进行编译，具体步骤如下：

1
2
3
4
5
6
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config

现象：小海龟的背景颜色由蓝色变为白色。

image-20220110232902080

image-20220110232751454

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

机器人中的坐标变换（参考《机器人学导论》）：

image-20220110234826366

image-20220110235326959

通过tf管理两个坐标系。

坐标变换的示例，打开终端输入以下命令：

1
2
3
4
sudo apt-get install ros-melodic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
rosrun tf view_frames

第一句命令的意思是需要安装turtle_tf示例的功能包。

工具一：看到系统中所有tf之间的关系

rosrun tf view_frames可以可视化看到系统中所有tf之间的关系，也是调试机器人经常用到的工具，输入命令后需要监听5s，5s内所有坐标系之间的关系都保存下来。

image-20220111000720492

world坐标系就是全局坐标系，表示的是整个仿真器的坐标原点，即左下角的原点，这个坐标点是不会动的。

工具二：通过tf_echo查看两个坐标系之间的关系：

1
rosrun tf tf_echo turtle1 turtle2

这个描述了turtle2坐标系通过怎样的变换变换到turtle1坐标系。

Translation：即在xyz三个方向上的平移。

Rotation：坐标系通过怎样的旋转可以变成一样的姿态。分别进行四元数、弧度、角度单位来进行描述。

image-20220111001708097

工具三：通过rviz查看两个坐标系之间的关系：

1
rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

image-20220111002507221

两个小海龟之间的坐标系的关系，可以通过两者和world坐标系的关系计算得到，两个四元矩阵具体计算如下：

image-20220111002759743

10. tf坐标系广播与监听的编程实现

创建功能包

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

10.1 创建tf广播器代码（C++/python）

如何实现一个tf广播器

  • 定义TF广播器（TransformBroadcaster）；
  • 创建坐标变换值；
  • 发布坐标变换（SendTransform）。

在catkin_ws/src/learning_tf/src文件夹下创建turtle_tf_broadcaste.cpp文件，具体C++代码内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
/**
 * 该例程产生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) );//设置xyz方向上的平移参数，由于是平面所以z为0。
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);//设置xyz轴的旋转角度，由于是平面所以xy轴都为0，只存在z轴的旋转
	transform.setRotation(q);

	// 广播world与海龟坐标系之间的tf数据，广播之后可以在tf树中看到对应的关系
	br.sendTransform(tf==StampedTransform(transform, ros==Time::now(), "world", turtle_name));
    //tf是有时间的概念，所以会有时间戳，记录的是当前时间。
}

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, &poseCallback);

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

	return 0;
};

具体python代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
# 该例程将请求/show_person服务，服务数据类型learning_service::Person

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

10.2 创建tf监听器代码（C++/python）

如何实现一个tf监听器

  • 定义TF广播器（TransformListener）；
  • 查找坐标变换（waitForTransform、lookupTransform）。

在catkin_ws/src/learning_tf/src文件夹下创建turtle_tf_Listener.cpp文件，具体内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>  //给海龟发送速度指令
#include <turtlesim/Spawn.h>  //另外一只海龟通过Spawn来产生

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循环的频率
	while (node.ok())
	{
		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle2", "/turtle1", ros==Time(0), ros==Duration(3.0));//是否存在turtle1和turtle2坐标系
			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代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
# 该例程将请求/show_person服务，服务数据类型learning_service::Person

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() # 创建tf_listener

    # 通过Spawn产生新的海龟
    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)#创建publisher发布海龟2的速度指令

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():  # 查询坐标1和坐标2之间的坐标关系，查询的是当前时间，得到平移trans和旋转rot
        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()

10.3 配置编译规则

打开catkin_ws/src/learning_tf/src文件夹，配置CMakeLists.txt中的编译规则：

  • 设置需要编译的代码和生成 的可执行文件
  • 设置链接库
1
2
3
4
5
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

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

然后回到工作空间的根目录catkin_ws通过catkin_make进行编译。

通过以下命令运行：

1
2
3
4
5
6
7
8
9
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

上面第6、7行代码中turtle1_tf_broadcaster会取代程序中的my_tf_broadcaster，因此虽然是同一个程序，但是两个节点的名字不一样，可以运行两次。

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

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

launch文件中有很多标签，标签中有很多属性，用launch文件可以不用打开很多终端启动很多节点。

原来是通过roscore启动ROS Master，launch文件可以自动启动ROS Master。

1
2
3
4
<launch>
	<node pkg="turtlesim" name="sim1" type="turtlesim_node"/>
    <node pkg="turtlesim" name="sim2" type="turtlesim_node"/>
</launch>

基本属性：

pkg：节点所在的功能包名称

type：节点的可执行文件名称

name：节点运行时的名称

可选属性：

output：是否将某个节点的日志信息打印到终端

respawn：节点启动失败是否进行重启

required：某个节点是否一定要启动起来

ns：namespace，给每个节点一个命名空间，避免命名冲突

args：给每个节点输入一些参数来使用

image-20220111185654776

其他常用属性：

<param>/<rosparam>：设置ROS系统运行中的参数，存储在参数服务器中。

<param name="output_frame" value="odom"/>

  • name：参数名
  • value：参数值

加载参数文件中的多个参数：<rosparam file="params.yaml" command="load" ns= "params"/>

<arg>：launch文件内部的局部变量，仅限于launch文件使用，也可以给node作为输入参数

<arg name="arg-name" default="arg-value"/>

  • name：参数名
  • value：参数值

调用：

<param name="foo" value="$(arg arg-name)"/>

<node name="node" pkg="package" type="type" args="$arg arg-name"/>

两种参数设置对比：

image-20220111193459643

image-20220111193427479

image-20220111193835852

11.1 launch文件编程实现-示例一

launch文件编程实现：

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_launch

image-20220111203810852

一般在创建launch功能包之后，里面没有launch文件，通过新建launch文件夹方便管理功能包里面的各种资源。

image-20220111204053287

在launch文件夹中新建simple.launch文件，里面的内容为：

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

两个node标签分别启动两个node节点，一个是learning_topic功能包下面的talker节点，一个是learning_topic功能包下面的listener节点，talker的节点名为person_subscriber，listener的节点名为person_publisher，默认两个节点名不会打印到终端，为了保证显示效果，将其output设置为screen。

在工作空间的根目录~/catkin_ws中通过catkin_make编译一下。

1
rosluanch learning_launch simple.launch

image-20220111205437126

image-20220111205515374

可以通过图片看出，所有的节点在一个终端启动，publisher发布一个信息，subscriber订阅一个信息。

11.2 launch文件编程实现-示例二

在launch文件夹中，新建turtlesim_parameter_config.launch文件，具体内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<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>

param和rosparam都是设置变量的，第一个参数名为turtle_number，数值为2。rosparam通过查找地址，加载一个参数文件param.yaml。

$(find learning_launch)指的是搜索learning_launch文件夹的完整路径。

在learning_launch文件夹下新建config文件夹，在config文件夹下新建param.yaml文件，param.yaml具体内容如下：

1
2
3
4
5
6
A: 123
B: "hello"

group:
  C: 456
  D: "hello"

通过roslaunch learning_launch turtlesim_parameter_config.launch命令来启动launch文件。

通过rosparam list列出参数：

image-20220111215032993

image-20220111215238752

11.3 launch文件编程实现-示例三

在~/catkin_ws/src/learning_launch/launch文件夹中，新建start_tf_demo_c++.launch文件，具体内容如下：

相应的launch文件内容为：

1
2
3
4
5
6
7
8
9
10
11
12
<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>

在终端中运行命令roslaunch learning_launch start_tf_demo_c++.launch，得到如下效果：

image-20220111220735050

可以通过键盘控制小海龟1的移动，小海龟2会跟着移动。

针对python编写的launch文件如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
<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>

11.4 launch文件编程实现-示例四

在~/catkin_ws/src/learning_launch/launch文件夹中，新建turtlesim_remap.launch文件，具体内容如下：

1
2
3
4
5
6
7
8
9
<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>

一个launch文件包含另外一个launch文件（simple.launch）。

第6行代码是将/turtle1/cmd_vel改为/cmd_vel。

1
roslaunch learning_launch turtlesim_remap.launch

效果如下：/turtle1/cmd_vel变为/cmd_vel。发布一个话题，可以看到小乌龟发生移动。

image-20220111222426143

12. 常用可视化工具的使用

12.1 Qt工具箱

Qt工具箱常用工具：

日志输出工具——rqt_console

计算图可视化工具——rqt_graph

数据绘图软件——rqt_plot

图像渲染工具——rqt_image_view

例程实现：

1
2
roscore
rosrun turtlesim turtlesim_node

在终端中输入rqt_+Tab键，可以看到相关的工具，输入rqt_console命令：

image-20220111224658477

例如通过控制小海龟移动撞墙后会有warning警告：

image-20220111224647109

image-20220111225010422

通过rqt_plot命令打开rqt_plot：

image-20220111225556948

命令rqt_image_view用来显示摄像头图像，界面如下图：

image-20220111225753802

image-20220111225910615

也可以通过在终端中输入rqt来进行，可以通过选择各种插件Plugins来进行相关操作。

12.2 Rviz

Rviz是一款三维可视化工具，可以很好的兼容基于ROS软件框架的机器人平台。

  • 在rviz中，可以使用可扩展标记语言XML对机器人、周围物体等任何事物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面中呈现出来。
  • 同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等信息。
  • 总而言之，rviz通过机器人模型参数、机器人发布的传感信息等数据，为用户进行所有可监测信息的图形化显示。用户和开发者也可以在rviz的控制界面下，通过按钮、滑动条、数值等方式，控制机器人的行为。

image-20220111231005413

12.3 Gazebo

Gazebo是一款功能强大的三维物理仿真平台

  • 具备强大的物理引擎
  • 高质量的渲染
  • 方便的编程与图形接口
  • 开源免费

其典型应用场景包括：

  • 测试机器人算法
  • 机器人的设计
  • 现实场景下的回溯测试

image-20220111231618821

image-20220111231715803

Gazebo的启动：

通过命令roslaunch gazebo_ros可以看到相关launch文件。

13. 课程总结与进阶攻略

参考链接：

http://wiki.ros,org/ros_control

http://wiki.ros,org/gmapping/

http://wiki.ros,org/hector_slam

image-20220111233417230

image-20220111233509788

斯坦福大学公开课——机器人学：https://www.bilibili.com/video/av4506104

台湾交通大学——机器人学：https://www.bilibili.com/video/av18516816

Andrew Davison的机器人学讲座课程：http://www.doc.ic.ac.uk/~ajd/Robotics/index.html

ETH - Robotic Systems Lab：http://www.rsl.ethz.ch/education-students/lectures.html

古月学院：PC：https://class.guyuehome.com 手机：“古月居”微信公众号→古月学院

ROS：https://www.ros.org/

ROS Wiki：http://wiki.ros.org/

ROS Con 2012 ~2019：https://roscon.ros.org/world/2021/

ROS Robots：https://robots.ros.org/

Ubuntu Wiki：https://wiki.ubuntu.org.cn/

古月居：https://www.guyuehome.com

古月居泡泡：https://www.guyuehome.com/Bubble

古月学院：https://class.guyuehome.com

image-20220111235717355

image-20220111235740995