目录总览

小车yolo机械臂(一)ros下gazebo搭建小车(可键盘控制)安装摄像头仿真 加载yolo检测识别标记物体

小车yolo机械臂(二)机械臂仿真 ros下从xacro模型文件搭建Moveit!+Gazebo仿真系统

小车yolo机械臂(三)ROS消息订阅监听 rospy.Subscriber 订阅监听yolo python实现订阅/darknet_ros/bounding_boxes topic

小车yolo机械臂(四)python ros 和darknet_ros 使用launch文件启动脚本

小车yolo机械臂(五)让小车在命令下运动起来 rostopic pub /cmd_vel geometry_msgs/Twist

小车yolo机械臂(六)ros gazebo 小车摄像头根据darknet_ros中yolo目标检测的信息进行自主运动

小车yolo机械臂(七)小车与机械臂融合,并控制机械臂运动 gazebo control

小车yolo机械臂(八)ros小车和机械臂gazebo仿真,机械臂根据darknet_ros中yolo检测结果来自动运动 python实现

项目下载

这一篇博客,我们要将已经融合的小车和机械臂模型,添加darknet_ros(进行yolo目标检测),让机械臂根据yolo检测结果自动运动。
这篇博客所用到的代码已经上传到github:
项目下载

其中darknet_ros的的权重需要单独下载,其方法在之前的博客说了,ros下gazebo搭建小车(可键盘控制)安装摄像头仿真 加载yolo检测识别标记物体

arm_car_world.launch中添加darknet_ros
arm_car_gazebo/launch/arm_car_gazebo_control.launch中添加如下代码,用于启动darknet_ros

<launch>

    <!-- 启动Gazebo  -->
    <include file="$(find arm_car_gazebo)/launch/arm_car_world.launch" />   

    <!-- 启动Gazebo controllers -->
    <include file="$(find arm_car_gazebo)/launch/arm_car_gazebo_controller.launch" />   

    <!-- 启动darknet_ros的yolo检测-->
    <include file="$(find darknet_ros)/launch/darknet_ros.launch">
    </include>

</launch>

这个darknet_ros的导入在前面有博客讲解,链接在此(后面写入)

我们运行下面的的命令:

roslaunch arm_car_gazebo arm_car_gazebo_control.launch

可以看到如下的图(人是自己手动添加的)

在这里插入图片描述

我们看看这次运行的仿真模型有那些的话题(topic)
输入命令:

rostopic list

我们可以看到输出结果:

/arm/joint1_position_controller/command
/arm/joint2_position_controller/command
/arm/joint3_position_controller/command
/arm/joint_states
/camera/camera_info
/camera/image_raw
/camera/image_raw/compressed
/camera/image_raw/compressed/parameter_descriptions
/camera/image_raw/compressed/parameter_updates
/camera/image_raw/compressedDepth
/camera/image_raw/compressedDepth/parameter_descriptions
/camera/image_raw/compressedDepth/parameter_updates
/camera/image_raw/theora
/camera/image_raw/theora/parameter_descriptions
/camera/image_raw/theora/parameter_updates
/camera/parameter_descriptions
/camera/parameter_updates
/clock
/cmd_vel
/darknet_ros/bounding_boxes
/darknet_ros/check_for_objects/cancel
/darknet_ros/check_for_objects/feedback
/darknet_ros/check_for_objects/goal
/darknet_ros/check_for_objects/result
/darknet_ros/check_for_objects/status
/darknet_ros/detection_image
/darknet_ros/found_object
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/joint_states
/odom
/rosout
/rosout_agg
/tf
/tf_static

其中的:

/darknet_ros/bounding_boxes
/darknet_ros/check_for_objects/cancel
/darknet_ros/check_for_objects/feedback
/darknet_ros/check_for_objects/goal
/darknet_ros/check_for_objects/result
/darknet_ros/check_for_objects/status
/darknet_ros/detection_image
/darknet_ros/found_object

就是我们darknet_ros的话题,和之前的博客一样,我们在终端中打印出yolo检测的结果,输入命令:

rostopic echo /darknet_ros/bounding_boxes

结果:

header: 
  seq: 57
  stamp: 
    secs: 185
    nsecs: 860000000
  frame_id: "detection"
image_header: 
  seq: 5346
  stamp: 
    secs: 181
    nsecs: 941000000
  frame_id: "camera_link"
bounding_boxes: 
  - 
    probability: 0.866776227951
    xmin: 640
    ymin: 56
    xmax: 779
    ymax: 388
    id: 0
    Class: "person"
  - 
    probability: 0.82601493597
    xmin: 878
    ymin: 170
    xmax: 927
    ymax: 346
    id: 0
    Class: "person"

我们在看看/darknet_ros/bounding_boxes的信息。输入命令:

rostopic info /darknet_ros/bounding_boxes

结果如下:

Type: darknet_ros_msgs/BoundingBoxes

Publishers: 
 * /darknet_ros (http://winston-ThinkPad-T450:33187/)

Subscribers: 
 * /rostopic_8512_1604234684470 (http://winston-ThinkPad-T450:40669/)

可以看出/darknet_ros/bounding_boxes话题的类型为:darknet_ros_msgs/BoundingBoxes,这个在我们的arm_listener_yolo.py文件中会用到。

创建arm_listener_yolo.py
创建文件
在arm_car_gazebo文件下新建scripts文件,用来存放python文件,在scripts下新建python文件:arm_listener_yolo.py

当然,这个新建的python文件也需赋予权限,不然无法执行。
在arm_listener_yolo.py所在目录执行:

chmod +x arm_listener_yolo.py

监听yolo的检测结果

直接上代码:

#!/usr/bin/env python  
import rospy
from darknet_ros_msgs.msg import BoundingBoxes

def callback(data):
    print data.bounding_boxes[0].Class

def listener():
    rospy.init_node('topic_subscriber')

    sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

这里的python文件实现了对/darknet_ros/bounding_boxes的监听,并返回yolo的检测结果。

我们开始运行,在仿真程序运行的基础上,我们运行以下命令:

 rosrun arm_car_gazebo arm_listener_yolo.py

结果就会出现:

在这里插入图片描述

说明我们已经获取到了检测结果!

根据监听结果来控制机械臂

接下来,我们继续修改python文件,使python文件可操控机械臂。
在上一篇博客,已经实现了使用命令来控制机械臂,在终端中执行命令:

rostopic pub /arm/joint2_position_controller/command std_msgs/Float64 1.0

我们现在要把这个操作用python来实现,我们首先输入/arm/joint2_position_controller/command的信息,执行命令:

rostopic info /arm/joint2_position_controller/command

结果如下:

Type: std_msgs/Float64

Publishers: None

Subscribers: 
 * /gazebo (http://winston-ThinkPad-T450:46703/)

可以看到/arm/joint1_position_controller/command话题(topic)的类型为:std_msgs/Float64
所以python文件这样修改:

#!/usr/bin/env python  
import rospy
from darknet_ros_msgs.msg import BoundingBoxes
from std_msgs.msg import Float64

def callback(data):
    print data.bounding_boxes[0].Class

    if data.bounding_boxes[0].Class == 'person':
        pub = rospy.Publisher('/arm/joint2_position_controller/command', Float64, queue_size=10)
        pub.publish(1.0)
        print "arm jiont2 1.0"

def listener():
    rospy.init_node('topic_subscriber')

    sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, callback)

    rospy.spin()

if __name__ == '__main__':
    listener()

我们在终端中运行:

rosrun arm_car_gazebo arm_listener_yolo.py

我们需要观察当小车检测到人的时候,机械臂会不会运动起来。

小车初始状态如下图:

在这里插入图片描述

当检测到人时:

在这里插入图片描述

可以看到,机械臂运动了,当检测到人的时候,机械臂动了。

arm_car_world.launch中添加arm_listener_yolo.py

我们最后一步就是实现全自动,让launch文件来启动arm_listener_yolo.py

代码如下:

<launch>

    <!-- 启动Gazebo  -->
    <include file="$(find arm_car_gazebo)/launch/arm_car_world.launch" />   

    <!-- 启动Gazebo controllers -->
    <include file="$(find arm_car_gazebo)/launch/arm_car_gazebo_controller.launch" />   

    <!-- 启动darknet_ros的yolo检测-->
    <include file="$(find darknet_ros)/launch/darknet_ros.launch">
    </include>

    <!-- 启动监听订阅yolo检测话题的python --><!---->
    <node pkg="arm_car_gazebo" name="arm_listener_yolo" type="arm_listener_yolo.py" output="screen">
    </node>

</launch>

最后运行:

roslaunch arm_car_gazebo arm_car_gazebo_control.launch

当我们添加人,yolo检测到人后,机械臂就会自动运动。

在这里插入图片描述