单二维码与自动跟随(改)

本篇内容：使用视觉识别完成自动跟随

• 视觉识别基于Aruco实现
• 示例程序仅提供基于使用方式，仅用于示例与教学，实际使用请注意飞行安全
• 本程序依赖视觉识别sunray_vision_detection程序请自行下载编译
• 程序对识别精度有一定要求，请检查实际检测结果输出是否正确，错误的检查结果会导致降落失败甚至炸机的结果

相关依赖安装
sudo apt-get install libv4l-dev
sudo apt-get install v4l-utils
sudo apt-get install libopencv-dev
sudo apt-get install libopencv-contrib-dev

单二维码示例

程序示例中使用的尺寸：0.15x0.15m（二维码黑色边框）

自动启动脚本

• 启动跟车脚本

• 文件：Sunray/scripts_exp/demo_follow_car.sh
• 脚本内容：

复制代码

gnome-terminal --window -e 'bash -c "roslaunch sunray_uav_control sunray_mavros_exp.launch; exec bash"' \
--tab -e 'bash -c "sleep 5.0; roslaunch web_cam web_cam.launch ; exec bash"' \
--tab -e 'bash -c "sleep 5.0; roslaunch sunray_uav_control external_fusion.launch external_source:=3; exec bash"' \
--tab -e 'bash -c "sleep 2.0; roslaunch sunray_uav_control sunray_control_node.launch uav_id:=1; exec bash"' \
--tab -e 'bash -c "sleep 2.0; roslaunch sunray_uav_control terminal_control.launch uav_id:=1; exec bash"' \
gnome-terminal --window -e 'bash -c "sleep 3.0; roslaunch sunray_tutorial qrcode_detection_down.launch ; exec bash"' \
--tab -e 'bash -c "sleep 5.0; rosrun rqt_image_view rqt_image_view; exec bash"' \
--tab -e 'bash -c "sleep 8.0; roslaunch sunray_tutorial follow_a_car.launch; exec bash"' \

• 终端输入：

复制代码

./demo_follow_car.sh

• 启动跟标脚本

• 文件：Sunray/scripts_exp/demo_follow_tag.sh
• 脚本内容：

复制代码

gnome-terminal --window -e 'bash -c "roslaunch sunray_uav_control sunray_mavros_exp.launch; exec bash"' \
--tab -e 'bash -c "sleep 5.0; roslaunch web_cam web_cam.launch ; exec bash"' \
--tab -e 'bash -c "sleep 5.0; roslaunch sunray_uav_control external_fusion.launch external_source:=3; exec bash"' \
--tab -e 'bash -c "sleep 2.0; roslaunch sunray_uav_control sunray_control_node.launch uav_id:=1; exec bash"' \
--tab -e 'bash -c "sleep 2.0; roslaunch sunray_uav_control terminal_control.launch uav_id:=1; exec bash"' \
gnome-terminal --window -e 'bash -c "sleep 3.0; roslaunch sunray_tutorial qrcode_detection_down.launch ; exec bash"' \
--tab -e 'bash -c "sleep 5.0; rosrun rqt_image_view rqt_image_view; exec bash"' \
--tab -e 'bash -c "sleep 8.0; roslaunch sunray_tutorial follow_a_tag.launch; exec bash"' \

• 终端输入：

复制代码

./demo_follow_tag.sh

手动启动节点

运行mavros连接飞控

bash 复制代码

# 打开终端 启动ros
cd ~/Sunray
roscore

# 启动mavros连接无人机
roslaunch sunray_uav_control sunray_mavros_exp.launch 

运行定位算法

bash 复制代码

# 启动外部定位节点和动捕
roslaunch sunray_uav_control external_fusion.launch external_source:=3

启动相机节点

如果打开相机不为下视相机或打开失败请修改配置中的串口为实际设备序号 一般为/dev/video0-videon
配置文件：sunray_vision_detection/web_cam/config/web_cam.yml

复制代码

roslaunch web_cam web_cam.launch
# 可以打开rqt_image_view查看是否有对应的图像

运行二维码识别节点

复制代码

roslaunch sunray_tutorial qrcode_detection.launch
# 可以打开rqt_image_view查看是否有对应的图像

运行自动降落节点

请根据实际实际跟随方式选择要运行的节点，一下脚本二选一运行

• 下视单个二维码识别

复制代码

roslaunch sunray_tutorial follow_a_car.launch
# 可以打开rqt_image_view查看是否有识别到二维码

• 前视单个二维码识别

复制代码

roslaunch sunray_tutorial follow_a_tag.launch
# 可以打开rqt_image_view查看是否有识别到二维码

• 启动脚本
• 请修改camera_name和image_topic为实际图像话题，订阅的话题将以camera_name/image_topic的形式订阅

# qrcode_detection.launch
<launch>
    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find sunray_tutorial)/config/rosconsole.config"/>
    <arg name="camera_name" default="/sunray/camera/monocular_down" />
    <arg name="image_topic" default="image_raw" />
    <arg name="queue_size" default="1" />

    <node pkg="qrcode_detection_ros" type="qrcode_detection_ros_node" name="qrcode_detection_ros_node" output="screen">
        <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
        <remap from="camera_info" to="$(arg camera_name)/camera_info" />

        <param name="queue_size" type="int" value="$(arg queue_size)" />

        <param name="uav_id" type="int" value="1" />
        <param name="x_bias" type="double" value="0" />
        <param name="y_bias" type="double" value="0" />
        <param name="draw_tag_detections_image" type="bool" value="true" />

        <param name="algorithm_parameters" type="str" value="$(find sunray_tutorial)/config/qrcode_detector_params.json" />
        <param name="camera_parameters" type="str" value="$(find sunray_tutorial)/config/head_camera.yaml" />
        <param name="local_saving_path" type="str" value="$(find sunray_tutorial)/output" />
    </node>
</launch>

• qrcode_detector_params.json 识别相关配置
  实际使用时修改dictionaryId为对应的二维码标签与markerLengths实际尺寸即可，以单二维码黑色边框实际尺寸为准

{
    "ArucoDetector": {
        "dictionaryId": 1,
        "markerIds": [92],
        "markerLengths": [0.155],
        "adaptiveThreshConstant": 7,
        "adaptiveThreshWinSizeMax": 23,
        "adaptiveThreshWinSizeMin": 3,
        "adaptiveThreshWinSizeStep": 10, 
        "aprilTagCriticalRad": 0.17453292519,
        "aprilTagDeglitch": 0,
        "aprilTagMaxLineFitMse": 10.0,
        "aprilTagMaxNmaxima": 10,
        "aprilTagMinClusterPixels": 5,
        "aprilTagMinWhiteBlackDiff": 5,
        "aprilTagQuadDecimate": 0.0,
        "aprilTagQuadSigma": 0.0,
        "cornerRefinementMaxIterations": 30,
        "cornerRefinementMethod": 0,
        "cornerRefinementMinAccuracy": 0.1,
        "cornerRefinementWinSize": 5,
        "detectInvertedMarker": false,
        "errorCorrectionRate": 0.6,
        "markerBorderBits": 1,
        "maxErroneousBitsInBorderRate": 0.35,
        "maxMarkerPerimeterRate": 4.0,
        "minCornerDistanceRate": 0.05,
        "minDistanceToBorder": 3,
        "minMarkerDistanceRate": 0.05,
        "minMarkerLengthRatioOriginalImg": 0,
        "minMarkerPerimeterRate": 0.03,
        "minOtsuStdDev": 5.0,
        "minSideLengthCanonicalImg": 32,
        "perspectiveRemoveIgnoredMarginPerCell": 0.13,
        "perspectiveRemovePixelPerCell": 4,
        "polygonalApproxAccuracyRate": 0.03,
        "useAruco3Detection": false
    }
}

• head_camera.yaml 相机内参

%YAML:1.0
---
calibration_time: "Mon Aug 12 14:19:51 2024"
image_width: 640
image_height: 480
camera_name: usb_cam
distortion_model: plumb_bob
camera_matrix: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 5.8073836624234639e+02, 0., 3.4026653041851887e+02, 0.,
       5.8193473945541200e+02, 2.3147452752869887e+02, 0., 0., 1. ]
distortion_coefficients: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 3.6130919074799477e-02, 1.1900433468807413e-01,
       2.6210657400892435e-03, -1.3749174061884208e-03,
       -1.7425085463579266e+00 ]
rectification_matrix: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
projection_matrix: !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [ 5.8073836624234639e+02, 0., 3.4026653041851887e+02,
       -1.7632733596808731e-01, 0., 5.8193473945541200e+02,
       2.3147452752869887e+02, -4.5388923997248358e-02, 0., 0., 1.,
       4.3764930595089069e-01 ]
avg_reprojection_error: 1.6889888091837532e-01
aruco_reprojection_error: 1.2109431741860688e+00