（古月居）ROS 21讲笔记（三）

1.创建工作空间与功能包

• 工作空间是存放工程及相关文件的文件夹，之后我们的操作都将会在该空间下进行，在 ROS 中，我们需要建立四个标准的工作空间，分别是：
  src: 代码空间（Source Space），存放 ROS 功能包的源码文件；
  build: 编译空间（Build Space），存放编译过程中产生的缓存信息和中间文件；
  devel: 开发空间（Development Space），存放编译生成的可执行文件；
  install: 安装空间（Install Space），编译成功后，可以使用 make install 将可执行文件安装到该空间中；

1.1 创建工作空间

• 创建工作空间

// 新建一个名为catkin_new_ws的工作空间
$  mkdir -p ~/catkin_new_ws/src
$  cd ~/catkin_new_ws/src
$  catkin_init_workspace

• 编译工作空间

$  cd ~/catkin_new_ws/
$  catkin_make

• 设置环境变量

$  source devel/setup.bash 
// 查看环境变量
$  echo $ROS_PACKAGE_PATH

• 创建 install 工作空间

$  cd ~/catkin_new_ws/
$  catkin_make install

    当前创建的工作空间结构如下：

1.2 创建功能包

创建包的命令是 catkin_create_pkg <package_name> [depend1] [depend2] [depend3]，前面是包名，后面是依赖包，下面是一个例子：

// 创建功能包
$  cd ~/catkin_ws/src
$  catkin_create_pkg <package_name> [depend1] [depend2] [depend3]
// 编译功能包

注意：在同一工作空间下，不能存在同名功能包；但在不同工作空间下，可以存在同名功能包

2.话题编程

2.1 发布者Publisher的编程实现

• 以小乌龟速度为例，话题模型（Publisher/Subscriber）如下：
  

• 先创建功能包

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

• 再往功能包下的 src 中添加的 cpp 文件，该文件实现了发布者。

//实现发布者的步骤：
1、初始化ROS节点；
2、向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
3、创建消息数据；
4、按照一定频率循环发布消息

// 该例程将发布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, "my_velocity_publisher");
	// 创建节点句柄
	ros::NodeHandle n;
	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度20
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 20);
	// 设置循环的频率
	ros::Rate loop_rate(10);
	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist velocity_msg;
		velocity_msg.linear.x = 0.2;
		velocity_msg.angular.z = 0.1;

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

• 在添加了源文件后，我们需要修改编译规则，进行编译，

//配置CMakeLists.txt中的编译规则
// 1、设置需要编译的代码和生成的可执行文件
add_executable(my_velocity_publisher src/my_velocity_publisher.cpp)
// 2、设置链接库
target_link_libraries(my_velocity_publisher ${catkin_LIBRARIES})

• 编译并运行发布者

$ cd ~/catkin_new_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun my_topic my_velocity_publisher

2.2 订阅者Subscriber的编程实现

• 模型如下：
  

• 再次往之前创建的功能包下的 src 中添加的 cpp 文件，该文件实现了订阅者。

//实现订阅者的步骤：
1、初始化ROS节点；
2、订阅需要的话题；
3、循环等待话题消息，接收到消息后进入回调函数；
4、在回调函数中完成消息处理

// 该例程将订阅/turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const geometry_msgs::Twist& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subscriber turtle velocity [%0.6f m/s, %0.6f rad/s]", 
				msg.linear.x, msg.angular.z)
}

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

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

    // 创建一个Subscriber，订阅名为/turtle1/cmd_vel的topic，注册回调函数poseCallback
    ros::Subscriber velocity_sub = n.subscribe("/turtle1/cmd_vel", 10, poseCallback);

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

    return 0;
}

• 在添加了源文件后，我们需要修改编译规则，进行编译，

//配置CMakeLists.txt中的编译规则
// 1、设置需要编译的代码和生成的可执行文件
add_executable(my_velocity_subscribersrc/my_velocity_subscriber.cpp)
// 2、设置链接库
target_link_libraries(my_velocity_subscriber ${catkin_LIBRARIES})

• 编译并运行订阅者者

$ cd ~/catkin_new_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun my_topic my_velocity_subscriber

2.3 话题消息的定义及使用

• 话题的模型如下：
  
• 自定义话题消息的步骤

// 1、定义msg文件

// 2、在package.xml中添加功能包依赖
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

// 3、在CMakeLists.txt添加编译选项
find_package(...  message_generation)
add_message_files(FILES Fruit.msg)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(... message_runtime)

// 4、编译生成语言相关文件

// msg 文件
string name
uint8 color
uint8 weight

uint8 others = 0
uint8 red = 1
uint8 green = 2

发布者文件: fruit_publisher.cpp

/**
 * 该例程将发布/fruit_info话题，消息类型my_topic::Fruit
 */
 
#include <ros/ros.h>
#include my_topic/Fruit

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

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

	// 创建一个Publisher，发布名为/fruit_info的topic，消息类型为my_topic::Fruit，队列长度10
	ros::Publisher fruit_pub = n.advertise<my_topic::Fruit>("/fruit_info", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化my_topic::Fruit类型的消息
		my_topic::Fruit fruit_msg;
		fruit_msg.name   = "apple";
		fruit_msg.size   = 10;
        fruit_msg.weight = 2;
        fruit_msg.color  = my_topic::Fruit::red;

	    // 发布消息
		fruit_pub.publish(fruit_msg);
		ROS_INFO("Publish Fruit Info: name:%s weight:%d color:%d",
                fruit_msg.name.c_str(), fruit_msg.weight, fruit_msg.color);

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

	return 0;
}

订阅者文件: fruit_subscriber.cpp

// 该例程将订阅/fruit_info话题，消息类型my_topic::Fruit
 
#include <ros/ros.h>
#include my_topic/Fruit

// 接收到订阅的消息后，会进入消息回调函数
void fruitInfoCallback(const my_topic::Fruit& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subscrib Fruit Info: name:%s weight:%d color:%d",
            msg.name.c_str(), msg.weight, msg.color);
}

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

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

    // 创建一个Subscriber，订阅名为/fruit_info的topic，注册回调函数fruitInfoCallback
    ros::Subscriber fruit_sub = n.subscribe("/fruit_info", 10, fruitInfoCallback);

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

    return 0;
}

• 配置编译规则，进行编译，

// 1、设置需要编译的代码和生成的可执行文件
add_executable(fruit_publisher src/fruit_publisher.cpp)
add_executable(fruit_subscriber src/fruit_subscriber.cpp)

// 2、设置链接库
target_link_libraries(fruit_publisher ${catkin_LIBRARIES})
target_link_libraries(fruit_subscriber ${catkin_LIBRARIES})

// 3、添加依赖项
add_dependencies(fruit_publisher ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(fruit_subscriber ${PROJECT_NAME}_generate_messages_cpp)

• 编译并运行

$ cd ~/catkin_new_ws/
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun my_topic fruit_publisher
$ rosrun my_topic fruit_subscriber

结果如下：

3.服务编程

3.1 客户端Client的编程实现

小乌龟客户端/服务器的模型如下

• 创建功能包

$ cd ~/catkin_new_ws/src
$ catkin_create_pkg my_service roscpp rospy std_msgs geometry_msgs turtlesim

• 再往功能包下的 src 中添加的 cpp 文件，该文件实现了客户端

//实现客户端的步骤：
1、初始化 ROS 节点；
2、创建一个 Client 实例
3、发布服务请求数据；
4、等待Server处理之后的应答结果

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

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

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "my_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;
};

• 在添加了源文件后，我们需要修改编译规则，进行编译，

//配置CMakeLists.txt中的编译规则
// 1、设置需要编译的代码和生成的可执行文件
add_executable(my_turtle_spawn src/my_turtle_spawn.cpp)
// 2、设置链接库
target_link_libraries(my_turtle_spawn ${catkin_LIBRARIES})

• 编译并运行发布者

$ cd ~/catkin_new_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun my_service my_turtle_spawn

运行结果如下：

3.2 服务端Server的编程实现

小乌龟客户端/服务器的模型如下

• 再往之前创建的功能包下的 src 中添加的 cpp 文件，该文件实现了服务器

//实现客户端的步骤：
1、初始化 ROS 节点；
2、创建 Server 实例；
3、玄幻等待服务请求，进入回调函数；
4、在回调函数中完成服务功能的处理，并反馈应答数据

// 该例程将执行/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.txt中的编译规则
// 1、设置需要编译的代码和生成的可执行文件
add_executable(turtle_command_server src/turtle_command_server.cpp)
// 2、设置链接库
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

• 编译并运行发布者

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

运行结果如下：

3.3 服务数据的定义与使用

• 服务的模型如下：
  

• 自定义服务数据的步骤

// 1、定义srv文件

// 2、在package.xml中添加功能包依赖
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

// 3、在CMakeLists.txt添加编译选项
find_package(...  message_generation)
add_service_files(FILES Fruit.srv)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(... message_runtime)

// 4、编译生成语言相关文件

// msg 文件
string name
uint8 color
uint8 weight

uint8 others = 0
uint8 red = 1
uint8 green = 2

服务器文件: fruit_server.cpp

// 该例程将执行/show_fruit服务，服务数据类型my_service::Fruit

#include <ros/ros.h>
#include "my_service/Fruit.h"

// service回调函数，输入参数req，输出参数res
bool fruitCallback(my_service::Fruit::Request  &req,
         		   my_service::Fruit::Response &res)
{
    // 显示请求数据
    ROS_INFO("Fruit: name:%s weight:%d color:%d", req.name.c_str(),req.weight,req.color);

	// 设置反馈数据
	res.result = "Show successfully!";

    return true;
}

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

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

    // 创建一个名为/fruit_server的server，注册回调函数fruitCallback
    ros::ServiceServer fruit_service = n.advertiseService("/fruit_server", fruitCallback);

    // 循环等待回调函数
    ROS_INFO("Ready to show fruit info.");
	ros::spin();

    return 0;
}

客户端文件: fruit_client.cpp

// 该例程将请求/show_fruit服务，服务数据类型my_service::Fruit

#include <ros/ros.h>
#include "my_service/Fruit.h"

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

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

    // 发现/show_fruit服务后，创建一个服务客户端，连接名为/show_fruit的service
	ros::service::waitForService("/show_fruit");
	ros::ServiceClient fruit_client = node.serviceClient<my_service::Fruit>("/show_fruit");

    // 初始化my_service::Fruit的请求数据
	my_service::Fruit srv;
	srv.name   = "banana";
	srv.weight = 2;
	srv.color  = my_service::Fruit::Request::green;

    // 请求服务调用
	ROS_INFO("Call service to show fruit: name:%s weight:%d color:%d", 
			 srv.request.name.c_str(), srv.request.weight, srv.request.color);

	fruit_client.call(srv);

	// 显示服务调用结果
	ROS_INFO("Show fruit result:%s", srv.response.result.c_str());

	return 0;
}

• 配置编译规则，进行编译，

// 1、设置需要编译的代码和生成的可执行文件
add_executable(fruit_server src/fruit_servercpp)
add_executable(fruit_client src/fruit_client.cpp)

// 2、设置链接库
target_link_libraries(fruit_server ${catkin_LIBRARIES})
target_link_libraries(fruit_client ${catkin_LIBRARIES})

// 3、添加依赖项
add_dependencies(fruit_server ${PROJECT_NAME}_gencpp)
add_dependencies(fruit_client ${PROJECT_NAME}_gencpp)

• 编译并运行

$ cd ~/catkin_new_ws/
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun my_service fruit_server
$ rosrun my_service fruit_client

结果如下：

4.参数使用

参数的使用与编程方法

参数模型如下：

• 创建功能包

$ cd ~/catkin_new_ws/src
$ catkin_create_pkg my_parameter roscpp rospy std_msgs

• 再创建YAML参数文件，为了方便，可以采用以下操作保存读取参数

// 列出当前所有参数
$ rosparam list
// 显示某个参数值
$ rosparam get param_key
// 设置某个参数值
$ rosparam set param_key param_value
// 保存参数到文件
$ rosparam dump file_name
// 从文件读取参数
$ rosparam load file_name
// 删除参数
$ rosparam delete param_key

保存得到的参数文件 turtlesim.yaml 如下：

rosdistro: 'melodic

  '
roslaunch:
  uris: {host_cloud__33517: 'http://cloud:33517/'}
rosversion: '1.14.12

  '
run_id: 71cb7532-3d0e-11ec-8842-000c293bd7b4
turtlesim: {background_b: 255, background_g: 86, background_r: 69}

我们可以将其中的背景颜色参数修改，读取参数再次打开就可以看到小乌龟背景变了

图1 background_b = 255

图2 background_b = 0

// 该例程设置/读取海龟例程中的参数

#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("/turtlesim/background_r", 255);
	ros::param::set("/turtlesim/background_g", 255);
	ros::param::set("/turtlesim/background_b", 255);

	ROS_INFO("Set Backgroud Color[255, 255, 255]");

    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);

	ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);

	// 调用服务，刷新背景颜色
	ros::service::waitForService("/clear");
	ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
	std_srvs::Empty srv;
	clear_background.call(srv);
	
	sleep(1);

    return 0;
}

• 在添加了源文件后，我们需要修改编译规则，进行编译，

//配置CMakeLists.txt中的编译规则
// 1、设置需要编译的代码和生成的可执行文件
add_executable(parameter_config src/parameter_config.cpp)
// 2、设置链接库
target_link_libraries(parameter_config ${catkin_LIBRARIES})

• 编译并运行发布者

$ cd ~/catkin_new_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun my_parameter parameter_config

运行结果如下：

r,g,b = 69, 86, 255

r,g,b = 255, 255, 255