跳转至

自定义srv

1. 自定义srv消息

1.1 创建package(创建过可省略)

ros2 pkg create --build-type ament_cmake msg_srv

可以修改package.xml文件如下信息

<description>C++ client server tutorial</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

1.2 在工程下创建srv文件夹,创建AddThreeInts.srv并填写如下内容

int64 a
int64 b
int64 c
---
int64 sum

1.3 配置cmake

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  ...
  "srv/AddThreeInts.srv"
)

1.4 配置package.xml文件

<build_depend>rosidl_default_generators</build_depend>

<exec_depend>rosidl_default_runtime</exec_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

1.5 编译工程

colcon build --packages-select msg_srv

1.6 显示消息详情

ros2 interface show msg_srv/srv/AddThreeInts

2. 使用自定义的srv消息

2.1 c++使用

client和server代码实现

client

#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "msg_srv/srv/add_three_ints.hpp"
using namespace std;


int main(int argc,char*argv[]){
    rclcpp::init(argc, argv);
    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("client_node");
    rclcpp::Client<msg_srv::srv::AddThreeInts>::SharedPtr client =
            node->create_client<msg_srv::srv::AddThreeInts>("add_three_ints");

    //request data
    auto req =  std::make_shared<msg_srv::srv::AddThreeInts::Request>();
    req->a = 10;
    req->b = 20;
    req->c = 30;
    //wait service come to use
    while (!client->wait_for_service(1s)) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
            return 0;
        }
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
    }
    //send request
    auto result = client->async_send_request(req);

    if(rclcpp::spin_until_future_complete(node,result)==rclcpp::FutureReturnCode::SUCCESS){
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
    } else{
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
    }

    rclcpp::shutdown();
    return 0;
}

server

#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "msg_srv/srv/add_three_ints.hpp"
using namespace std;

void add(msg_srv::srv::AddThreeInts::Request::SharedPtr req,msg_srv::srv::AddThreeInts::Response::SharedPtr res){
    res->sum = req->a + req->b+req->c;
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",req->a, req->b);
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)res->sum);
}

int main(int argc,char*argv[]){
    rclcpp::init(argc, argv);
    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("server_node");
    //create server
    rclcpp::Service<msg_srv::srv::AddThreeInts>::SharedPtr service =
            node->create_service<msg_srv::srv::AddThreeInts>("add_three_ints", add);


    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

配置cmake

find_package(msg_srv REQUIRED)

...
add_executable(client_srv src/client_srv.cpp)
add_executable(server_srv src/server_srv.cpp)

...
ament_target_dependencies(client_srv
        rclcpp msg_srv)
ament_target_dependencies(server_srv
        rclcpp msg_srv)

install(TARGETS
        ...
        client_srv
        server_srv
        DESTINATION lib/${PROJECT_NAME})

package.xml配置

<depend>msg_srv</depend>

编译工程

colcon build

运行节点

ros2 run cpp_srvcli client_srv
ros2 run cpp_srvcli server_srv

2.2 python使用

client和server代码实现

client

import rclpy
from rclpy.node import Node
from msg_srv.srv import AddThreeInts

class MyNode(Node):
    def __init__(self):
        super(MyNode, self).__init__('client_py')
        self.cli = self.create_client(AddThreeInts,'py_service')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available,waiting again...')
        self.req = AddThreeInts.Request()

    def send_request(self,a,b,c):
        self.req.a = int(a)
        self.req.b = int(b)
        self.req.c = int(c)
        self.future = self.cli.call_async(self.req)

def main():
    rclpy.init()
    node = MyNode()

    # send request
    node.send_request(10,20,30)
    while rclpy.ok():
        rclpy.spin_once(node)
        if node.future.done():
            try:
                response = node.future.result()
            except Exception as e:
                node.get_logger().info(f'servie error:{e}')
            else:
                node.get_logger().info(f'result:{response.sum}')
                break

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

server

from msg_srv.srv import AddThreeInts
import rclpy
from rclpy.node import Node


class MyServer(Node):
    def __init__(self):
        super(MyServer, self).__init__('py_server')
        self.service = self.create_service(AddThreeInts, 'py_service', self.call_back)

    def call_back(self, req, res):
        res.sum = req.a + req.b + req.c
        return res


def main():
    rclpy.init()
    node = MyServer()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

package.xml配置

<depend>msg_srv</depend>

setup.py配置

entry_points={
        'console_scripts': [
            ...
            'client_srv = py_srvcli.client_srv:main',
            'server_srv = py_srvcli.server_srv:main',
        ],
    },

编译工程

colcon build

运行节点

ros2 run py_srvcli client_srv 
ros2 run py_srvcli server_srv