新增红外小程序;重写颜色跟踪代码

src/infrared_maze/CMakeLists.txt

@@ -138,6 +138,7 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_node src/infrared_maze.cpp)
+add_executable(infrared_demo_node src/infrared_demo.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -148,11 +149,15 @@ add_executable(${PROJECT_NAME}_node src/infrared_maze.cpp)
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(infrared_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_node
    ${catkin_LIBRARIES}
  )
+target_link_libraries(infrared_demo_node
+   ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##

src/infrared_maze/cfg/param.yaml

@@ -14,6 +14,7 @@
 #   odom_frame:odom的tf名称.
 #   base_frame:底盘的tf名称.
 #   forward_dist:每次前进时移动的距离.
+#   center_dist:红外测距时悬停的距离.
 # Author: corvin,adam
 # History:
 #   20200404:init this file.
@@ -42,3 +43,5 @@ init_tolerance_dist: 0.01
 init_linear_x: 0.17
 init_linear_y: 0.05
 init_angular_speed: 0.2
+
+center_dist: 0.15

src/infrared_maze/include/infrared_demo.h

@@ -0,0 +1,30 @@
+/***************************************************************
+ Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ Description:红外小demo头文件
+ Author: adam_zhuo
+ History:
+    20210429:初始化文件.-adam_zhuo
+ **************************************************************/
+#ifndef _INFRARED_DEMO_H
+#define _INFRARED_DEMO_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include "ros_arduino_msgs/InfraredSensors.h"
+
+class Infrared_Demo
+{
+private:
+    ros::NodeHandle nh;
+    ros::Subscriber _infrared_sub;
+    ros::Publisher _cmd_vel_pub;
+    geometry_msgs::Twist _twist;
+    float center_dist;
+public:
+    Infrared_Demo();
+    ~Infrared_Demo();
+    void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr &data);
+};
+
+
+#endif

src/infrared_maze/launch/infrared_demo.launch

@@ -0,0 +1,16 @@
+<!--
+  Copyright: 2016-2021 www.corvin.cn
+  Author: adam_zhuo
+  Description:
+    infrared_dist_demo
+  History:
+    20210429: intial this file.
+-->
+<launch>
+  <arg name="cfg_file" default="$(find infrared_maze)/cfg/param.yaml" />
+
+  <!-- startup infrared_demo_node -->
+  <node pkg="infrared_maze" type="infrared_demo_node" name="infrared_demo_node" output="screen">
+    <rosparam file="$(arg cfg_file)" command="load" />
+  </node>
+</launch>

src/infrared_maze/src/infrared_demo.cpp

@@ -0,0 +1,54 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-29 11:24:02
+ * @Description: 实现Infrafed_Demo类，main函数入口
+ * @History: 20210429:初始化-adam_zhuo
+ */
+#include <infrared_demo.h>
+
+Infrared_Demo::Infrared_Demo(){
+
+    //从参数服务器中取“center_dist”数据
+    ros::param::param<float>("~center_dist", center_dist, 0.2);
+
+    //订阅红外数据
+    _infrared_sub = nh.subscribe<ros_arduino_msgs::InfraredSensors>("/mobilebase_arduino/sensor/GP2Y0A41", 1, &Infrared_Demo::infrared_callback, this);
+    //注册小车速度发布者
+    _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+}
+
+Infrared_Demo::~Infrared_Demo(){
+}
+
+/**
+ * @description: 订阅红外数据回调函数
+ */
+void Infrared_Demo::infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr &data){
+
+    //获取前红外信息
+    float front_dist = data->front_dist;
+
+    //根据前红外反馈的距离来控制小车运动，离小车较远时小车前进，离小车较近时小车后退
+    if(front_dist > 0.28){
+        _twist.linear.x = 0;
+        _cmd_vel_pub.publish(_twist);
+    }else{
+        float err_dist = front_dist - center_dist;
+        _twist.linear.x = 0.74 * err_dist * 2;
+        if(abs(_twist.linear.x) < 0.015){
+            _twist.linear.x = 0;
+        }
+        _cmd_vel_pub.publish(_twist);
+    }
+}
+
+int main(int argc, char **argv){
+
+    //初始化节点
+    ros::init(argc, argv, "infrared_demo_node");
+    //根据Infrafed_Demo声明对象，调用构造函数
+    Infrared_Demo infrared_demo;
+    //使ROS中回调函数生效
+    ros::spin();
+}

src/rasp_camera/CMakeLists.txt

@@ -7,7 +7,11 @@ project(rasp_camera)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED
+  roscpp
+  sensor_msgs
+  cv_bridge
+  OpenCV REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -112,8 +116,8 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
-# ${catkin_INCLUDE_DIRS}
+  include
+  ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
@@ -129,7 +133,10 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/rasp_camera_node.cpp)
+add_executable(${PROJECT_NAME}_color_detection_node 
+                src/color_detection_node.cpp
+                src/color_detection.cpp
+                src/kalman_filter.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -142,9 +149,10 @@ include_directories(
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+target_link_libraries(${PROJECT_NAME}_color_detection_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+)
 
 #############
 ## Install ##

src/rasp_camera/include/color_detection.h

@@ -0,0 +1,41 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-27 14:24:49
+ * @Description: 颜色检测头文件
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#ifndef _COLOR_DETECTION_H
+#define _COLOR_DETECTION_H
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <geometry_msgs/Twist.h>
+
+#include "kalman_filter.h"
+
+class Color_Detection
+{
+    private:
+        ros::Subscriber _img_sub;
+        ros::Publisher _cmd_vel_pub;
+        ros::NodeHandle nh;
+        geometry_msgs::Twist _twist;
+        float x;
+        float area;
+        float x_center = 160;
+        float area_center = 3000;
+        double ticks = 0;
+        bool found = false;
+        int not_found_count = 0;
+        Kalman_Filter img_kalman_filter;
+    public:
+        Color_Detection();
+        ~Color_Detection();
+        void img_callback(const sensor_msgs::CompressedImage::ConstPtr & data);
+        void cmd_vel_pub(float x, float area);
+};
+
+#endif

src/rasp_camera/include/kalman_filter.h

@@ -0,0 +1,46 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-27 15:42:41
+ * @Description: 利用卡尔曼滤波进行指定颜色小球跟踪头文件
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#ifndef _KALMAN_FILTER_H
+#define _KALMAN_FILTER_H
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/video/video.hpp>
+
+#include <iostream>
+#include <vector>
+
+using namespace std;
+
+// >>>>> 被跟踪颜色阈值
+#define MIN_H_BLUE 200
+#define MAX_H_BLUE 300
+// <<<<< 被跟踪颜色阈值
+
+class Kalman_Filter
+{
+private:
+    cv::Mat frame;
+    int stateSize = 6;
+    int measSize = 4;
+    int contrSize = 0;
+    unsigned int type = CV_32F;
+    cv::KalmanFilter kf;
+    cv::Mat state; 
+    cv::Mat meas;
+public:
+    Kalman_Filter();
+    ~Kalman_Filter();
+    void found_draw(double dT, cv::Mat &img, float &x, float &area);
+    void kalman_update(bool &found, vector<cv::Rect> balls_box);
+};
+
+
+
+#endif

src/rasp_camera/launch/color_detection.launch

@@ -0,0 +1,13 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-30 11:16:35
+ * @Description: 启动颜色检测节点
+ * @History: 20210430:初始化文件-adam_zhuo
+-->
+<launch>
+  <!-- 启动color_detection_node -->
+  <node pkg="rasp_camera" type="rasp_camera_color_detection_node" name="color_detection_node" output="screen">
+
+  </node>
+</launch>

src/rasp_camera/package.xml

@@ -49,7 +49,15 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/rasp_camera/scripts/color_recognition.py

@@ -1,80 +0,0 @@
-#!/usr/bin/env python
-# _*_ coding:utf-8 _*_
-import rospy, cv2, cv_bridge, math, datetime
-import numpy as np
-from sensor_msgs.msg import Image
-from geometry_msgs.msg import Twist
-from std_msgs.msg import Float64
-
-ball_color = 'red'
-
-color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
-              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
-              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
-              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
-              'yellow': {'Lower': np.array([21, 39, 64]), 'Upper': np.array([34, 255, 255])},
-              }
-class Color_Recognition:
-
-    def __init__(self):
-        self.bridge = cv_bridge.CvBridge()
-        # cv2.namedWindow('window', 1)
-
-        self.image_sub = rospy.Subscriber('/raspicam_node/image', Image, self.image_callback)
-        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-
-
-        self.twist = Twist()
-        self.err = 0
-        self.cnt = 0
-
-        # self.lift_height_pub.publish(20)
-
-
-    def image_callback(self, msg):
-        # To get the image from the camera and convert it to binary image by using opencv
-        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
-
-        cv2.imshow('window_img',img)
-        cv2.waitKey(3)
-        
-        gs_frame = cv2.GaussianBlur(img, (5, 5), 0)
-        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像
-        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # 腐蚀 粗的变细
-        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
-        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
-
-        if len(cnts) == 0:
-            return
-        c = max(cnts, key=cv2.contourArea)
-        print("area:",cv2.contourArea(c))
-        rect = cv2.minAreaRect(c)
-        box = cv2.boxPoints(rect)
-        cv2.drawContours(img, [np.int0(box)], -1, (0, 255, 255), 2)
-
-        cv2.imshow('window_contours', img)
-        cv2.waitKey(3)
-        x_center = 160
-        current_center = 0.5*(box[2,0]-box[0,0])+(box[0,0])
-        err_center = current_center - x_center
-
-        area = 3000
-        current_area = cv2.contourArea(c)
-        err_area = current_area - area
-        
-        self.twist.linear.x = 0
-        self.twist.angular.z = 0
-
-        if abs(err_area)>500:
-            self.twist.linear.x = 0.0074 * -err_area * 0.006
-            print 'linear.x', self.twist.linear.x
-        if abs(err_center)>5:
-            self.twist.angular.z = 0.74 * -err_center * 0.006
-            print 'angular.z', self.twist.angular.z
-        self.cmd_vel_pub.publish(self.twist)
-
-
-if __name__ == '__main__':
-    rospy.init_node('color_recognition')
-    color_recognition = Color_Recognition()
-    rospy.spin()

src/rasp_camera/scripts/color_recognition2.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python
-import rospy, cv2, cv_bridge, math, datetime
-import numpy as np
-from sensor_msgs.msg import CompressedImage
-from geometry_msgs.msg import Twist
-from std_msgs.msg import Float64
-
-# color_range = {
-# "red": [(0, 164, 66), (255, 255, 255)], 
-# "green": [(19, 0, 0), (255, 108, 255)], 
-# "blue": [(17, 0, 0), (250, 255, 110)], 
-# "black": [(0, 0, 0), (56, 255, 255)], 
-# "white": [(193, 0, 0), (255, 250, 255)], 
-#               }
-
-color_range = {
-"red": [(0, 43, 46), (10, 255, 255)], 
-"green": [(35, 43, 46), (77, 255, 255)], 
-"blue": [(100, 43, 46), (124, 255, 255)], 
-"black": [(0, 0, 0), (56, 255, 255)], 
-"white": [(193, 0, 0), (255, 250, 255)], 
-              }
-
-range_rgb = {"red": (0, 0, 255),
-              "blue": (255, 0,0),
-              "green": (0, 255, 0),
-              "black": (0, 0, 0),
-              }
-kernel = np.ones((5, 5), np.uint8)
-target_color = "green"
-class Color_Recognition:
-    
-    def __init__(self):
-        self.bridge = cv_bridge.CvBridge()
-        self.image_sub = rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage, self.image_callback)
-        self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
-        self.twist = Twist()
-
-    def get_area_max_contour(self,contours) :
-        contour_area_temp = 0
-        contour_area_max = 0
-        area_max_contour = None
-
-        for c in contours : 
-            contour_area_temp = math.fabs(cv2.contourArea(c)) 
-            if contour_area_temp > contour_area_max :
-                contour_area_max = contour_area_temp
-                if contour_area_temp > 64:  
-                    area_max_contour = c
-
-        return area_max_contour, contour_area_max
-
-    def image_callback(self, msg):
-        image = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="bgr8")
-
-        # img_center_x = image.shape[:2][1]/2
-        # img_center_y = image.shape[:2][0]/2
-
-        # img_gaussian_blur = cv2.GaussianBlur(image, (5, 5), 0)
-        # img_hsv = cv2.cvtColor(img_gaussian_blur, cv2.COLOR_BGR2HSV)
-        # img_erode = cv2.erode(img_hsv, None, iterations=2)
-        # img_mask = cv2.inRange(img_erode, color_range[target_color][0], color_range[target_color][1])
-        # contours = cv2.findContours(img_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
-        # area_max_contour = self.get_area_max_contour(contours)[0]
-        # if area_max_contour is not None:    
-        #     rect = cv2.minAreaRect(area_max_contour)
-        #     box = cv2.boxPoints(rect)
-        #     cv2.drawContours(image, [np.int0(box)], -1, (0, 255, 255), 2)
-        img_gaussian_blur = cv2.GaussianBlur(image, (5, 5), 0)
-        img_hsv = cv2.cvtColor(img_gaussian_blur, cv2.COLOR_BGR2HSV)
-        img_erode = cv2.erode(img_hsv, None, iterations=2)
-        img_mask = cv2.inRange(img_erode, color_range[target_color][0], color_range[target_color][1])
-        img_opening = cv2.morphologyEx(img_mask, cv2.MORPH_OPEN, kernel)
-        img_edges = cv2.Canny(img_opening, 50, 100)
-        circles = cv2.HoughCircles(img_edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=10,
-            minRadius=10, maxRadius=500)
-        # if circles is not None:
-
-        cv2.imshow("circle", img_edges)
-        cv2.waitKey(3)
-
-if __name__ == "__main__":
-    rospy.init_node('color_recognition')
-    Color_Recognition()
-    rospy.spin()
-
-

src/rasp_camera/src/color_detection.cpp

@@ -0,0 +1,134 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-27 14:30:30
+ * @Description: 颜色检测类实现
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#include "color_detection.h"
+
+Color_Detection::Color_Detection(){
+
+    //订阅者订阅压缩图像
+    _img_sub = nh.subscribe<sensor_msgs::CompressedImage>("/raspicam_node/image/compressed", 1,
+                                                         &Color_Detection::img_callback, this);
+    //发布者发布速度消息
+    _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+    
+}
+
+Color_Detection::~Color_Detection(){
+
+}
+
+/**
+ * @description: 订阅图像数据回调函数
+ */
+void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr & data){
+
+    cv_bridge::CvImagePtr cv_ptr;
+    //将ROS中图像数据转化成OpenCV-BRG8数据
+    cv_ptr = cv_bridge::toCvCopy(data, sensor_msgs::image_encodings::BGR8);
+
+    cv::Mat img, img_copy;
+    cv_ptr->image.copyTo(img);
+    cv_ptr->image.copyTo(img_copy);
+    
+    double prec_tick = ticks;
+    ticks = (double)cv::getTickCount();
+
+    double dT = (ticks - prec_tick) / cv::getTickFrequency();
+    
+    if(found){
+        //卡尔曼滤波预测跟踪小球
+        img_kalman_filter.found_draw(dT, img_copy, x, area);
+        //根据小球与小车的位置关系发布小车的速度
+        Color_Detection::cmd_vel_pub(x, area);
+    }
+
+    // >>>>> OpenCV处理图像找出画面中蓝色小球
+    cv::Mat img_blur;
+    cv::GaussianBlur(img, img_blur, cv::Size(5, 5), 3.0, 3.0);
+
+    cv::Mat img_hsv;
+    cv::cvtColor(img_blur, img_hsv, CV_BGR2HSV);
+
+    cv::Mat img_range = cv::Mat::zeros(img.size(), CV_8UC1);
+    //修改上下限可以调整颜色
+    cv::inRange(img_hsv, cv::Scalar(MIN_H_BLUE / 2, 100, 80),
+                cv::Scalar(MAX_H_BLUE / 2, 255, 255), img_range);
+
+    cv::erode(img_range, img_range, cv::Mat(), cv::Point(-1, -1), 2);
+    cv::dilate(img_range, img_range, cv::Mat(), cv::Point(-1, -1), 2);
+
+    vector<vector<cv::Point>> contours;
+    cv::findContours(img_range, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+
+    vector<vector<cv::Point>> balls;
+    vector<cv::Rect> balls_box;
+    for (size_t i = 0; i < contours.size(); i++)
+    {
+        cv::Rect b_box;
+        b_box = cv::boundingRect(contours[i]);
+        float ratio = (float)b_box.width / (float)b_box.height;
+        if(ratio > 1.0f){
+            ratio = 1.0f / ratio;
+        }
+        if(ratio > 0.75 && b_box.area() >= 400){
+            balls.push_back(contours[i]);
+            balls_box.push_back(b_box);
+        }
+    }
+    cout << "Balls found:" << balls_box.size() << endl;
+    // <<<<< OpenCV处理图像找出画面中蓝色小球
+
+    if(balls.size() == 0){
+        not_found_count++;
+        cout << "notFoundCount:" << not_found_count << endl;
+        if(not_found_count >= 100 ){
+            found = false;
+        }
+    }else{
+        not_found_count = 0;
+        //卡尔曼滤波更新
+        img_kalman_filter.kalman_update(found, balls_box);
+    }
+
+    //显示图像
+    cv::imshow("color_detection", img_copy);
+    cv::waitKey(1);
+    
+}
+
+/**
+ * @description: 根据小球与小车位置控制小车移动，当小球不在小车中心时控制小车旋转，
+ *               当小球距小车较远时小车前进，当小球距小车较近时小扯后退
+ */
+void Color_Detection::cmd_vel_pub(float x, float area){
+    _twist.linear.x = 0;
+    _twist.angular.z = 0;
+
+    float x_err = x - x_center;
+    float area_err = area - area_center;
+    if(abs(x_err) > 5){
+        _twist.angular.z = 0.74 * - x_err * 0.006;
+    }
+    if(abs(_twist.angular.z) > 0.8){
+        _twist.angular.z = copysign(0.8, _twist.angular.z);
+    }
+    if(abs(_twist.angular.z) < 0.1){
+        _twist.angular.z = 0;
+    }
+    if(abs(area_err) > 500){
+        _twist.linear.x = 0.0074 * - area_err * 0.006;
+    }
+    if(abs(_twist.linear.x) > 0.3){
+        _twist.linear.x = copysign(0.2, _twist.linear.x);
+    }
+    if(abs(_twist.linear.x) < 0.015){
+        _twist.linear.x = 0;
+    }
+    
+    _cmd_vel_pub.publish(_twist);
+
+}

src/rasp_camera/src/color_detection_node.cpp

@@ -0,0 +1,18 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-27 14:55:50
+ * @Description: 颜色检测节点main函数入口文件
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#include "color_detection.h"
+
+int main(int argc, char **argv){
+
+    //初始化节点
+    ros::init(argc, argv, "color_detection_node");
+    //根据Color_Detection声明对象，调用构造函数
+    Color_Detection color_detection;
+    //使ROS中回调函数生效
+    ros::spin();
+}

src/rasp_camera/src/kalman_filter.cpp

@@ -0,0 +1,94 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-27 15:55:45
+ * @Description: 卡尔曼滤波跟踪小球实现
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#include "kalman_filter.h"
+
+/**
+ * @description: 构造函数中初始化卡尔曼滤波各项参数
+ */
+Kalman_Filter::Kalman_Filter(){
+    kf = cv::KalmanFilter(stateSize, measSize, contrSize, type);
+    state = cv::Mat(stateSize, 1, type);
+    meas = cv::Mat(measSize, 1, type);
+    cv::setIdentity(kf.transitionMatrix);
+
+    kf.measurementMatrix = cv::Mat::zeros(measSize, stateSize, type);
+    kf.measurementMatrix.at<float>(0) = 1.0f;
+    kf.measurementMatrix.at<float>(7) = 1.0f;
+    kf.measurementMatrix.at<float>(16) = 1.0f;
+    kf.measurementMatrix.at<float>(23) = 1.0f;
+
+    kf.processNoiseCov.at<float>(0) = 1e-2;
+    kf.processNoiseCov.at<float>(7) = 1e-2;
+    kf.processNoiseCov.at<float>(14) = 5.0f;
+    kf.processNoiseCov.at<float>(21) = 5.0f;
+    kf.processNoiseCov.at<float>(28) = 1e-2;
+    kf.processNoiseCov.at<float>(35) = 1e-2;
+
+    cv::setIdentity(kf.measurementNoiseCov, cv::Scalar(1e-1));
+}
+
+Kalman_Filter::~Kalman_Filter(){
+
+}
+
+/**
+ * @description: 卡尔曼滤波预测小球位置
+ */
+void Kalman_Filter::found_draw(double dT, cv::Mat &img, float &x, float &area){
+    kf.transitionMatrix.at<float>(2) = dT;
+    kf.transitionMatrix.at<float>(9) = dT;
+    state = kf.predict();
+    cout << "State post:" << endl << state << endl;
+    cv::Rect pred_rect;
+    pred_rect.width = state.at<float>(4);
+    pred_rect.height = state.at<float>(5);
+    pred_rect.x = state.at<float>(0) - pred_rect.width / 2;
+    pred_rect.y = state.at<float>(1) - pred_rect.height / 2;
+
+    cv::Point center;
+    center.x = state.at<float>(0);
+    center.y = state.at<float>(1);
+
+    x = center.x;
+    area = pred_rect.area();
+
+    cv::circle(img, center, 2, CV_RGB(255, 0, 0), -1);
+    cv::rectangle(img, pred_rect, CV_RGB(255, 0, 0), 2);
+}
+
+/**
+ * @description: 卡尔曼滤波更新
+ */
+void Kalman_Filter::kalman_update(bool &found, vector<cv::Rect> balls_box){
+    meas.at<float>(0) = balls_box[0].x + balls_box[0].width / 2;
+    meas.at<float>(1) = balls_box[0].y + balls_box[0].height / 2;
+    meas.at<float>(2) = (float)balls_box[0].width;
+    meas.at<float>(3) = (float)balls_box[0].height;
+    
+    if(!found){
+        kf.errorCovPre.at<float>(0) = 1; // px
+        kf.errorCovPre.at<float>(7) = 1; // px
+        kf.errorCovPre.at<float>(14) = 1;
+        kf.errorCovPre.at<float>(21) = 1;
+        kf.errorCovPre.at<float>(28) = 1; // px
+        kf.errorCovPre.at<float>(35) = 1; // px
+
+        state.at<float>(0) = meas.at<float>(0);
+        state.at<float>(1) = meas.at<float>(1);
+        state.at<float>(2) = 0;
+        state.at<float>(3) = 0;
+        state.at<float>(4) = meas.at<float>(2);
+        state.at<float>(5) = meas.at<float>(3);
+    
+        kf.statePost = state;
+                
+        found = true;
+    }else{
+        kf.correct(meas);
+    }
+}