ROS与AR.Drone-ardrone_tutorials-标签检测和反馈处理

标签检测和反馈处理

说明

本篇介绍如何检测标签并获取反馈内容，并处理。

准备

找到Orange-[Green|Yellow|Blue]-Orange 的标签，可以是贴纸。
编辑launch

$ vim keyboard_controller_with_tags.launch

或

$ vim joystick_controller.launch

更改内容

找到<param name="enemy_colors" value="3" />

enemy_colors的值和标签的对应

1->Orange-Green-Orange
2->Orange-Yellow-Orange
3->Orange-Blue-Orange

标签检测和反馈数据

启动，不起飞

$ roslaunch ardrone_tutorials keyboard_controller_with_tags.launch

或

$ roslaunch ardrone_tutorials joystick_controller.launch

打开新终端，显示所有主题

$ rostopic list

同终端再显示导航数据

$ rostopic echo /ardrone/navdata

显示反馈的数据
因为没起飞，很多数据为0。

在前相机移动Orange-Color-Orange标签，应该被检测，终端显示相应数据

注意tags_xc 和tags_yc值，是标签在图片中的x/y位置
视频显示窗口将在这些坐标显示一个点，并在点上方的距离测量

放置标签到飞行器可见的地方，让飞行器起飞。
在终端观察数据变化，当进行不同飞行动作。

绘制AR.Drone的状态

启动rxplot

$ rxplot /ardrone/navdata/rotX,/ardrone/navdata/rotY

显示飞行器的roll （rotX）和 pitch (rotY) 曲线图
其他图如 yaw (rotZ), x和y velocity (vx and vy), altitude (altd)等

通过绘图方式研究飞行器如何飞行

$ rxplot /cmd_vel/linear/y,/cmd_vel/linear/x

这个绘制 roll (linear/y) and pitch (linear/x) 命令.
有一个最大可控角度值$\pm1$，可通过启动文件的参数euler_angle_max设置。
注意移动的方向：move along the x-axis = /cmd_vel/linear/x = commanded pitch ， tilt around y-axis = /ardrone/navdata/rotY = feedback pitch

记录数据

记录数据，通过rosbag来是实现

$ rosbag record -O ARDroneFlight.bag /ardrone/navdata /cmd_vel

这命令将会记录/ardrone/navdata和/cmd_vel到“ARDroneFlight.bag”,可命其他名字
保持飞行一段时间在按 Ctrl-C 中止
可以记录全部数据，通过-a参数，但是这样会保存视频流，数据包会变得非常大

降落飞行器，关闭所有终端。重开终端

$ roscore

新终端

$ rxplot /cmd_vel/linear/y,/cmd_vel/linear/x

重放

$ rosbag play ARDroneFlight.bag

自定义数据处理

这一节介绍如何写一个节点来处理反馈数据，还可以通过反馈来控制飞行器。ROS通过主题的订阅和发布来通讯。

查看主题信息

rosmsg show ardrone_autonomy/Navdata

内容

$ rosmsg show ardrone_autonomy/Navdata
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 batteryPercent
uint32 state
int32 magX
int32 magY
int32 magZ
int32 pressure
int32 temp
float32 wind_speed
float32 wind_angle
float32 wind_comp_angle
float32 rotX
float32 rotY
float32 rotZ
int32 altd
float32 vx
float32 vy
float32 vz
float32 ax
float32 ay
float32 az
uint32 tags_count
uint32[] tags_type
uint32[] tags_xc
uint32[] tags_yc
uint32[] tags_width
uint32[] tags_height
float32[] tags_orientation
float32[] tags_distance
float32 tm

最小可行的订阅

编写minimum_viable_subscriber.py

$ roscd ardrone_tutorials
$ vim src/minimum_viable_subscriber.py

内容

import roslib
import rospy

roslib.load_manifest('ardrone_tutorials')

from ardrone_autonomy.msg import Navdata

def ReceiveData(data):
  print '[{0:.3f}] Pitch: {1:.3f}'.format(data.header.stamp.to_sec(), data.rotY)

rospy.init_node('minimum_viable_subscriber')
sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)

while not rospy.is_shutdown():
  pass

测试代码

新终端打开

$ roscore

新终端打开

$ roscd ardrone_tutorials
$ python src/minimum_viable_subscriber.py

回放

$ rosbag play ARDroneFlight.bag

反馈，在运行代码的终端可看到，提取了Pitch部分的数据

[1353596639.491] Pitch: 4.375
[1353596639.511] Pitch: 4.128
[1353596639.531] Pitch: 3.750
[1353596639.552] Pitch: 3.304
[1353596639.571] Pitch: 3.063

分析上述订阅代码

代码

import roslib
import rospy

导入ROS包

代码

roslib.load_manifest('ardrone_tutorials')

加载项目的manifest文件，其中包含项目的依赖。

代码

from ardrone_autonomy.msg import Navdata

导入需要的节点

代码

def ReceiveData(data):
  print '[{0:.3f}] Pitch: {1:.3f}'.format(data.header.stamp.to_sec(), data.rotY)

定义回调函数函数处理Navdata信息，并格式化输出Pitch数值

代码

rospy.init_node('minimum_viable_subscriber')

初始化节点

代码

sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)

订阅主题信息，并交给回调函数处理

代码

while not rospy.is_shutdown():
  pass

如果不中止就一直处理信息

发布数据

最小可行的发布

创建 minimum_viable_publisher.py，内容发布到/ardrone/average_pitch主题
文件内容

import roslib
import rospy

roslib.load_manifest('ardrone_tutorials')

from ardrone_autonomy.msg import Navdata
from std_msgs.msg import String

messages = []

def ReceiveData(data):
  messages.append(data)

rospy.init_node('minimum_viable_publisher')
sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)
pub_Average = rospy.Publisher('/ardrone/average_pitch', String)

r = rospy.Rate(1)
while not rospy.is_shutdown():
  if len(messages)>0:
    avg = sum([m.rotY for m in messages])/len(messages)
    messages = []

    avgmsg = String()
    avgmsg.data = 'Average Pitch: {0:.3f}'.format(avg)
    pub_Average.publish(avgmsg)
  r.sleep()

运行

#新终端
$ roscore

#新终端
$ roscd ardrone_tutorials
$ python src/minimum_viable_publisher.py

#新终端
$ rostopic echo /ardrone/average_pitch


#新终端
$ rosbag play ARDroneFlight.bag

#回到rostopic echo终端
data: Average Pitch: 0.649
---
data: Average Pitch: 1.078
---
data: Average Pitch: 3.781
---
data: Average Pitch: 3.285
---

分析代码

代码

from ardrone_autonomy.msg import Navdata
from std_msgs.msg import String

导入需要的库

代码

messages = []
def ReceiveData(data):
  messages.append(data)

定义回调函数，并数据到队列里

代码

pub_Average = rospy.Publisher('/ardrone/average_pitch', String)

发布主题为字符串内容，发布到主题/ardrone/average_pitch

代码

r = rospy.Rate(1)

定义rate函数，控制处理频率，目前控制1秒。

代码

while not rospy.is_shutdown():
  if len(messages)>0:
    avg = sum([m.rotY for m in messages])/len(messages)
    messages = []

    avgmsg = String()
    avgmsg.data = 'Average Pitch: {0:.3f}'.format(avg)
    pub_Average.publish(avgmsg)

如果收到信息，我们将会通过记录的数据计算平均值，转成字符值发布到主题上。

代码

r.sleep()

进行睡眠状态，之后再进入下一个循环在计算。

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号