引言:为了提高yolo识别的质量,提高了yolo的版本,改用yolov8进行物体识别,同时系统兼容了低版本的yolo,包括基于C++的yolov3和yolov4,以及yolov7。
简介,为了提高识别速度,系统采用了GPU进行加速,在使用7W功率的情况,大概可以稳定在20FPS,满功率情况下可以适当提高。
硬件:D435摄像头,Jetson orin nano 8G
环境:ubuntu20.04,ros-noetic, yolov8
注:目标跟随是在木根识别的基础上进行,因此本小节和yolov8识别小节类似,只是在此基础上添加了跟随控制程序
步骤一: 启动摄像头,获取摄像头发布的图像话题
roslaunch realsense2_camera rs_camera. launch
没有出现红色报错,出现如下界面,表明摄像头启动成功
步骤二:启动yolov8识别节点
roslaunch yolov8_ros yolo_v8. launch
launch文件如下,参数use_cpu设置为false,因为实际使用GPU加速,不是CPU跑,另外参数pub_topic是yolov8识别到目标后发布出来的物体在镜头中的位置,程序作了修改,直接给出目标物的中心位置,其中参数image_topic是订阅的节点话题,一定要与摄像头发布的实际话题名称对应上。
< ? xml version= "1.0" encoding= "utf-8" ? >
< launch>
< ! -- Load Parameter -- >
< param name= "use_cpu" value= "false" / >
< ! -- Start yolov8 and ros wrapper -- >
< node pkg= "yolov8_ros" type= "yolo_v8.py" name= "yolov8_ros" output= "screen" >
< param name= "weight_path" value= "$(find yolov8_ros)/weights/yolov8n.pt" / >
< param name= "image_topic" value= "/camera/color/image_raw" / >
< param name= "pub_topic" value= "/object_position" / >
< param name= "camera_frame" value= "camera_color_frame" / >
< param name= "visualize" value= "false" / >
< param name= "conf" value= "0.3" / >
< / node>
< / launch>
出现如下界面表示yolov8启动成功
步骤三:打开rqt工具,查看识别效果
注:步骤三不是必须的,可以跳过直接进行步骤四
rqt_image_view
等待出现如下界面后,选择yolov8/detection_image查看yolov8识别效果
步骤四:启动跟随控制程序
(1)、终端启动程序
roslaunch follow_yolov8 follow_yolov8. launch
(2)、launch文件详解
< ? xml version= "1.0" encoding= "utf-8" ? >
< launch>
< param name= "target_object_id" value= "chair" / >
< node pkg= "follow_yolov8" type= "follow_yolov8" name= "follow_yolov8" output= "screen" / >
< / launch>
launch文件中加载的参数target_object_id是指定跟随的目标名称,无人机在识别到这个目标以后,会通过全向的速度控制保持目标始终在无人机的视野中。launch文件中指定参数chair,因此在识别chair以后,可以看到终端会打印日志已经识别到指定的目标物
步骤五:控制部分代码
此处抛砖引玉,仅仅做最简单的速度控制,读者可以根据自己的理解,添加类似PID等控制跟随的算法,本文不再展开
#include < ros/ ros. h>
#include < std_msgs/ Bool. 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 < mavros_msgs/ PositionTarget. h>
#include < cmath>
#include < tf/ transform_listener. h>
#include < nav_msgs/ Odometry. h>
#include < mavros_msgs/ CommandLong. h>
#include < yolov8_ros_msgs/ BoundingBoxes. h>
#include < string>
#define MAX_ERROR 50
#define VEL_SET 0.15
#define ALTITUDE 0.40
using namespace std;
yolov8_ros_msgs : : BoundingBoxes object_pos;
nav_msgs : : Odometry local_pos;
mavros_msgs : : State current_state;
mavros_msgs : : PositionTarget setpoint_raw;
double position_detec_x = 0 ;
double position_detec_y = 0 ;
std : : string Class = "no_object" ;
std : : string target_object_id = "eight" ;
void state_cb ( const mavros_msgs : : State: : ConstPtr& msg) ;
void local_pos_cb ( const nav_msgs : : Odometry: : ConstPtr& msg) ;
void object_pos_cb ( const yolov8_ros_msgs : : BoundingBoxes: : ConstPtr& msg) ;
int main ( int argc, char ** argv )
{
setlocale ( LC_ALL , "" ) ;
ros : : init ( argc, argv, "follow_yolov8" ) ;
ros : : NodeHandle nh;
ros : : Subscriber state_sub = nh. subscribe< mavros_msgs: : State> ( "mavros/state" , 100 , state_cb) ;
ros : : Subscriber local_pos_sub = nh. subscribe< nav_msgs: : Odometry> ( "/mavros/local_position/odom" , 100 , local_pos_cb) ;
ros : : Subscriber object_pos_sub = nh. subscribe< yolov8_ros_msgs: : BoundingBoxes> ( "object_position" , 100 , object_pos_cb) ;
ros : : Publisher local_pos_pub = nh. advertise< geometry_msgs: : PoseStamped> ( "mavros/setpoint_position/local" , 100 ) ;
ros : : Publisher mavros_setpoint_pos_pub = nh. advertise< mavros_msgs: : PositionTarget> ( "/mavros/setpoint_raw/local" , 100 ) ;
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 : : ServiceClient ctrl_pwm_client = nh. serviceClient< mavros_msgs: : CommandLong> ( "mavros/cmd/command" ) ;
ros : : Rate rate ( 20.0 ) ;
ros : : param: : get ( "target_object_id" , target_object_id) ;
while ( ros: : ok ( ) && current_state. connected)
{
ros : : spinOnce ( ) ;
rate. sleep ( ) ;
}
setpoint_raw. type_mask = 1 + 2 + + 64 + 128 + 256 + 512 + 1024 + 2048 ;
setpoint_raw. coordinate_frame = 8 ;
setpoint_raw. position. x = 0 ;
setpoint_raw. position. y = 0 ;
setpoint_raw. position. z = 0 + ALTITUDE ;
mavros_setpoint_pos_pub. publish ( setpoint_raw) ;
for ( int i = 100 ; ros: : ok ( ) && i > 0 ; -- i)
{
mavros_setpoint_pos_pub. publish ( setpoint_raw) ;
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 ( ) ;
}
}
if ( ros: : Time: : now ( ) - last_request > ros: : Duration ( 5.0 ) )
break ;
mavros_setpoint_pos_pub. publish ( setpoint_raw) ;
ros : : spinOnce ( ) ;
rate. sleep ( ) ;
}
while ( ros: : ok ( ) )
{
if ( Class == target_object_id)
{
ROS_INFO ( "识别到目标,采用速度控制进行跟随" ) ;
if ( position_detec_x- 320 >= MAX_ERROR )
{
setpoint_raw. velocity. y = - VEL_SET ;
}
else if ( position_detec_x- 320 <= - MAX_ERROR )
{
setpoint_raw. velocity. y = VEL_SET ;
}
else
{
setpoint_raw. velocity. y = 0 ;
}
if ( position_detec_y- 240 >= MAX_ERROR )
{
setpoint_raw. velocity. x = - VEL_SET ;
}
else if ( position_detec_y- 240 <= - MAX_ERROR )
{
setpoint_raw. velocity. x = VEL_SET ;
}
else
{
setpoint_raw. velocity. x = 0 ;
}
}
else
{
setpoint_raw. velocity. x = 0 ;
setpoint_raw. velocity. y = 0 ;
}
setpoint_raw. type_mask = 1 + 2 + + 64 + 128 + 256 + 512 ;
setpoint_raw. coordinate_frame = 8 ;
setpoint_raw. velocity. x = 0 ;
setpoint_raw. position. z = 0 + ALTITUDE ;
setpoint_raw. yaw = 0 ;
mavros_setpoint_pos_pub. publish ( setpoint_raw) ;
ros : : spinOnce ( ) ;
rate. sleep ( ) ;
}
return 0 ;
}
void state_cb ( const mavros_msgs : : State: : ConstPtr& msg)
{
current_state = * msg;
}
void local_pos_cb ( const nav_msgs : : Odometry: : ConstPtr& msg)
{
local_pos = * msg;
}
void object_pos_cb ( const yolov8_ros_msgs : : BoundingBoxes: : ConstPtr& msg)
{
object_pos = * msg;
position_detec_x = object_pos. bounding_boxes[ 0 ] . xmin;
position_detec_y = object_pos. bounding_boxes[ 0 ] . ymin;
Class = object_pos. bounding_boxes[ 0 ] . Class;
}
从图中可以看出,在10W功率的情况下,大概在30帧的效果,识别准确度比较高