ROS

ROS 命令行的使用

以小海龟为例

启动 ROS Master
$ roscore
启动小海龟仿真器
$ rosrun turtlesim turtlesim_node
启动海龟控制节点
$ rosrun turtlesim turtle_teleop_key

用以显示系统计算图类工具

rqt_graph

显示系统中所有节点相关的指令 rosnode

将所有系统当中的节点都列出来
$ rosnode list
查看节点信息
$ rosnode info /turtlesim

话题相关的命令行工具 rostopic

话题列表
$ rostopic list
pub 发布数据给某一个 topic
$ rostopic pub 话题名 消息结构 "具体数据"

$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 
  // 海龟会动起来
  
  // -r 10 一秒钟发布 10 次指令
  rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 

消息相关 msg

查看消息结构
$ rosmsg show geometry_msgs/Twist

服务相关命令行 rosservice

查看列表
$ rosservice list

/spawn 是产生海龟

$  rosservice call /spawn "x: 2.0
y: 2.0
theta: 0.0
name: 'turtle2'" 

话题记录和复现 rosbag

记录所有数据
$ rosbag record -a -o cmd_record
复现
$ rosbag play cmd_record.bag

创建工作空间与功能包

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

• src：代码空间
• build：编译空间
• devel：开发空间
• install：安装空间

创建工作空间

$ mkdir catkin_ws
$ cd catkin_ws
$ mkdir src
$ cd src
$ catkin_init_workspace

编译工作空间

$ cd ~/catkin_ws/
$ catkin_make                                    产生 build 和 devel
$ catkin_make install                            产生 install

设置环境变量

创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg test_pkg std_msgs rospy roscpp         依赖

编译功能包

$ cd  ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

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

检查环境变量

$ echo $ROS_PACKAGE_PATH

发布者 Publisher 的编程实现

创建功能包

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

创建发布者代码（C++）velocity_publisher.cpp

如何实现一个发布者

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

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

        // 发布消息
        turtle_vel_pub.publish(vel_msg);
        ROS_INFO("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

配置发布者代码编译规则

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

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

add_executable(velocity_publisher src/velocity_publisher.cpp) 将哪一个程序文件编译成哪一个可执行文件
target_link_libraries(velocity_publisher ${catkin_LIBRARIES}) 帮助可执行文件与 ROS 的一些库做链接

编译并运行发布者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher

创建发布者代码（Python）velocity_publisher.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将发布 turtle/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("Publish 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.ROSInterrupException:
		pass

订阅者 Subscriber 的编程实现

创建订阅者代码（C++）

如何实现一个订阅者

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

/**
 * 该例程将订阅 /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，注册回调函数 poseCallback
    ros::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})

编译并运行订阅者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher

创建订阅者代码（Python）

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将订阅 /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():
	# 节点初始化
	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()

话题消息的定义和使用

自定义话题消息

如何自定义话题消息

• 定义 msg 文件

string name
uint8  sex
uint8  age

uint8 unknow = 0
uint8 male   = 1
uint8 female = 2

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

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

• 在 CMakeLists.txt 添加编译选项

find_package( ... message_generation)

add_message_files(FILES Person.msg) 发现接口，针对 Person.msg 做编译
generate_messages(DEPENDENCIES std_msgs) 编译 Person.msg 需要以来 ros 已有的包

catkin_package( ... message_runtime)

• 编译生成语言相关文件

$ catkin_make

创建发布者代码（C++）

/**
 * 该例程将发布 /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;
}

创建订阅者代码（C++）

/**
 * 该例程将订阅 /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.subcriber("/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_messages_cpp)

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

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun learning_topic person_subscriber
$ rosrun learning_topic person_publisher

创建发布者代码和订阅者代码（Python）

发布者

#!/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("Publish person message[%s %d %d]", person_msg.name, person_msg.age, person_msg.sex)

		# 按照循环频率延时
		rate.sleep()

if __name__ == '__main__':
	try:
		velocity_publisher()
	except rospy.ROSInterrupException:
		pass

订阅者

#!/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("Subscibe Person Info: name:%s  age:%d  sex:%d", msg.name, msg.age, msg.sex)

def person_subscriber():
    # 节点初始化
    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 的编程实现

创建功能包

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

创建客户端代码（C++）

如何实现一个客户端

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

/**
 * 该例程将请求 /spawn 服务，服务数据类型 turtlesim::Spawn
 */

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

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

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

    // 发现 /spawn 服务后，创建一个服务客户端，连接名为 /spawn 的 service
    ros::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 spawn 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("Spawn turtle successfully [name:%s]", srv.response.name.c_str());

    return 0;
}

配置客户端代码编译规则

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

编译并运行客户端

$ cd ~/catkin_ws/src
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_spawn

创建客户端代码（Python）

#!/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)

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

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

服务端 Server 的编程实现

创建服务器代码（C++）

如何实现一个服务器

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

/**
 * 该例程将执行 /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，输出参数 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
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

    // 创建一个 Publisher，发布名为 /turtle/cmd_vel 的 topic，消息类型为 grometry_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})

编译并运行服务器

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_service turtle_command_server
$ rosservice call /turtle_command "{}"

创建服务器代码（Python）

如何实现一个服务器

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

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将执行 /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:
		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():
	# 节点初始化
	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()

服务数据的定义与使用

自定义服务数据

如何自定义服务数据

• 定义 srv 文件（Person.srv）

string name
uint8  age
uint8  sex

uint8 unknow = 0
uint8 male   = 1
uint8 female = 2
---
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)
generate_messages(DEPENDNENCIES std_msgs)

catkin_package(...... message_runtime)

• 编译生成语言相关文件

$ catkin_make

创建服务器和客户端代码（C++）

如何实现一个服务器

• 初始化 ROS 节点
• 创建 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 information.");
    ros::spin();

    return 0;
}

客户端

/**
 * 该例程将请求 /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;
}

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

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

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

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)

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

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun learning_service person_server
$ rosrun learning_service person_client

创建客户端和服务端代码（Python）

服务端

#!/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():
	# 节点初始化
	rospy.init_node('person_server')

	# 创建一个名为 /show_person 的 server，注册回调函数 personCallback
	s = rospy.Service('/show_person', Person, personCallback)

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

if __name__ == "__main__":
	person_server()

客户端

#!/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()))

参数的使用与编程方法

参数模型

创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_parameter roscpp rospy std_srvs

参数命令行使用

YAML 参数文件

rosdistro: 'noetic

  '
roslaunch:
  uris:
    host_huipukui_virtual_machine__40585: http://huipukui-virtual-machine:40585/
rosversion: '1.16.0

  '
run_id: f0adfd20-30d2-11ee-8881-8174c73a3bf3
turtlesim:
  background_b: 255
  background_g: 86
  background_r: 69

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

编程方法（C++）paarameter_config.cpp

如何获取/设置参数

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

/**
 * 该例程设置/读取海龟例程中的参数
 */
#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("/turtlesim/background_r", red);
    ros::param::get("/turtlesim/background_g", green);
    ros::param::get("/turtlesim/background_b", blue);

    ROS_INFO("Get Background 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 Background 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 Background 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})

编译并运行发布者

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bask
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_parameter parameter_config

编程方法（Python）parameter_config.py

如何获取/设置参数

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

#!/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 Background 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 Background Color[%d, %d, %d]", red, green, blue)

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

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

	# 发现 /clear 服务后。创建一个服务客户端，连接名为 /clear 的 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）

TF 功能包能干什么

• 时间属性：默认记录 10s 中之内机器人所有坐标系之间的位置关系
• 实现机器人中繁杂的坐标系之间的管理和查询

TF 坐标变换如何实现

• 广播 TF 变换
• 监听 TF 变换

例：海龟跟随效果

$ sudo apt-get install ros-melodic-turtle-tf
$ roslaunch turtle-tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key
$ rosrun tf view_frames

命令行工具

$ rosrun tf tf_echo turtle1 turtle2

可视化工具

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

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

创建功能包

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

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

如何实现一个广播器

• 定义 TF 广播器
• 创建坐标变换值
• 发布坐标变换

/**
 * 该例程产生 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, &poseCallback);

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

    return 0;
}

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

如何实现一个 TF 监听器

• 定义 TF 监听器
• 查找坐标变换

/**
 * 该例程监听 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 {
            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;
}

配置 tf 广播器与监听器代码编译规则

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

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

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

编译并运行

$ 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

创建 tf 广播器与监听器代码（Python）

turtle_tf_broadcaster.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

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

turtle_tf_listener.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

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

launch 启动文件的使用方法

Launch 文件

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

Launch 文件语法

<launch>
    <node pkg="turtlesim" name="sim1" type="turtlesim_node"/>
    <node pkg="turtlesim" name="sim2" type="turtlesim_node"/>
</launch>

<launch>

launch 文件中的根元素采用 <launch> 标签定义

<node>

启动节点

<node pkg="package-name" type="executable-name" name="node-name"/>

• pkg：节点所在的功能包名称
• type：节点的可执行文件名称
• name：节点运行时的名称
• output、reapawn、required、ns、args

<param>/<rosparam>

设置 ROS 系统运行中的参数，存储在参数服务器中

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

• name：参数名
• alue：参数值

加载参数文件中的多个参数：

<rosparam file="params.yaml" command="load" ns="params"/>

<arg>

launch 文件内部的局部变量，仅限于 launch 文件使用

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

<remap> 重映射

重映射 ROS 计算图资源的命名

<remap from="/turtlebot/cmd_vel" to="/cmd_vel"/>

• from：原命名
• to：映射之后的命名

<include> 嵌套

包含其他 launch 文件，类似 C 语言中的头文件包含

<include file="$(dirname)/other.launch"/>

• file：包含的其他 launch 文件路径

创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_launch

Launch 示例

simple.launch

<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

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

start_tf_demo_c++.launch

 <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

<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

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

常用可视化工具的使用

Qt 工具箱

日志输出工具 —— rqt_console

计算图可视化工具 —— rqt_graph

数据绘图工具 —— rqt_plot

图像渲染工具 —— rqt_image_view

综合 —— rqt

Rviz

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

$ roscore
$ rosrun rviz rviz

Gazebo

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

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

其典型的应用场景包括

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

启动

$ roslaunch gazebo_ros willowgarage_world.launch 

课程总结与进阶攻略