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

目录总览

小车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检测到人后，机械臂就会自动运动。