ros学习#6 ros2通信（服务通信）+车道线识别实践

上一章节学习了ros2的话题通信，实现了系统状态的监测，这个过程简单来讲就是服务端将系统的状态话题发布出来，然后在订阅端显示，就好比我告诉A一个新闻，然后A把这个新闻整理出来，整个过程是一个单向通信的过程。但是有时候通信不一定是单向的，比如我告诉A一个案件，A在研究完这个案件之后还要把疑点返回给我，那这个过程就是一个双向通信的过程，这时候就要引入服务的概念。

一、服务通信介绍

服务通信的双向通信机制是基于请求和响应的，启动海龟模拟器，通过rqt工具或者下面这行代码查看当前的服务列表：

ros2 service list -t

rqt工具界面如下，我们打开spawn服务，它的作用是产生一个新的海龟：

可以更改其中的一些参数，然后点击右上角的call看看海龟节点有什么响应。

其实服务跟话题类似，也是分为四个要素：服务端，客户端，服务名称，服务接口。只不过服务的消息接口有些特殊，它分为请求和响应两部分。

因为我是车辆工程专业，所以这里我用一个识别车道线的简单项目来学习服务通信。

二、车道线识别服务

1、项目简介

这个项目的目的是识别车道线，并将检测结果返回，并且显示识别后的图片。大体思路是用python写一个服务端获取图片并且识别车道线，然后用C++获取response的结果在客户端显示识别结果界面。

2、自定义服务接口

创建工作空间然后构建，这些都不详细描述了，然后进入src文件夹创建功能包。

首先我们应该创建服务接口，因为ros2已有的服务接口没有能完成这个功能的，所以需要自定义服务接口，首先创建一个接口功能包：

~/lane_detect_ws/src$ ros2 pkg create interfaces --build-type ament_cmake --dependencies rosidl_default_generators sensor_msgs --license Apache-2.0

然后在这个功能包下创建服务接口文件，依次执行下面几个命令：

~/lane_detect_ws/src$ cd interfaces
~/lane_detect_ws/src/interfaces$ mkdir srv
~/lane_detect_ws/src/interfaces$ cd srv
~/lane_detect_ws/src/interfaces/srv$ touch LaneDetector.srv

注意这里的LaneDetector.srv文件名只能以大写字母开头且以大小写字母组成。（但是这个服务接口生成的头文件并不是这样的，这个后面会说）

在工作空间根目录下输入code ./ 命令用vscode打开工作空间，在刚刚创建的 LaneDetector.srv文件中输入下面的代码：

sensor_msgs/Image image         #原始图像
---
bool success                    #检测是否成功
float32 time                    #检测时间
int32[] left_lines              #左车道线点
int32[] right_lines             #右车道线点

“---”上半部分是请求（request）部分，下半部分是响应（response）部分。其实在写这个服务接口的时候我是不知道写什么内容的，我只想着服务端请求应该得获取图片，然后返回响应应该得告诉客户端检测有没有成功，然后花费了多少时间。直到写客户端的时候我才想起来服务端的response得给客户端返回车道线位置我才能根据位置坐标划线，然后才添加了后面两行。

具体来讲写服务接口在我看来的思路就是：

1、首先明确项目的目的，分清服务端和客户端分别要完成什么功能。我这里项目的最终目的是识别车道线并显示在屏幕上，想法是在服务端要进行图片识别，然后客户端要显示识别后的车道线图片。

2、然后思考服务端和客户端完成这些功能都需要什么参数来完成。我这里服务端要想识别图片肯定得先获得图片，而客户端要显示识别后的车道线，那就得获得识别后的车道线位置，那就得定义一个数组存储这些点，我觉得这个过程倒是不急，可以一边写客户端一边想接口里缺了什么，然后再补充就好了。

写完服务接口就需要在cmake文件和package.xml文件中进行注册，两者添加的代码按顺序分别如下：

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/LaneDetector.srv"
  DEPENDENCIES sensor_msgs
)

<member_of_group>rosidl_interface_packages</member_of_group>

具体在我上一篇文章的话题自定义接口里也有介绍，member_of_group就是声明这个功能包是消息接口功能包。

然后就是构建并声明环境变量，运行ros2 interface show interfaces/srv/LaneDetector查看以下接口文件看看有没有定义成功，下面这种就是定义成功了：

3、写服务端识别车道线

有了服务接口就可以开始写服务端和客户端了，首先写一下服务端。我这里服务端的主要作用就是接收图片，然后在服务端识别车道线并将结果返回给客户端，所以服务端的主要工作就是车道线识别，这个用python完成。

首先创建一个python功能包，在工作空间的src目录下执行下面命令，并为它添加rclpy 和我们自己写的interfaces依赖：

~/lane_detect_ws/src$ ros2 pkg create lane_detection --build-type ament_python --dependencies rclpy interfaces --license Apache-2.0

创建完功能包后编写服务端代码，可以先写一个大体框架：

import rclpy                               
from rclpy.node import Node
import cv2
from interfaces.srv import LaneDetector        #导入服务接口
from ament_index_python.packages import get_package_share_directory  #这个函数用于找到功能包的share目录
from cv_bridge import CvBridge     #由于ros和opencv的图片格式不相容，所以需要转换成opencv的格式
import time           #计时函数

class LaneDetectionNode(Node):
    def __init__(self):
        super().__init__('lane_detection_node')     #继承父类函数，并将节点命名为lane_detection_node
        self.bridge = CvBridge()                    #转换成opencv的图片格式
        self.image_path = get_package_share_directory('lane_detection')+'/resource/picture.jpg'   #设置图片的默认路径
        #创建服务，第一个参数是服务接口，第二个参数是服务名称，第三个参数是回调函数
        self.service = self.create_service(LaneDetector, '/lane_detection', self.detect_lane_callback)  

    def detect_lane_callback(self, request, response):
        #to do:检测车道线
        pass
       

def main(args=None):
    rclpy.init(args=args)
    node = LaneDetectionNode()
    rclpy.spin(node)
    rclpy.shutdown()

这个代码里面我还添加了图片的默认路径，这是为了方便写完服务端后进行验证写的是否正确，所以就在resource文件夹里放了一张名为picture.jpg的图片当做客户端上传的图片，还要在setup.py里面添加一下图片路径：

 data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + "/resource", ['resource/picture.jpg']),
    ],

其中'share/' + package_name + "/resource"路径为目标路径，['resource/picture.jpg']为源文件路径。在构建功能包时将在install/lane_detect/目录下创建目标路径，然后把源文件路径中的文件复制到目标路径中。

我这个原始图片长这样，从网上随便找的：

下面就可以开始写车道线检测的代码，我的做法是先另开一个项目，然后用opencv单独写一个车道线检测的项目，然后运行再debug测试，直到能完整无误地实现功能之后再把代码迁移到服务端的回调函数里，只需要稍微改一下就可以，改完之后的服务端代码如下：

车道线检测代码参考OpenCV实战案例——车道线识别-优快云博客

import rclpy                               
from rclpy.node import Node
import cv2
import numpy as np
from interfaces.srv import LaneDetector        #导入服务接口
from ament_index_python.packages import get_package_share_directory  #这个函数用于找到功能包的share目录
from cv_bridge import CvBridge     #由于ros和opencv的图片格式不相容，所以需要转换成opencv的格式
import time           #计时函数

#计算直线斜率函数
def calculate_slope(line):
    x_1, y_1, x_2, y_2 = line[0]
    return (y_2 - y_1) / (x_2 - x_1)

#剔除斜率不一样的线段
def reject_abnormal_lines(lines, threshold):
    slopes = [calculate_slope(line) for line in lines]
    while len(lines) > 0:
        mean = np.mean(slopes)
        diff = [abs(s - mean) for s in slopes]
        idx = np.argmax(diff)
        if diff[idx] > threshold:
            slopes.pop(idx)
            lines.pop(idx)
        else:
            break
    return lines
#最小二乘法拟合直线
def least_squares_fit(lines):
    x_coords = np.ravel([[line[0][0], line[0][2]] for line in lines])  #找到所有横坐标，展平成一维数组，（x1, y1, x2, y2）
    y_coords = np.ravel([[line[0][1], line[0][3]] for line in lines])  #找到所有纵坐标，展平成一维数组
    poly = np.polyfit(x_coords, y_coords, deg=1)  # 进行直线拟合，得到多项式系数
    point_min = (np.min(x_coords), np.polyval(poly, np.min(x_coords)))
    point_max = (np.max(x_coords), np.polyval(poly, np.max(x_coords)))  # 根据多项式系数，计算两个直线上的点
    return np.array([point_min, point_max], dtype=np.int64)

class LaneDetectionNode(Node):
    def __init__(self):
        super().__init__('lane_detector_service')     #继承父类函数，并将节点命名为lane_detector_service
        self.bridge = CvBridge()                    #转换成opencv的图片格式
        self.image_path = get_package_share_directory('lane_detection')+'/resource/picture.jpg'   #设置图片的默认路径
        #创建服务，第一个参数是服务接口，第二个参数是服务名称，第三个参数是回调函数
        self.service = self.create_service(LaneDetector, '/lane_detection', self.detect_lane_callback)  

    def detect_lane_callback(self, request, response):
        #检测车道线
        if request.image.data:       #通过ros2 interface show可以在服务接口定义中能找到data的定义，这里意思是如果检测到图片就直接转换图片格式
            cv_image = self.bridge.imgmsg_to_cv2(request.image)
        else:    #如果没检测到就要在默认路径读取图片                  
            cv_image = cv2.imread(self.image_path)
        start_time = time.time()
        self.get_logger().info('图像加载完成，开始检测')
        #定义一个卷积核
        kernel = np.ones((1,1),np.uint8)
        g_img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        canny = cv2.Canny(g_img, 279, 320)
        #去除噪声
        canny = cv2.dilate(canny, kernel, iterations=4)
        canny = cv2.erode(canny, kernel, iterations=1)
        mask = np.zeros_like(canny)
        mask = cv2.fillPoly(mask, np.array([[[20,1250],[1750,1250],[800,780],[680,780]]]),color=255)
        roi = cv2.bitwise_and(canny,mask)
        #获取所有线段
        lines = cv2.HoughLinesP(roi, 1, np.pi / 180, 15, minLineLength=50, maxLineGap=20) 
        left_lines = [line for line in lines if calculate_slope(line) > 0]
        right_lines = [line for line in lines if calculate_slope(line) < 0]
        reject_abnormal_lines(left_lines, threshold=0.1)
        reject_abnormal_lines(right_lines, threshold=0.1)
        left_lines_ = least_squares_fit(left_lines)
        right_lines_ = least_squares_fit(right_lines)
        if len(left_lines_)>0 and len(right_lines_)>0:
            response.success = True
            end_time = time.time()
            self.get_logger().info(f'检测成功，耗时{end_time - start_time}秒')
            
        else:
            response.success = False
            self.get_logger().info('检测失败')

        #给response赋值
        response.time = end_time - start_time
        for l_line in left_lines_:
            for l_point in l_line:
                response.left_lines.append(l_point)
        for r_line in right_lines_:
            for r_point in r_line:
                response.right_lines.append(r_point)
        return response
    

def main(args=None):
    rclpy.init(args=args)
    node = LaneDetectionNode()
    rclpy.spin(node)
    rclpy.shutdown()

最后给response赋值的时候套了两个for循环写的有点过于臃肿了，因为这个left_lines_计算出来是一个包含两个点的二维数组，所以我得展开成一维数组然后赋值给response，其实也可以在消息接口定义左右各两个点的数组，这样就只需要一个for循环就够了。

写完之后构建并声明环境变量，然后运行这个节点，再打开一个新的终端，call一下这个服务，call服务的代码如下：

~/lane_detect_ws$ ros2 service call /lane_detection interfaces/srv/LaneDetector

不出意外的话就会出现下面的结果：

出现问题

这个过程我出了点意外，报错了：

看报错是说cvtcolor这个命令没检测到图片，我首先去install文件夹里找了一下，发现在对应文件夹果然没发现resource文件夹，反而多了个长得很奇怪的文件夹：

最下面这个lane_detectionresource文件夹是什么鬼……这种情况就是setup.py文件写文件路径写错了，打开一看果然：

最后一行应该是/resource，结果我写成了 resource/，导致出现了那个很奇怪的文件夹……修改之后就正常了。

4、写客户端显示识别结果

然后下面开始写客户端代码，客户端的任务就是显示识别的图片标出车道线的位置。这里用C++编写也是为了熟悉不同语言的编写节点方式。首先创建功能包，添加上rclcpp和interfaces依赖：

~/lane_detect_ws/src$ ros2 pkg create lane_detect_client --build-type ament_cmake --dependencies interfaces rclcpp --license Apache-2.0

然后在功能包的src目录下创建 lanedetection_client.cpp文件，写入代码：

#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/lane_detector.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include "opencv4/opencv2/opencv.hpp"
#include <ament_index_cpp/get_package_share_directory.hpp>

using LaneDetector = interfaces::srv::LaneDetector;

class LaneDetectorClient : public rclcpp::Node
{
public:
  LaneDetectorClient() : Node("lanedetection_client")
  {
    client_ = this->create_client<LaneDetector>("/lane_detection");
    auto package_dir = ament_index_cpp::get_package_share_directory("lane_detection");
    image_path_ = package_dir + "/resource/picture.jpg";
    image_ = cv::imread(image_path_, cv::IMREAD_COLOR);
  }

  void send_request()
  {
    while (!client_->wait_for_service(std::chrono::seconds(1)))
    {
      RCLCPP_INFO(this->get_logger(), "等待服务上线...");
    }
    auto request = std::make_shared<LaneDetector::Request>();


    sensor_msgs::msg::Image image_msg;
    cv_bridge::CvImage cv_img(std_msgs::msg::Header(), "bgr8", image_);
    cv_img.toImageMsg(image_msg);
    request->image = image_msg;


    auto future_result = client_->async_send_request(request);
    rclcpp::spin_until_future_complete(shared_from_this(), future_result);
    if (rclcpp::spin_until_future_complete(shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS)
    {
      auto response = future_result.get();
      RCLCPP_INFO(this->get_logger(), "接收到响应：检测到车道线,耗时%f", response->time);
      show_lane_location(response);
    }
  }

  void show_lane_location(const LaneDetector::Response::SharedPtr response)
  {
    cv::line(image_, cv::Point(response->left_lines[0], response->left_lines[1]), cv::Point(response->left_lines[2], response->left_lines[3]), cv::Scalar(0, 0, 255), 5);
    cv::line(image_, cv::Point(response->right_lines[0], response->right_lines[1]), cv::Point(response->right_lines[2], response->right_lines[3]), cv::Scalar(0, 0, 255), 5);
    cv::namedWindow("result", cv::WINDOW_NORMAL);
    cv::resizeWindow("result", 800, 600);
    cv::imshow("result", image_);
    cv::waitKey(0);
    cv::destroyAllWindows();
  }

private:
  rclcpp::Client<LaneDetector>::SharedPtr client_;
  std::string image_path_;
  cv::Mat image_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LaneDetectorClient>();
  node->send_request();
  rclcpp::shutdown();
  return 0;
}

cmake文件配置要添加依赖和加入可执行文件，添加以下代码：

# find dependencies

find_package(ament_cmake REQUIRED)
find_package(interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(OpenCV 4 REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(ament_index_cpp REQUIRED)

add_executable(lanedetection_client 
src/lanedetection_client.cpp
)

ament_target_dependencies(lanedetection_client
  rclcpp
  interfaces
  OpenCV
  cv_bridge
  ament_index_cpp
)

install(TARGETS
  lanedetection_client
  DESTINATION lib/${PROJECT_NAME}
)

然后配置package.xml文件：

  <depend>rclcpp</depend>
  <depend>interfaces</depend>
  <depend>opencv4</depend>
  <depend>ament_index_cpp</depend>

出现问题

这个代码我前前后后改过很多次：

1、头文件导入错误

首先是报错LaneDetector.hpp文件，这是因为我在include这个消息接口的时候，我当时输入的是：

#include "interfaces/srv/LaneDetector.hpp"

当时我还很奇怪怎么就找不到，他明明就在这里啊，然后我就去install文件夹里面找了一下：

原来它的头文件名字叫lane_detector.hpp！！！以后出现找不到什么包或者节点的时候可以去install文件夹里面看一眼，因为有可能是头文件写错了，也有可能没生成可执行文件，这个文件夹可以粗略判断一下哪里出了问题。

2、opencv的调用问题

因为我一直都是用python写opencv程序，第一次用C++来写opencv，我在构建功能包时报错说找不到opencv的依赖，我改了半天原来是在cmake文件里面OpenCV必须得注意大小写，不然报错： ament_target_dependencies() the passed package name 'opencv4' was not found，然而package.xml文件里就不用。而且cmake文件里面ament_target_dependencies参数里面不能加依赖的版本号，但是find_package里面就可以加。

3、cv_bridge版本问题

报错/usr/bin/ld: warning: libopencv_imgcodecs.so.4.5d, needed by /opt/ros/humble/lib/libcv_bridge.so, may conflict with libopencv_imgcodecs.so.3.4。意思是cv_bridge需要opencv4.x版本，但是依赖添加的是3.x版本的，所以我在cmake文件的find_package里面指定了opencv4版本，不指定的话他就会默认使用opencv3.x，另外在package.xml文件里也要同步配置opencv4 。

4、cv_bridge不会使用

因为没在C++里面用过cv_bridge，所以有些函数不知道怎么用，就不停报错。这里可以选中cv_bridge然后转到定义里面看看库里面的函数有没有能用的

这里我把OpenCV图片格式转换为ROS图片格式的方法是：

sensor_msgs::msg::Image image_msg;
cv_bridge::CvImage cv_img(std_msgs::msg::Header(), "bgr8", image_);
cv_img.toImageMsg(image_msg);
request->image = image_msg;

改了很多次终于能运行不报错了…………

然后就是在一个终端ros2 run运行服务端节点，然后在另一个终端运行客户端节点，运行结果如下：