代码拉取完成，页面将自动刷新
### ROS-PX4无人机仿真教程（二）：无人机目标识别并降落之识别圆形图片（非定点降落）
糯米喵 松灵机器人
本套教程适用于有 ros 基础的学习者，需要使用 px4 固件，教程内容会涉及到仿真飞机的搭建，仿真自主飞行，仿真 slam 建图，以及实机的相应操作，后面教程会陆续更新。
本篇教程主要为: 1.将圆形地标图像导入 gazebo
2.飞行一周过程中识别目标图片并降落
### 1、 将地标图像导入 gazebo ：
在 `~/.gazebo/model`下，创建如下结构的文件夹及文件
```
land_mark
model.sdf 文件
model.config 文件
materials 文件夹
| --scripts 文件夹
land_mark.material
| --textures 文件夹
aim.png 地标图像
```
在命令行输入
`mkdir land_mark/materials/scripts`
创建对应的文件夹，在 `scripts` 文件夹下的命令行中输入
`touch land_mark.material`
创建文件
其他文件及文件夹的创建同上
各文件内容如下：
`model.sdf`文件
```
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="land_mark">
<link name='link'>
<pose>0 0 0.115 0 0 0</pose>
<inertial>
<mass>0.390</mass>
<inertia>
<ixx>0.00058</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00058</iyy>
<iyz>0</iyz>
<izz>0.00019</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>.496 .496 .01</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.496 .496 .01</size>
</box>
</geometry>
<material>
<script>
<uri>model://land_mark/materials/scripts</uri>
<uri>model://land_mark/materials/textures</uri>
<name>Mark/Diffuse</name>
</script>
</material>
</visual>
</link>
</model>
</sdf>
```
文件`model.config`
```
<?xml version="1.0"?>
<model>
<name>land_mark</name>
<version>1.0</version>
<sdf version="1.4">model.sdf</sdf>
<author>
<name>chow</name>
</author>
<description>
Landing Mark
</description>
</model>
```
文件`land_mark.material`
```
material Mark/Diffuse
{
receive_shadows off
technique
{
pass
{
texture_unit
{
texture ../textures/aim.png
}
}
}
}
```
```aim.png```为图片名称，若有格式错误将 png 后缀换成 jpg 即可
最后将整个```land_mark```包放入主目录`.gazebo/model`下
在左边```insert```中找到`land_mark`，点击将圆形地标拖入 gazebo,这样我们用于目标识别的圆形地标图像就导入 gazebo 中了
### 2. 飞行一周过程中识别目标图片并降落
利用霍夫变换获取圆形，原理：选取一个圆的任意点设定为圆形进行绘制圆形，交与一点，再将平面直角坐标系上的各点，通过公式转到极坐标上，获取圆心位置，然后通过半径进行绘制出圆。
树莓派快速安装 cv2：
```sudo apt-get install python-opencv```
### 目标识别：
（1）建立工作空间，在新的命令行依次输入：
```
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脚本
```
（2）自定义消息类型 由cv2识别出的圆形中心点将依照某种自定义格式发布到topic上，自定义center消息类型如下： 在msg文件夹下创建`center.msg`文件，消息格式我定义为图片的宽和高，圆形中心的位置，以及是否检测到圆形，文件内容如下：
```
uint32 width
uint32 height
float64 x
float64 y
bool iffind
```
（3）创建识别脚本 在`scripts`下创建`track.py`，利用霍夫变换获取圆形的原理。
代码简介：
```
cv2.HoughCircles(cimage,cv2.HOUGH_GRADIENT,1,20,param1=50,param2=30,minRadius=0,maxRadius=0)
```
cv2.HOUGH_GRADIENT：基于梯度计算
1：步长
20：最小距离即检测出来的两个圆形之间的最小距离，超过这个最小距离才会认定是两个圆
param1=50,param2=30：高低阈值的设定
minRadius，默认值0，表示圆半径的最小值
maxRadius，也有默认值0，表示圆半径的最大值
```
import rospy
import matplotlib.pyplot as plt
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):
dst = cv2.pyrMeanShiftFiltering(frame, 10, 100)
gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
plt.subplot(121), plt.imshow(gray, 'gray')
plt.xticks([]), plt.yticks([])
circles1 = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1,
20, param1=50, param2=30, minRadius=0, maxRadius=0)
print("running")
print(circles1)
if circles1 is not None:
circles = circles1[0, :, :]
circles = np.uint16(np.around(circles))
for i in circles[:]:
cv2.circle(frame,(i[0], i[1]), i[2], (255, 0, 0),2)
cv2.circle(frame,(i[0], i[1]), 2, (255, 0, 255), 2)
cv2.rectangle(frame, (i[0] - i[2], i[1] + i[2]), (i[0] + i[2], i[1] - i[2]), (255, 255, 0), 5)
print([i[0],i[1]])
publish([i[0],i[1]], width, height)
else:
center_temp = center()
center_temp.iffind = False
print(center_temp)
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()
```
（4）通过圆形中心点距离图片中心的距离控制，使用 pid 算法。
此算法控制无人机飞行一周的过程中若是识别到目标图片则降落，也可以先飞行一圈在终点识别是否有目标图片再进行降落或悬停，本教程主要采用第一种算法。
两种`control.cpp`控制代码相应如下:
第一种：
```
#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()&&(landmark.iffind==0))
{
if(ros::Time::now() - last_request > ros::Duration(5.0))
break;
if(landmark.iffind==1)
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()&&(landmark.iffind==0))
{
if(ros::Time::now() - last_request > ros::Duration(5.0))
break;
if(landmark.iffind==1)
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()&&(landmark.iffind==0))
{
if(ros::Time::now() - last_request > ros::Duration(5.0))
break;
if(landmark.iffind==1)
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()&&(landmark.iffind==0))
{
if(ros::Time::now() - last_request > ros::Duration(5.0))
break;
if(landmark.iffind==1)
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/800;
vel.twist.linear.y = err_y/800;
//如果位置很正开始降落
if(err_x < 5&& err_y < 5)
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;
}
```
第二种：
```
#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/800;
vel.twist.linear.y = err_y/800;
//如果位置很正开始降落
if(err_x < 5 && err_y < 5)
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;
}
```
（5）`CmakeLists.txt` 代码如下：
```
cmake_minimum_required(VERSION 2.8.3)
project(landing)
find_package(catkin REQUIRED COMPONENTS
mavros
message_generation
roscpp
rospy
std_msgs
)
add_message_files(FILES center.msg) # 这句话要注意添加
generate_messages(DEPENDENCIES std_msgs) # 这句话要注意添加
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES landing
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(landing_node src/control.cpp) # 这句话要注意添加
# 这句话要注意添加
target_link_libraries(landing_node
${catkin_LIBRARIES}
)
```
(6) `package.xml` 代码如下：
```
<?xml version="1.0"?>
<package format="2">
<name>landing</name>
<version>0.0.0</version>
<description>The landing package</description>
<maintainer email="1596724983@qq.com">cyrilsterling</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>mavros</build_depend>
<!-- 这句话要注意添加 -->
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>mavros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>mavros</exec_depend>
<!-- 这句话要注意添加 -->
<exec_depend>message_generation</exec_depend>
<export>
</export>
</package>
```
(7)在新的命令行编译`landing`文件夹，输入如下命令：
```
cd ~/catkin_ws
catkin_make --only-pkg-with-deps landing
```
如图则编译成功
最后开启三个新的命令行，一定要先 source,运行顺序如下：
```roslaunch px4 mavros_posix_sitl.launch
python src/landing/scripts/track.py
rosrun landing landing_node
```
若想获得无人机相机摄像头拍摄的飞行时画面，可在新的终端中输入：
```
rqt
```
即可看到
当无人机在飞行过程中识别到圆形图片时，在 python 端口则会返回无人机中心与圆形中心的差距，结果如图:
以下是给大家的演示视频：
至此无人机目标识别并降落教程已全部完成！
教程下期预告：无人机 gmapping 建图
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手