PX4地标识别及控制算法

使用轮廓提取，多边形抽象，过滤面积较小的图形，然后过滤出四边形，再过滤掉非凸形。得到的四边形里通过简单的聚类方法寻找中心距离最近的一类，其中心的平均值即为地标中心

创建包

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_create_pkg landing roscpp rospy std_msgs geometry_msgs mavros
cd ~/catkin_ws/src/landing
mkdir msg #存放自定义消息类型
mkdir scripts #存放python脚本，打开相机

自定义消息类型
vi src/landing/msg/center.msg

uint32 width
uint32 height
float64 x
float64 y
bool iffind

python脚本
vi src/landing/scripts/track.py

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from landing.msg import center
import os
import cv2
import numpy as np
import time

center_publish=rospy.Publisher('/center', center, queue_size=1)
def callback(Image):
    img = np.fromstring(Image.data, np.uint8)
    img = img.reshape(240,320,3)
    track(img, Image.width, Image.height)

def listener():
    rospy.init_node('track')
    rospy.Subscriber('/iris/usb_cam/image_raw', Image, callback)
    rospy.spin()

def track(frame, width, height):
    img = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
    _, img = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
    contours = cv2.findContours(img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    rects = []
    centers = []
    for contour in contours[1]:
        if cv2.contourArea(contour) < 100: 
            continue
        epsilon = 0.02 * cv2.arcLength(contour, True)
        approx = cv2.approxPolyDP(contour, epsilon, True)
        if approx.shape[0] == 4 and cv2.isContourConvex(approx):
            rects.append(approx)
            centers.append((approx[0]+approx[1]+approx[2]+approx[3]).squeeze()/4)

    center_iter = list(range(len(centers)))
    result = []
    threshold = 20
    while len(center_iter) is not 0:
        j = 1
        resultnow = []
        while j < len(center_iter):
            if np.sum((centers[center_iter[0]] - centers[center_iter[j]])**2) < threshold:
                resultnow.append(center_iter[j])
                center_iter.pop(j)
                j = j-1
            j = j+1
        resultnow.append(center_iter[0])
        center_iter.pop(0)
        if len(result) < len(resultnow):
            result = resultnow
    rects = np.array(rects)[result]
    if len(result) > 2:
                centers = np.sum(np.array(centers)[result], axis=0).astype(np.double) / len(result)
                publish(centers, width, height)
    else:
        center_temp = center()
        center_temp.iffind = False
        center_publish.publish(center_temp)
    
def publish(centers, width, height):
    center_temp = center()
    center_temp.width = width
    center_temp.height = height
    center_temp.x = centers[1]
    center_temp.y = centers[0]
    center_temp.iffind = True
    center_publish.publish(center_temp)

if __name__ == '__main__':
    listener()

飞机控制

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <landing/center.h> // 导入自定义消息类型

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
   current_state = *msg;
}

geometry_msgs::PoseStamped local_position;
void position_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
   local_position = *msg;
}

landing::center landmark;
void center_cb(const landing::center::ConstPtr &msg)
{
   landmark = *msg;
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "landing_node");
   ros::NodeHandle nh;

   ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
                               ("mavros/state", 10, state_cb);
   ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
                                  ("mavros/local_position/pose", 10, position_cb);
   ros::Subscriber center_sub = nh.subscribe<landing::center>
                                ("center", 10, center_cb);
   ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
                                  ("mavros/setpoint_position/local", 10);
   ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
                                  ("mavros/setpoint_velocity/cmd_vel", 10);
   ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
                                      ("mavros/cmd/arming");
   ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
                                        ("mavros/set_mode");

   ros::Rate rate(20.0);

   while(ros::ok() && current_state.connected)
   {
      ros::spinOnce();
      rate.sleep();
   }

   geometry_msgs::PoseStamped pose;//姿态控制
   pose.pose.position.x = 0;
   pose.pose.position.y = 0;
   pose.pose.position.z = 2;

   geometry_msgs::TwistStamped vel;//速度控制

   for(int i = 100; ros::ok() && i > 0; --i)
   {
      local_pos_pub.publish(pose);
      ros::spinOnce();
      rate.sleep();
   }

   mavros_msgs::SetMode offb_set_mode;
   offb_set_mode.request.custom_mode = "OFFBOARD";

   mavros_msgs::CommandBool arm_cmd;
   arm_cmd.request.value = true;

   ros::Time last_request = ros::Time::now();

   //起飞
   while(ros::ok())
   {
      if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
      {
         if(set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
         {
            ROS_INFO("Offboard enabled");
         }

         last_request = ros::Time::now();
      }
      else if(!current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
      {
         if(arming_client.call(arm_cmd) && arm_cmd.response.success)
         {
            ROS_INFO("Vehicle armed");
         }

         last_request = ros::Time::now();
      }
      else if(ros::Time::now() - last_request > ros::Duration(5.0))
      {
         break;
      }

      local_pos_pub.publish(pose);

      ros::spinOnce();
      rate.sleep();
   }

   //逛一圈
   last_request = ros::Time::now();

   while(ros::ok())
   {
      if(ros::Time::now() - last_request > ros::Duration(5.0))
      {
         break;
      }

      vel.twist.linear.x = 1;
      vel.twist.linear.y = 0;
      vel.twist.linear.z = 0;
      local_vel_pub.publish(vel);
      ros::spinOnce();
      rate.sleep();
   }

   last_request = ros::Time::now();

   while(ros::ok())
   {
      if(ros::Time::now() - last_request > ros::Duration(5.0))
      {
         break;
      }

      vel.twist.linear.x = 0;
      vel.twist.linear.y = 1;
      vel.twist.linear.z = 0;
      local_vel_pub.publish(vel);
      ros::spinOnce();
      rate.sleep();
   }

   last_request = ros::Time::now();

   while(ros::ok())
   {
      if(ros::Time::now() - last_request > ros::Duration(5.0))
      {
         break;
      }

      vel.twist.linear.x = -1;
      vel.twist.linear.y = 0;
      vel.twist.linear.z = 0;
      local_vel_pub.publish(vel);
      ros::spinOnce();
      rate.sleep();
   }

   last_request = ros::Time::now();

   while(ros::ok())
   {
      if(ros::Time::now() - last_request > ros::Duration(5.0))
      {
         break;
      }

      vel.twist.linear.x = 0;
      vel.twist.linear.y = -1;
      vel.twist.linear.z = 0;
      local_vel_pub.publish(vel);
      ros::spinOnce();
      rate.sleep();
   }

   //控制降落部分
   while(ros::ok())
   {
      //高度低于0.3时转为降落模式
      if(local_position.pose.position.z < 0.3)
      {
         break;
      }

      //如果找到地标，控制方向
      if(landmark.iffind)
      {
         //计算两方向err
         double err_x = landmark.height / 2.0 - landmark.x;
         double err_y = landmark.width / 2.0 - landmark.y;
         ROS_INFO_STREAM("state=" << err_x << " " << err_y);
         //速度控制
         vel.twist.linear.x = err_x / 400;
         vel.twist.linear.y = err_y / 400;

         //如果位置很正开始降落
         if(err_x < 10 && err_y < 10)
         {
            vel.twist.linear.z = -0.2;
         }
         else
         {
            vel.twist.linear.z = 0;
         }

         local_vel_pub.publish(vel);
         ros::spinOnce();
         rate.sleep();
      }
      //如果找不到矩形地标，回到2m高度
      else
      {
         pose.pose.position.x = local_position.pose.position.x;
         pose.pose.position.y = local_position.pose.position.y;
         pose.pose.position.z = 2;
         local_pos_pub.publish(pose);
         ros::spinOnce();
         rate.sleep();
      }
   }

   offb_set_mode.request.custom_mode = "AUTO.LAND";

   if(set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
   {
      ROS_INFO("Offboard enabled");
      last_request = ros::Time::now();
   }

   return 0;
}

vi landing/CMakeLists.txt

# 按提示增加

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )
add_message_files(FILES center.msg)

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   geometry_msgs#   std_msgs
# )
generate_messages(DEPENDENCIES std_msgs)

## 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/landing_node.cpp)
add_executable(landing_node src/control.cpp)

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(landing_node
  ${catkin_LIBRARIES}
)

vi landing/package.xml

<!-- 增加两行 -->
<build_depend>message_generation</build_depend>
<exec_depend>message_generation</exec_depend>

编译
catkin build

运行

roslaunch px4 mavros_posix_sitl.launch
python src/landing/scripts/track.py
rosrun landing landing_node

实际效果
https://www.bilibili.com/video/BV1RE411M755?from=search&seid=18007439676134041510

rqt_graph

rqt_console