增加三个小程序，根据imu信息可以使小车方向固定，红外测据传感器距离保持，最后就是颜色跟踪小程序，小车可以跟着指定颜色移动

src/robot_demo/CMakeLists.txt

@@ -0,0 +1,232 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_demo)
+
+set(OpenCV_DIR /usr/share/OpenCV)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## 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 COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  cv_bridge
+  ros_arduino_msgs
+  OpenCV REQUIRED
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES robot_demo
+#  CATKIN_DEPENDS roscpp stdmsgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/robot_demo.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## 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}_color_node
+              src/color_detection_node.cpp
+              src/color_detection.cpp
+              src/kalman_filter.cpp)
+
+add_executable(${PROJECT_NAME}_imu_node src/imu_demo_node.cpp)
+add_executable(${PROJECT_NAME}_infrared_node src/infrared_demo_node.cpp)
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## 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(${PROJECT_NAME}_infrared_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_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+)
+
+target_link_libraries(${PROJECT_NAME}_imu_node
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_infrared_node
+   ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_demo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_demo/cfg/param.yaml

@@ -0,0 +1,10 @@
+#####################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:测试红外小程序
+#   center_dist:红外测距时悬停的距离.
+# Author: adam_zhuo
+# History:
+#   20210512:init this file.
+######################################################
+
+center_dist: 0.15

src/robot_demo/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/robot_demo/include/imu_demo.h

@@ -0,0 +1,36 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-30 15:29:44
+ * @Description: imu小程序头文件
+ * @History: 20210430:初始化文件-adam_zhuo
+ */
+#ifndef _IMU_DEMO_H
+#define _IMU_DEMO_H
+
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Twist.h>
+
+class Imu_Demo
+{
+private:
+    bool start_flag;
+    int target_angle;
+    float target_radian;
+    float current_radian;
+    float target_radian_copy;
+    float current_radian_copy;
+    ros::NodeHandle nh;
+    ros::Subscriber _angle_sub;
+    ros::Subscriber _yaw_sub;
+    ros::Publisher _cmd_vel_pub;
+    geometry_msgs::Twist twist;
+public:
+    Imu_Demo();
+    ~Imu_Demo();
+    void angle_callback(const std_msgs::Float32::ConstPtr &data);
+    void yaw_callback(const std_msgs::Float32::ConstPtr &data);
+};
+
+#endif

src/robot_demo/include/infrared_demo.h

@@ -0,0 +1,29 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 09:48:43
+ * @Description: 红外demo头文件
+ * @History: 20210512:初始化文件-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/robot_demo/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/robot_demo/launch/color_demo.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="robot_demo" type="robot_demo_color_node" name="robot_demo_color_node" output="screen">
+
+  </node>
+</launch>

src/robot_demo/launch/imu_demo.launch

@@ -0,0 +1,12 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 11:16:35
+ * @Description: 启动imu程序节点
+ * @History: 20210512:初始化文件-adam_zhuo
+-->
+<launch>
+  <!-- 启动robot_demo_imu_node -->
+  <node pkg="robot_demo" type="robot_demo_imu_node" name="robot_demo_imu_node" output="screen">
+  </node>
+</launch>

src/robot_demo/launch/infrared_demo.launch

@@ -0,0 +1,16 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 09:53:40
+ * @Description: 启动红外demo节点
+ * @History: 20210429:初始化文件-adam_zhuo
+-->
+<launch>
+  <!-- 加载参数 -->
+  <arg name="cfg_file" default="$(find robot_demo)/cfg/param.yaml" />
+
+  <!-- 启动robot_demo_infrared_node -->
+  <node pkg="robot_demo" type="robot_demo_infrared_node" name="robot_demo_infrared_node" output="screen">
+    <rosparam file="$(arg cfg_file)" command="load" />
+  </node>
+</launch>

src/robot_demo/package.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_demo</name>
+  <version>0.0.0</version>
+  <description>The robot_demo package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="adam_zhuo@corvin.cn">adam_zhuo</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robot_demo</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>ros_arduino_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>ros_arduino_msgs</build_export_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>ros_arduino_msgs</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_demo/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(100, 100, MIN_H_BLUE / 2),
+                cv::Scalar(255, 255, MAX_H_BLUE / 2), 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/robot_demo/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/robot_demo/src/imu_demo_node.cpp

@@ -0,0 +1,80 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-30 15:29:58
+ * @Description: imu小程序
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+#include "imu_demo.h"
+
+/**
+ * @description: 构造函数初始化变量。
+ */
+Imu_Demo::Imu_Demo(){
+    start_flag = false;
+    _angle_sub = nh.subscribe<std_msgs::Float32>("/angle", 1, &Imu_Demo::angle_callback, this);
+    _yaw_sub = nh.subscribe<std_msgs::Float32>("/yaw_data", 1, &Imu_Demo::yaw_callback, this);
+    _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+}
+
+Imu_Demo::~Imu_Demo(){
+
+}
+
+/**
+ * @description: 接收指定角度回调函数。程序启动后,发送指定角度，任意放置小车，小车最终朝向都会是指定角度。
+ */
+void Imu_Demo::angle_callback(const std_msgs::Float32::ConstPtr &data){
+    start_flag = true;
+    target_radian = data->data/180*M_PI;
+    target_radian_copy = target_radian;
+    if(target_radian < 0)
+        target_radian += (M_PI * 2);
+}
+
+/**
+ * @description: yaw角回调函数，不断检测当前小车朝向与指定角度的差值，如果大于一个阈值，则控制小车旋转。
+ */
+void Imu_Demo::yaw_callback(const std_msgs::Float32::ConstPtr &data){
+
+    //如果没有接收到发送的指定角度，则不做任何处理直接return
+    if(!start_flag)
+        return;
+
+    //获取当前yaw角值
+    current_radian = data->data ;
+    current_radian_copy = current_radian;
+
+    // >>>判断与指定角度的差值，控制小车旋转
+    if(current_radian < 0)
+        current_radian += (M_PI * 2);
+    current_radian = current_radian - target_radian;
+
+    if(current_radian < 0)
+        current_radian += (M_PI * 2);
+
+    if(abs(target_radian_copy - current_radian_copy) < 0.1)
+    {
+        twist.angular.z = 0;
+    }
+    else
+    {
+        twist.angular.z = copysign(0.8, (current_radian - M_PI));
+    }
+    // <<<判断与指定角度的差值，控制小车旋转
+
+    //向小车发送控制命令
+    _cmd_vel_pub.publish(twist);
+}
+
+int main(int argc, char **argv)
+{
+    //初始化节点
+    ros::init(argc, argv, "imu_demo_node");
+
+    //根据Imu_Demo声明对象，调用构造函数
+    Imu_Demo imu_demo;
+
+    //使ROS中回调函数生效
+    ros::spin();
+}

src/robot_demo/src/infrared_demo_node.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/robot_demo/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);
+    }
+}