ROS 应用开发入门 服务端Server的编程

ROS是机器人操作系统的简称，本文介绍ROS应用开发，服务端Server的编程。程序的功能是通过ROS的服务调用功能调用本服务。本服务被调用后发布命令让小乌龟转圆圈，以后再调用就是乒乓功能，停止或开始乌龟转圆圈。先是c++代码，然后是python 代码，你也可选择只看一种你熟悉的。

功能包建立

本文是 ROS 应用开发入门 客户端Client的编程 的继续，如果你在那文建立了功能包，这里就不用了，直接跳过本节。

在 ROS 开发应用准备：创建工作空间 一文中建立了ROS工作空间，现在就建立一个功能包：

cd ~/catkin_ws/src

catkin_create_pkg  learning_service std_msgs roscpp rospy geometry_msgs turtlesim

第1行是回到工作空间的src 目录，功能包建立必须在这个目录下运行。

第2行是建立功能包的命令，第一个参数是功能包的名字，这里是learning_service ， 接下来是功能包的依赖库，这里是std_msgs roscpp rospy geometry_msgs turtlesim 共5个依赖库。

上面命令执行后，可以在src 目录下看到有一个目录，名字是learning_service，其下有下面内容：

CMakeLists.txt  include  package.xml  scripts  src

其中scripts 是后来手工建立的，用于存放 python代码。

c++ 源代码

在src 目录下，也是~/catkin_ws/src/learning_service/src 目录， 建立一个文件turtle_command_server.cpp，

cd ~//catkin_ws/src/learning_service/src

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

这个代码有清晰的注释，这是一个实现服务端的基本流程，这里不多重复了。

配置cmake文件

在 ~/catkin_ws/src/learning_service/ 目录下，有个CMakeLists.txt 文件，我们需要修改这个文件

cd ~/catkin_ws/src/learning_service

nano CMakeLists.txt 

在这个文件中添加下面2行，

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

添加的位置是Build的最后部分，参考下面位置，就是 ## install ## 前面：

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

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

#############
## Install ##
#############

 有turtle_spawn 的那2行是Client 一文的内容。添加后保存，退出

这样编译配置就完成了。

编译和运行测试

编译必须回到 ~/catkin_ws 目录下

cd ~/catkin_ws

catkin_make

编译后应该source 一次：

source devel/setup.bash

如果编译有错，就要排除错误，然后就运行测试。

打开一个终端，启动ros，执行

roscore

再打开一个终端，运行小乌龟

rosrun turtlesim turtlesim_node

上面是启动测试环境，如果有问题，可以 参看： ROS 下的仿真小乌龟，只是不启动键盘控制。

最后在我们的终端执行：

rosrun learning_service turtle_spawn_command_server

执行上面命令后，我们需要用ROS的服务调用功能来调用这个服务。

再打开一个终端，在其中输入下面命令：

rosservice call /turtle_command

这个调用没有参数，执行后，可以看到小乌龟在转圆圈了。

再次执行上面命令时，乌龟停下了，再执行，乌龟又转圆圈了，就是一个乒乓控制。

c++的开发就到此完成了。

python 代码 

为了不和c++混在一起，在~/catkin_ws/src/learning_service/目录下新建一个scripts目录，然后到这个目录下，新建一个turtle_command_server.py 文件

cd ~/catkin_ws/src/learning_service/scripts

nano turtle_command_server.py

文件内容是：

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

代码里有清晰说明，这里不重复。

python 运行测试

python 可以直接执行，不需要编译，

更改turtle_command_server.py 有执行属性 

chmod +x *.py

这个可以用 ls -l 查核：

同时应该source 一次： 

source ~/catkin_ws/devel/setup.bash

首先需要启动测试环境，一个终端启动roscore， 再一个终端启动小乌龟rosrun turtlesim turtlesim_node，如果有问题，可以 参看： ROS 下的仿真小乌龟，只是不启动键盘控制。 

然后执行如下命令：

rosrun learning_service turtle_command_server.py

执行上面命令后，我们需要用ROS的服务调用功能来调用这个服务。

再打开一个终端，在其中输入下面命令：

rosservice call /turtle_command

这个调用没有参数，执行后，可以看到小乌龟在转圆圈了。

再次执行上面命令时，乌龟停下了，再执行，乌龟又转圆圈了，就是一个乒乓控制。

源代码也可以在 https://github.com/huchunxu/ros_21_tutorials 下载

介绍到此。