古月居ROS入门21讲学习笔记

0、Ubuntu18.04 ROS安装：

链接：https://blog.csdn.net/weixin_45702256/article/details/109329644

一、基础语言

for循环：
c++:

#include<iostream>
using namespace std;

int main()
{
    int a=5;
    for(a;a<10;a++)
    {
        count<"a="<<a<<endl;
    }
    return 0;
}

python:

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

while循环：
python

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

面向对象：
C++

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

python：

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

python：

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

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

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

二、开发工具

三、基本概念

查看节点列表:rosnode list
发布话题消息:rostopic pub -r 10 /话题名
发布服务请求:rosservice call /服务文件 “变量:val”
话题记录: rosbag record -a -O fileName
话题复现: rosbag play fileName

四、ROS基础知识

4.1 小海龟例子

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

命令：

#查看节点
$ rqt_graph 

4.2 常用的命令行工具

4.2.1 话题相关命令

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

图片对应上述命令3,

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

#7 显示数据结构
rosmsg show 

4.2.2 服务相关命令

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

4.2.3 话题记录

用于数据的保存和复现

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

4.3 创建工作空间与功能包

创建工作空间

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

解析：<package_name>:功能包的名字
test_pkg 测试功能包 后面的依赖为用到的库(roscpp rospy std_msgs)c++、py、标准消息
这些包要建立在/home/wyh/catkin_ws/src/文件夹下

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

说明：src 文件夹下放置代码.py c++文件
include放置头文件
cmakelists.txt 编译环境

4.4 发布者Publisher的编程实现

话题模型

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

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

c++代码：

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

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

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

编译并运行：

python代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::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

4.5 订阅者Subscriber的编程实现

c++代码：

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

python代码：

#!/usr/bin/env python
# -*- 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():
	# 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()

4.6 话题消息的定义与使用

过程：1

2 packag.xml文件

3 .cmakelist文件：

message_generation

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

message_runtime

c++实现
person_publisher.cpp

/**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
#include <ros/ros.h>
#include "learning_topic/Person.h"
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_publisher");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度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;
}

person_subscriber.cpp

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

配置代码编译规则


添加依赖项：

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

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

python实现：
person_publisher.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布/person_info话题，自定义消息类型learning_topic::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

person_subscriber.py

#!/usr/bin/env python
# -*- 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()

4.7 客户端client的编程实现

话题模型


创建客户端代码

c++：

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

编译代码：

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

编译运行客户端

python代码实现：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/spawn服务，服务数据类型turtlesim::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, e:
        print "Service call failed: %s"%e
if __name__ == "__main__":
	#服务调用并显示调用结果
    print "Spwan turtle successfully [name:%s]" %(turtle_spawn())

4.8 服务端server的编程

创建服务器代码
c++：turtle_command_server.cpp

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>
ros::Publisher turtle_vel_pub;
bool pubCommand = false;
// service回调函数，输入参数req，输出参数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，发布名为/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

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

编译并运行服务器

python实现：
turtle_command_server.cpp

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/turtle_command服务，服务数据类型std_srvs/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():
	# 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()

4.9 服务数据的定义与使用

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

string name 
uint8  age
uint8  sex

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

2
代码：

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

3 在CMakeList.txt添加：

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

创建服务器代码：

c++：person_server.cpp

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

// service回调函数，输入参数req，输出参数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;
}

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

/**
 * 该例程将请求/show_person服务，服务数据类型learning_service::Person
 */

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

编译代码：

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

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

python代码：
服务器：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/show_person服务，服务数据类型learning_service::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()

客户端：

#!/usr/bin/env python
# -*- 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')
	# 发现/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())

4.10 参数的使用与编程方法

参数模型

创建功能包

参数命令行使用

修改参数： rosparm set 参数 要改成的参数值
修改后的查询更新 ： rosservice call /clear “{}”

编程方法：

c++实现：parameter_config.cpp

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

编译：

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

编译并运行

python代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程设置/读取海龟例程中的参数
import 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()

4.11 ROS中的坐标系管理系统

海龟案例：


a: rosrun tf view_frames
在主文件夹下生成frames.pdf


b:显示
tf tf_echo turtle1 turtle2
c: 利用rviz

4.12 tf坐标系广播与监听

创建功能包


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

turtle_tf_broadcaster.cpp

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

监听器：

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

• Python

广播器的编写

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务，服务数据类型learning_service::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()

监听器的编写

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/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()
    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()

4.13 launch启动文件的使用方法

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




例1：

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

例2：

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

例3：

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

例4：

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

例5：

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

4.14常用可视化工具的使用

4.14.1 rqt

4.14.2 rviz

roscore
rosrun rviz rviz

4.14.3 Gazebo

roslaunch gazebo_ros

4.15 资料学习

视频学习：https://www.bilibili.com/video/BV1zt411G7Vn?p=21

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

古-月：https://blog.csdn.net/hcx25909/category_1191901.html

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

ROS Wiki: https://wiki.ros.org/

ZhangRelay 专栏：https://blog.csdn.net/ZhangRelay