ROS入门-12.服务端Server的编程实现

实现服务通讯的服务端Server的编程实现
主要给海龟发送速度指令，通过topic发，Service实现；Client发布request来控制Server是不是给海龟发指令让其动起来，Client相当于让海龟运动及停止的开关，发布request，海龟就运动，再发布一个request，海龟停止；Server端来接收开关的指令并且完成topic海龟运动指令的发送；这里用到的Service名称（自定义的一个Service名字）为turtle_command ，关于Service的数据类型的定义，用到的是ros中针对服务的标准的一个定义，叫做Trigger，相当于是一个触发，触发这样一个信号，是运动还是停止的触发信号，我们通过Server一端收到触发信号之后来控制海龟是不是要去发送Twist这样一个速度指令，同时要去反馈一个response，告诉Client一端其请求是否已经执行成功，这时就可以通过Client一端知道海龟的运动状态，以上是该讲要去实现的服务模型
这里Server的实现里面既会包含Service的Server一端的实现，同时也会包含一个发布者publisher的实现，有两部分内容

第一步，创建服务器代码（C++例）

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

将课件代码/learning_service/src里的turtle_command_server.cpp代码文件放置于catkin_ws/src/learning_service/src里

turtle_command_server.cpp代码文件

/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 * Trigger是触发的一个信号
 */
 
#include <ros/ros.h>  //ROS头文件
#include <geometry_msgs/Twist.h>  //给海龟去发布速度指令Twist Topic这样一个头文件
#include <std_srvs/Trigger.h>  //用到的服务数据类型头文件

ros::Publisher turtle_vel_pub;   //创建一个全局的publisher头文件
bool pubCommand = false;   //定义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");

	// 设置反馈数据
	//同时要给Client这一端反馈一个应答数据，告诉数据执行成功还是失败
	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，后面是初始化的内容，如Service名字叫turtle_command（自己定义的），同时Service里的server这一端的处理跟我们在topic话题通讯当中的subscriber订阅者的处理有点像，并不知道数据什么时候进来，一样需要一个回调函数的处理来处理request的请求，commandCallback即为回调函数。一旦Server端收到request进来后，立刻跳到回调函数里，在回调函数里做针对server的处理，比如要让海龟动起来，就需要在commandCallback 里让海龟动起来
    //创建一个Service的server
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	//创建一个topic的publisher,主要是给海龟发布速度指令让海龟能动起来
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 循环等待回调函数
    //发布日志信息
    ROS_INFO("Ready to receive turtle command.");

	// 设置循环的频率，一秒钟10HZ
	ros::Rate loop_rate(10);

	while(ros::ok())
	{
		// 查看一次回调函数队列，是否有消息进入
    	ros::spinOnce();
		
		// 如果标志为true，则发布速度指令，则对应的海龟仿真器里的海龟就会动起来；如果标志为false，就不会有任何的数据发出，海龟就不会发生任何运动；标志位是在commandCallback里做处理的，所以注意，如果Client这一端有请求request发出之后，Server端收到请求就会立刻跳到回调函数里来
		
		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;
}


//回顾整个流程，初始化，创建句柄，创建一个server,之后等待server turtle_command的调用，一旦有request进来，立刻跳到commandCallback里做处理；一进来现对标志位取反，然后标志位在while循环中做判断，如果标志位是true，就会让海龟动，如果标志位是false，海龟就会停止运动       关键要去注意的一点是server的创建和server里的commandCallback里的机制跟topic里的subscriber订阅者有点类似；commandCallback里由两部分内容，一个request一个response专门针对请求内容和应答内容，只不过这里的Trigger，他的request是空的内容

小插入：通过rossrv show std_srvs/Trigger命令行看一下Trigger的定义
可见该service定义较简单，因为涉及到request和response两个部分数据，因此通过—做一个区隔，—之上是request内容，—之下是response内容
所以，这里可见发送的request内容是空的，这里的response就是success，message用于告诉Client一端发生了什么

第二步，配置服务器代码编译规则

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

将该两行代码粘贴至CMakeLists.txt文件，Install前，上一次所添加编译规则下
第一句，把turtle_command_server.cpp的代码编译成turtle_command_server这样一个可执行文件
第二句，添加链接库

第三步，编译并运行服务器

cd ~/catkin_ws //回到工作空间根目录下
catkin_make //用该指令去做编译
source devel/setup.bash //设置环境变量，可通过一些方法减去该步骤，跳过
roscore
rosrun turtlesim turtlesim_node //运行仿真器
rosrun learning_service turtle_command_server //运行刚才创建的server端

rosservice call /turtle_command “{}” //然后通过终端发送Client这样一个请求，请求server来让它让海龟动起来

turtle_command刚刚在程序里已经写好的service的服务的名字；看service的数据内容是怎样的，rosservice call /turtle_command后双击tab键补全“{}”，刚刚说过 request请求是空的，大括号里的内容就是request的内容是空的
回车后海龟动起来，同时收到server反馈回来的应答数据


Client相当于海龟的开关，发送一次运动，再次发送，停止，反复如此

rosrun learning_service turtle_command_server处显示收到的指令，yes是海龟动，no是海龟停止

接下来是python实现内容，代码流程一样
创建服务器代码

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

将课件代码/learning_service/scripts里的turtle_command_server.py代码文件复制放置于catkin_ws/src/learning_service/scripts里

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

#定义两个全局变量，一是标志位，一个是Publisher用来发布运动指令的
pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)


#在刚刚的C++程序里，调用ros::spinOnce();查询一次队列，看队列里有没有数据，有的话进入回调函数，没有则跳出来继续执行while循环里的程序。但是ros::spinOnce()在ros的python的接口里面是没有实现的，所以在python里面需要通过一个多线程的机制来去实现类似于ros::spinOnce()+if判断标志位这样一个功能    所以多了这样一个程序，他是一个线程，主要来实现判断if pubCommand 是true或者false,true则发送海龟运动指令，false则跳过
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)
	#在commandCallback里会对全局标志位做取反，接下来显示request的数据，同时反馈应答数据

	# 显示请求数据
	rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)

	# 反馈数据  success即是1；另外一个是message
	return TriggerResponse(1, "Change turtle command state!")

def turtle_command_server():
	# ROS节点初始化
    rospy.init_node('turtle_command_server')

	# 创建一个名为/turtle_command的server，注册回调函数commandCallback
	#通过调用rospy中的Service来创建服务的名字turtle_command，服务的数据类型Trigger，以及一旦有服务进来的commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)

	# 循环等待回调函数
    print "Ready to receive turtle command."
 
    #创建一个新的线程来启动def command_thread():	部分程序   同时启动spin，一个死循环，不能判断上面这些内容，所以在其之前单独起了一个线程去判断；在spin中会不断循环来看是否有request进来，一旦有则进入commandCallback
    thread.start_new_thread(command_thread, ())
    rospy.spin()

if __name__ == "__main__":
    turtle_command_server()
    #先进入该函数