在Gazebo仿真中实现ROS机器人的目标跟随

cd ~/catkin_ws/src/
git clone https://github.com/6-robot/wpr_simulation.git
git clone https://github.com/6-robot/wpb_home.git
cd ~/catkin_ws/src/wpr_simulation/scripts
./install_for_kinetic.sh
cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts
./install_for_kinetic.sh

cd ~/catkin_ws/src/
git clone https://github.com/6-robot/wpr_simulation.git
git clone https://github.com/6-robot/wpb_home.git
cd ~/catkin_ws/src/wpr_simulation/scripts
./install_for_melodic.sh
cd ~/catkin_ws/src/wpb_home/wpb_home_bringup/scripts
./install_for_melodic.sh

cd catkin_ws/src/

catkin_create_pkg cv_pkg roscpp cv_bridge

指令

含义

catkin_create_pkg

创建ROS源码包（package）的指令

cv_pkg

新建的ROS源码包命名

roscpp

C++依赖项，本例程使用C++编写，所以需要这个依赖项

cv_bridge

将ROS中的图像数据格式转换成OpenCV数据格式的工具包

#include <ros/ros.h>

#include <cv_bridge/cv_bridge.h>

#include <sensor_msgs/image_encodings.h>

#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <geometry_msgs/Twist.h>

usingnamespace cv;

usingnamespace std;

staticint iLowH = 10;

staticint iHighH = 40;

staticint iLowS = 90;

staticint iHighS = 255;

staticint iLowV = 1;

staticint iHighV = 255;

geometry_msgs::Twist vel_cmd;   //速度消息包

ros::Publisher vel_pub;      //速度发布对象

void Cam_RGB_Callback(const sensor_msgs::ImageConstPtr& msg)

{

    cv_bridge::CvImagePtr cv_ptr;

    try

    {

        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

    }

    catch (cv_bridge::Exception& e)

    {

        ROS_ERROR("cv_bridge exception: %s", e.what());

        return;

    }

    Mat imgOriginal = cv_ptr->image;

    //将RGB图片转换成HSV

    Mat imgHSV;

    vector hsvSplit;

    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);

    //在HSV空间做直方图均衡化

    split(imgHSV, hsvSplit);

    equalizeHist(hsvSplit[2],hsvSplit[2]);

    merge(hsvSplit,imgHSV);

    Mat imgThresholded;

    //使用上面的Hue,Saturation和Value的阈值范围对图像进行二值化

    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

    //开操作 (去除一些噪点)

    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));

    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);

    //闭操作 (连接一些连通域)

    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);

    //遍历二值化后的图像数据

    int nTargetX = 0;

    int nTargetY = 0;

    int nPixCount = 0;

    int nImgWidth = imgThresholded.cols;

    int nImgHeight = imgThresholded.rows;

    int nImgChannels = imgThresholded.channels();

    printf("横向宽度= %d   纵向高度= %d \n",nImgWidth,nImgHeight);

    for (int y = 0; y < nImgHeight; y++)

    {

        for(int x = 0; x < nImgWidth; x++)

        {

            if(imgThresholded.data[y*nImgWidth + x] == 255)

            {

                nTargetX += x;

                nTargetY += y;

                nPixCount ++;

            }

        }

    }

    if(nPixCount > 0)

    {

        nTargetX /= nPixCount;

        nTargetY /= nPixCount;

        printf("颜色质心坐标( %d , %d )  点数 = %d\n",nTargetX,nTargetY,nPixCount);

        //画坐标

        Point line_begin = Point(nTargetX-10,nTargetY);

        Point line_end = Point(nTargetX+10,nTargetY);

        line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);

        line_begin.x = nTargetX; line_begin.y = nTargetY-10;

        line_end.x = nTargetX; line_end.y = nTargetY+10;

        line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);

        //计算机器人运动速度

        float fVelFoward = (nImgHeight/2-nTargetY)*0.002;     //差值*比例

        float fVelTurn = (nImgWidth/2-nTargetX)*0.003;        //差值*比例

        vel_cmd.linear.x = fVelFoward;

        vel_cmd.linear.y = 0;

        vel_cmd.linear.z = 0;

        vel_cmd.angular.x = 0;

        vel_cmd.angular.y = 0;

        vel_cmd.angular.z = fVelTurn;

    }

    else

    {

        printf("目标颜色消失...\n");

        vel_cmd.linear.x = 0;

        vel_cmd.linear.y = 0;

        vel_cmd.linear.z = 0;

        vel_cmd.angular.x = 0;

        vel_cmd.angular.y = 0;

        vel_cmd.angular.z = 0;

    }

    vel_pub.publish(vel_cmd);

    printf("机器人运动速度( linear= %.2f , angular= %.2f )\n",vel_cmd.linear.x,vel_cmd.angular.z);

    //显示处理结果

    imshow("RGB", imgOriginal);

    imshow("Result", imgThresholded);

    cv::waitKey(1);

}

int main(intargc, char **argv)

{

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

    ros::NodeHandle nh;

    ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1 , Cam_RGB_Callback);

    vel_pub = nh.advertise("/cmd_vel", 30);

    ros::Rate loop_rate(30);

    //生成图像显示和参数调节的窗口空见

    namedWindow("Threshold", CV_WINDOW_AUTOSIZE);

    cvCreateTrackbar("LowH", "Threshold", &iLowH, 179); //Hue (0 - 179)

    cvCreateTrackbar("HighH", "Threshold", &iHighH, 179);

    cvCreateTrackbar("LowS", "Threshold", &iLowS, 255); //Saturation (0 - 255)

    cvCreateTrackbar("HighS", "Threshold", &iHighS, 255);

    cvCreateTrackbar("LowV", "Threshold", &iLowV, 255); //Value (0 - 255)

    cvCreateTrackbar("HighV", "Threshold", &iHighV, 255);

    namedWindow("RGB");

    namedWindow("Result");

    while( ros::ok())

    {

        ros::spinOnce();

        loop_rate.sleep();

    }

}

find_package(OpenCV REQUIRED)

add_executable(cv_follow_node

  src/cv_follow_node.cpp

)

add_dependencies(cv_follow_node

 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(cv_follow_node

  ${catkin_LIBRARIES}

  ${OpenCV_LIBS}

)

cd catkin_ws/

catkin_make

roslaunch wpr_simulation wpb_balls.launch

rosrun cv_pkg cv_follow_node

rosrun wpr_simulation ball_random_move

~/catkin_ws/src/wpr_simulation/src/demo_cv_follow.cpp

rosrun wpr_simulation demo_cv_follow

红色球

rosrun wpr_simulation ball_random_move red

绿色球

rosrun wpr_simulation ball_random_move green

蓝色球

rosrun wpr_simulation ball_random_move blue