ROS（2）服务模型

服务模型

服务端

构建服务端和服务/turtle_command，当调用服务时，服务端回调函数反馈数据，并控制速度话题的发布

实现：

• call /turtle_command 调用服务
• 回调数据
• 发布控制速度话题 /cmd_vel，话题类型为geometry_msgs::Twist
• 观察者turtlesim接收/cmd_vel话题
• 执行速度控制

server_command.cpp

#include 
#include 
#include 


bool pubCommand = false;

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::init(argc, argv, "server_command");

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

    // 构建服务
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallBack);

    // 构建发布者
    ros::Publisher turtle_vel_pub = n.advertise("/turtle1/cmd_vel", 10);

    ROS_INFO("Ready to receive turtle command.");

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

    while (ros::ok)
    {
        // 查看一次回调函数队列
        ros::spinOnce();

        // 构建速度信息
        if(pubCommand)
        {
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5f;
            vel_msg.angular.z = 0.2f;

            // 发布速度指令
            turtle_vel_pub.publish(vel_msg);
        }

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

    return 0;
}

调用服务 rosservice call /turtle_command "{}"

server_command.py

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

import rospy
import time
import _thread as thread
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger
from std_srvs.srv._Trigger import TriggerResponse

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

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 server_command_service():
    # 初始化节点
    rospy.init_node("turtle_command_server")
    
    # 构建服务,数据类型为Trigger
    ser = rospy.Service('/turtle_command', Trigger, commandCallBack)
    
    # 等待回调函数
    print("Ready to receive turtle command.")
    
    # 启动多线程
    thread.start_new_thread(command_thread, ())
    
    rospy.spin()


if __name__ == '__main__':
    server_command_service()

客户端

服务端为turtlesim的spawn服务，用于生成小海龟

创建客户端，向服务端发送请求call，请求数据类型为Spawn类型

client_spawn_service.cpp

#include 
#include 


// 实现客户端调用服务 spawn，生成小海龟

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

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

    // 等待服务端服务 spawn
    ros::service::waitForService("/spawn");

    // 构建客户端，连接服务spawn，类型为turtlesim功能包中Spawn类型数据
    ros::ServiceClient client_spawn = n.serviceClient("/spawn");

    // 构建Spawn类型数据并赋值
    turtlesim::Spawn spawn_info;
    // 生成小海龟信息，包括x,y值，以及小海龟名称
    spawn_info.request.x = 3.0f;
    spawn_info.request.y = 4.0f;
    spawn_info.request.name = "turtle2";

    // 打印生成小海龟的信息
    ROS_INFO("Call service to spawn turtle [x:%0.6f, y:%0.6f, name:%s]", spawn_info.request.x, spawn_info.request.y, spawn_info.request.name.c_str());

    // 客户端调用
    client_spawn.call(spawn_info);

    // 服务端回调信息，返回值
    ROS_INFO("Spawn turtle successfully [name:%s]", spawn_info.response.name.c_str());

    return 0;
}

client_spawn_service.py 

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

import rospy
from turtlesim.srv import Spawn


def turtle_spawn():
    # 初始化节点
    rospy.init_node('client_spawn')
    
    # 节点等待服务 /spawn
    rospy.wait_for_service('/spawn')
    
    try:
        # 构建服务
        spawn_turtle = rospy.ServiceProxy('/spawn', Spawn)
        
        # 调用服务，输入请求书u
        response = spawn_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())

自定义服务端和客户端

person_server.cpp

#include 
#include 

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::init(argc, argv, "person_server");

    ros::NodeHandle n;

    ros::ServiceServer person_server = n.advertiseService("/show_person", personCallBack);

    ROS_INFO("Ready to show person information");

    ros::spin();

    return 0;
}

person_client.cpp

#include 
#include 


int main(int argc, char** argv)
{
    ros::init(argc, argv, "person_client");

    ros::NodeHandle n;

    ros::service::waitForService("/show_person");
    ros::ServiceClient person_client = n.serviceClient("/show_person");

    learning_service::Person person_srv;
    person_srv.request.name = "Tom";
    person_srv.request.age = 19;
    person_srv.request.sex = learning_service::Person::Request::male;

    ROS_INFO("Call service to show person [name:%s, age:%d, sex:%d]", person_srv.request.name.c_str(), person_srv.request.age, person_srv.request.sex);

    person_client.call(person_srv);

    ROS_INFO("Show person result %s", person_srv.response.result.c_str());

    return 0;
}

person_server_python.py

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

import rospy
from learning_service.srv._Person 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", anonymous=True)
    
    rospy.Service("/show_person", Person, personCallBack)
    
    print("Ready to show person")
    rospy.spin()


if __name__ == '__main__':
    person_server()

person_client_python.py

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

import rospy
from learning_service.srv._Person import Person, PersonRequest
import sys

def person_client():
    rospy.init_node("person_client", anonymous=True)
    
    rospy.wait_for_service("/show_person")
    
    try:
        person_client = rospy.ServiceProxy("/show_person", Person)
        
        response = person_client("Tom", 19, PersonRequest.male)
        return response.result
        
    except rospy.ServiceException as e:
        rospy.loginfo("current service error %s", e)


if __name__ == '__main__':
    print("Show person result : %s " % person_client())

参数服务调用

构建客户端，调用服务端(turtlesim) /clear服务，数据类型为std_srvs::Empty

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

rosparam list  查看参数列表

rosservice call /clear  访问服务端，调用服务/clear，刷新背景

client_service_clear.cpp

#include 
#include 
#include 


int main(int argc, char** argv)
{
    int red, green, blue;

    ros::init(argc, argv, "client_service_clear");

    ros::NodeHandle n;

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

    ROS_INFO("Get Background 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 Background 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 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::service::waitForService("/clear");
    ros::ServiceClient param_client = n.serviceClient("/clear");
    std_srvs::Empty srv;
    param_client.call(srv);

    sleep(1);

    return 0;
}

client_service_clear_python.py

#!/usr/bin/env python3
# -*- coding:utf-8 -*-
import rospy
import string
from std_srvs.srv import Empty


def client_clear():
    rospy.init_node("client_service_clear_node")
    
    # 获取背景颜色
    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 [255, 255, 255]")
    
    # 获取背景颜色
    red = rospy.get_param("/turtlesim/background_r")
    green = rospy.get_param("/turtlesim/background_g")
    blue = rospy.get_param("/turtlesim/background_b")
    
    rospy.loginfo("Re-Get Background color [%d, %d, %d]", red, green, blue)
    
    rospy.set_param("/turtlesim/background_r", 255)
    rospy.set_param("/turtlesim/background_g", 0)
    rospy.set_param("/turtlesim/background_b", 0)
    
    rospy.wait_for_service("/clear")
    try:
        client = rospy.ServiceProxy("/clear", Empty)
        response = client()
        return response
    except rospy.ServiceException as e:
        print("current client service clear error %s", e)
        
    
if __name__ == '__main__':
    print("client clear result is %s" % client_clear())