按类别存档： 机器人

一、camera的驱动
二、在linux种使用uvc驱动
三、在ros中使用uvc驱动
四、在ros中使用usb驱动

• 一、camera的驱动

1、UVC
第一个概念是UVC：USB Video Device Class，大部分usb接口（包括主办集成）都兼容uvc，uvc的实现独立于linux/weindows等系统。
2、V4L
第二个概念是V4L或者V4L2：Video4Linux，它提供了一系列的API，新版本是2，V4L2。
3、usb_cam
ROS的usb_cam这个package实现了V4L，提供的node是usb_cam_node，发布的topic是<camera_name>/image，msg类型是sensor_msgs/Image ，参数主要有video_device，camera_name ，pixel_format ，camera_frame_id （The camera’s tf frame ），brightness。
4、uvc_camera
ROS的uvc_camera这个package实现了uvc视频流的用法，还支持两个相机组成的双目视觉，提供的node是UVC/USB camera，发布的topic主要有image_raw这个视频流，参数主要有device，fps，frame_id。该package已经不再维护，被libuvc_camera替代。
5、libuvc_camera
这个要求用户具有/dev/bus/usb/具体相机的写权限，所以最好：
$ sudo -E rosrun libuvc_camera camera_node vendor:=具体厂家
6、cv_camera
这个package是ROS提供的openCV驱动。

• 二、在linux种使用uvc驱动

1、查看ID
$ lsusb
–Bus 001 Device 003: ID 1578:0076
–Bus 001 Device 004: ID 0547:4d35 Anchor Chips, Inc.
–Bus 001 Device 005: ID 04f2:b2ea Chicony Electronics Co., Ltd Integrated Camera [ThinkPad]
如果开发过usb设备的驱动程序，就很清楚04f2:b2ea是description里面的vendor和product，最常见的罗技c270是046d:0825，可以通过google这个ID，确定是否在uvc的支持列表。也可以在本机上运行：
$ lsusb -d 04f2:b2ea -v | grep “14 Video”
bFunctionClass         14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
bInterfaceClass        14 Video
这些输出信息说明兼容UVC，否则就是non-UVC camera。
2、看下设备
$ ls /dev | grep video
–video0
这说明有了硬件。
3、再看
$ lsmod | grep uvcvideo
uvcvideo               72275  0
videobuf2_core         39510  1 uvcvideo
videodev              108952  2 uvcvideo,videobuf2_core
videobuf2_vmalloc      13048  1 uvcvideo
4、如果没出现可以显式的加载模块
$ sudo modprobe uvcvideo
5、验证
$ lshal | grep video
system.hardware.primary_video.product = 1029  (0x405)  (int)
system.hardware.primary_video.vendor = 5549  (0x15ad)  (int)
info.linux.driver = ‘uvcvideo’  (string)
info.linux.driver = ‘uvcvideo’  (string)
info.capabilities = {‘video4linux’, ‘video4linux.video_capture’} (string list)
info.category = ‘video4linux’  (string)
info.subsystem = ‘video4linux’  (string)
info.udi = ‘/org/freedesktop/Hal/devices/usb_device_4f2_b2ea_noserial_if0_video4linux’  (string)
linux.device_file = ‘/dev/video0’  (string)
linux.subsystem = ‘video4linux’  (string)
linux.sysfs_path = ‘/sys/devices/pci0000:00/0000:00:11.0/0000:02:03.0/usb1/1-2/1-2:1.0/video4linux/video0’  (string)
video4linux.device = ‘/dev/video0’  (string)
video4linux.version = ‘2’  (string)
$ lshal | grep Cam
info.product = ‘Integrated Camera [ThinkPad]’  (string)
usb_device.product = ‘Integrated Camera [ThinkPad]’  (string)
usb.interface.description = ‘Integrated Camera’  (string)
info.product = ‘Integrated Camera’  (string)
info.product = ‘Integrated Camera’  (string)
input.product = ‘Integrated Camera’  (string)
info.product = ‘5.0M USB2.0 Camera’  (string)
usb_device.product = ‘5.0M USB2.0 Camera’  (string)
6、说明ok的
$ dmesg | grep video
[  409.572621] Linux video capture interface: v2.00
[  409.663484] usbcore: registered new interface driver uvcvideo
[ 4822.404813] uvcvideo: Found UVC 1.00 device Integrated Camera (04f2:b2ea)
$ dmesg | grep Cam
[  168.027246] usb 1-1: Product: USB2-Camera
[  267.368271] usb 1-1: Product: USB2-Camera
[ 3700.024321] usb 1-1: Product: 5.0M USB2.0 Camera
[ 4822.259972] usb 1-2: Product: Integrated Camera
[ 4822.404813] uvcvideo: Found UVC 1.00 device Integrated Camera (04f2:b2ea)
[ 4822.530930] input: Integrated Camera as /devices/pci0000:00/0000:00:11.0/0000:02:03.0/usb1/1-2/1-2:1.0/input/input5
$ dmesg|tail -n20
7、开启视频
$ cheese –device=/dev/video0
这里用cheese查看视频。

• 三、在ros中使用uvc驱动

1、安装uvc_cam
$ cd ~/catkin_ws
$ git clone https://github.com/ericperko/uvc_cam.git
$ cd ..
$ rosmake uvc_cam
这个，[uvc_cam] 的package位于用户的catkin_ws下面，所以要保证ROS_PACKAGE_PATH包含这个位置，可以roscd到uvc_cam。
当然，安装ros提供的也是可以的：
$ sudo apt-get install ros-hydro-uvc-camera
这样的话，[uvc_camera] 这个package就位于/opt的share下面了，关键的还是没法测试后续的rbx1的例程。
2、驱动测试
运行测试脚本
$ roslaunch  uvc_cam test_uvc.launch
查看视频
$ rosrun image_view image_view image:=/camera/image_raw
ok
3、进一步测试
3.1、借用下rbx1现成的launch脚本来运行[uvc_cam]驱动，（不是[uvc_camera]）
$ roslaunch rbx1_vision  uvc_cam.launch  device:=/dev/video0
此时node增加了一个/uvc_cam_node，topic增加了许多。
3.2、或者使用[uvc_camera]而不用[uvc_cam]：
$ roslaunch rbx1_vision  uvc_camera.launch  device:=/dev/video0
同样的，node增加了一个/uvc_camera_node，topic增加了许多。
3.3、按照例程，后面还是使用 [uvc_cam] 。
3.4、查看视频：
利用vision_opencv这个package里面的image_view
$ rosrun image_view image_view image:=/camera/rgb/image_color
或者rqt里面plugins->imageview->选择/camera/rgb/image_color
$ rqt
ok

• 四、在ros中使用usb驱动

1、安装usb_cam
$ cd ~/catkin_ws/src
$ git clone https://github.com/bosch-ros-pkg/usb_cam.git
$ cd ~/catkin_ws
$ catkin_make
或者：
$ cd ~
$ rosmake usb_cam
2、驱动测试
运行测试脚本
$ roslaunch usb_cam usb_cam-test.launch
在usb_cam的launch下面的usb_cam-test.launch这个脚本，把pixel_format设成了yuyv，有可能需要修改成mjpeg，然后保存。
也可以外部设置：
$ rosparam set /usb_cam/pixel_format yuyv
查看视频：
$ rosrun image_view image_view image:=/？？？？？
3、为了进一步测试，写一个usb_cam.launch
$ cd ~/catkin_ws/src/rbx1/rbx1_vision/launch
仿照uvc_cam.launch的格式：
<launch>
<arg name=”device” default=”/dev/video0″ />
<node name=”uvc_cam_node” pkg=”uvc_cam” type=”uvc_cam_node” output=”screen”>
<remap from=”camera/image_raw” to=”camera/rgb/image_color” />
<param name=”device” value=”$(arg device)” />
<param name=”width” value=”320″ />
<param name=”height” value=”240″ />
<param name=”frame_rate” value=”20″ />
<param name=”exposure” value=”0″ />
<param name=”gain” value=”100″ />
</node>
</launch>
写成usb_cam.launch的脚本：
$ nano usb_cam.launch
<launch>
<arg name=”device” default=”/dev/video0″ />
<node name=”usb_cam_node” pkg=”usb_cam” type=”usb_cam_node” output=”screen”>
<remap from=”usb_cam_node/image_raw” to=”camera/rgb/image_color” />
<param name=”video_device” value=”$(arg device)” />
<param name=”image_width” value=”320″ />
<param name=”image_height” value=”240″ />
<param name=”framerate” value=”20″ />
<param name=”brightness” value=”128″ />
<param name=”contrast” value=”128″ />
<param name=”saturation ” value=”70″ />
</node>
</launch>
这里面为了使usb_cam和uvc_cam一样remap到camera/rgb/image_color，如果需要的话也加上修改图像格式这个：
<param name=”pixel_format ” value=”yuyv” />
4、进一步测试
运行驱动
$ roslaunch rbx1_vision  usb_cam.launch  device:=/dev/video0
查看视频
$ rosrun image_view image_view image:=/camera/rgb/image_color

1. 硬件
2. 软件
3. 软件 – ROS
3.1 arduino uno 部分
3.2 pc ros 部分
3.3 usage

MPU 6050 is a sensor based on MEMS technology with I2C protocol communication.
The accelerometer works on the principle of the piezoelectric effect.
The gyroscopes work on the principle of Coriolis acceleration.
REF： http://blog.csdn.net/eaibot/article/details/51398074/

1. 硬件

Arduino-UNO + MPU-6050

XCL，XDA:这2个pin是MPU6050做I2C的master时用来控制slave的。不接。
SDA，SCL：这2个pin是I2C接口。 接UNO的pin4(SDA)和pin5(SCL)。
AD0: 用来选定I2C地址的，接地时地址是0x68，接3.3V时是0x69. 接地。
INT：打断信号，MPU6050内建1024byte的FIFO buffer，当buffer有数据时可以interrupt通知arduino。接UNO的pin-2 (interrupt pin 0)
共享代码中都设置400KHz的I2C bus speed，100K应该也可。

2. 软件

下载：I2Cdev library： http://diyhacking.com/projects/I2Cdev.zip
下载：MPU 6050： http://diyhacking.com/projects/MPU6050.zip
两个库解压，复制到Arduino的libraries目录
File –> Examples –> MPU6050 –> Examples –> MPU6050_DMP6
编译，上传，打开串口监控设置波特率115200：
Send any character to begin DMP programming and demo:
ypr 28.37 -30.82 10.95
ypr 28.46 -30.82 10.95
ypr 28.56 -30.82 10.96
ypr 28.66 -30.82 10.96
…

注：
最初数据不准确。
注：
这里不是raw data，是DMP方式。
DMP即Digital Motion Processing，a built-in motion processor. It processes the values from the accelerometer and gyroscope to give accurate 3D values.

3. 软件 – ROS

REF： https://github.com/fsteinhardt/mpu6050_serial_to_imu/blob/master/README.md

3.1 arduino uno 部分

复制前面的ino代码，创建新项目。
修改部分：

*1 comment define OUTPUT_READABLE_YAWPITCHROLL：
#define OUTPUT_READABLE_YAWPITCHROLL ->
//#define OUTPUT_READABLE_YAWPITCHROLL
*2 uncomment OUTPUT_TEAPOT:
//#define OUTPUT_TEAPOT ->
#define OUTPUT_TEAPOT
*3 change code:
//uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘r’, ‘n’ };
———–>>>>>>>>>>>>>>>
uint8_t teapotPacket[28] = { ‘$’, 0x03, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘r’, ‘n’ };
*4 change code:
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
————>>>>>>>>>>>>>>>>>>>>>>>>
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
// gyro values
teapotPacket[10] = fifoBuffer[16];
teapotPacket[11] = fifoBuffer[17];
teapotPacket[12] = fifoBuffer[20];
teapotPacket[13] = fifoBuffer[21];
teapotPacket[14] = fifoBuffer[24];
teapotPacket[15] = fifoBuffer[25];
// accelerometer values
teapotPacket[16] = fifoBuffer[28];
teapotPacket[17] = fifoBuffer[29];
teapotPacket[18] = fifoBuffer[32];
teapotPacket[19] = fifoBuffer[33];
teapotPacket[20] = fifoBuffer[36];
teapotPacket[21] = fifoBuffer[37];
//temperature
int16_t temperature = mpu.getTemperature();
teapotPacket[22] = temperature >> 8;
teapotPacket[23] = temperature & 0xFF;
Serial.write(teapotPacket, 28);
teapotPacket[25]++; // packetCount, loops at 0xFF on purpose
编译，上传到arduino uno。

或者，
直接下载代码： https://github.com/fsteinhardt/mpu6050_serial_to_imu

解压， 对MPU6050.ino项目用Arduino IDE打开。
不过arduino带的wire版本和这里代码不兼容， wire的好几个函数改名了。
error: ‘class TwoWire’ has no member named ‘setClock’
需要修改两处：
// Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
和
// enable Arduino interrupt detection
Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));
//attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
attachInterrupt(0, dmpDataReady, RISING);
后面这个版本时间长了串口不稳定。

3.2 pc ros 部分

复制解压后的剩余代码到~/atkin_ws/src，
或者：
$ cd ~/catkin_ws/src/
$ git clone https://github.com/fsteinhardt/mpu6050_serial_to_imu
$ cd ~/catkin_ws/
$ catkin_make

如果需要：
$ sudo apt-get install ros-indigo-serial

Serial is a cross-platform, simple to use library for using serial ports on computers. This library provides a C++, object oriented interface for interacting with RS-232 like devices on Linux and Windows.
Not like ros-indigo-rosserial.

$ source ~/.bashrc
$ roslaunch mpu6050_serial_to_imu demo.launch
$ rostopic echo /imu_data

可以看到mpu6050的值转换为ROS的/imu 的消息：
header:
seq: 36872
stamp:
secs: 1508381861
nsecs: 177936189
frame_id: imu_link
orientation:
x: -0.0668025501072
y: -0.0957503132522
z: -0.175129953772
w: 0.977523017675
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0
y: -0.0266316109008
z: -0.0266316109008
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.185614013672
y: 2.42735229492
z: 11.7942736816
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Published Topics
* imu/data ([sensor_msgs::Imu])
The measured accelerometer, gyro and orientation values.
* imu/temperature ([sensor_msgs::Temperature])
The measured temperature in degrees Celsius.

Services:
* set_zero_orientation ([std_srvs/Empty])
This service sets the current orientation as the new zero orientation
so that from now on only the difference to this orientation is sent.

TF Published:
The resulting orientation can be published as a tf transform,
the frame names can be set using parameters.

Parameters:
* broadcast_tf (bool, default: true)
If true: publish the orientation of the IMU as a tf transform.
* tf_parent_frame_id (string, default: “imu_base”)
Sets the name of the parent frame in the tf transform.
… WE CHANGE TO base_link …
* tf_frame_id (string, default: “imu_link”)
Sets the name of the own frame in the tf transform.
* imu_frame_id (string, default: “imu_link”)
Sets the name of the frame for imu messages.

3.3 usage

Needed a tf:

Sample Launch:

4. 校准和标定

Put MPU6050, as horizontal as possible
– Write down offsets for that particular MPU6050 and use them with library’s functions “…setoffset…” in projects.

四元数到角度 ： Node发布的是四元数、角速度、线加速度， 如果直观看左转右转的角度：

#! /usr/bin/python

import PyKDL
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from math import *
import threading
import os
import subprocess
import yaml

def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res > pi:
res -= 2.0*pi
while res < -pi: res += 2.0*pi return res class CalibrateRobot: def __init__(self): self.lock = threading.Lock() self.sub_imu = rospy.Subscriber('/imu/data', Imu, self.imu_cb) self.last_imu_angle = 0 self.imu_angle = 0 while not rospy.is_shutdown(): rospy.sleep(0.1) rospy.loginfo("imu_angle:"+str(self.imu_angle*180/3.1415926)) def imu_cb(self, msg): with self.lock: angle = quat_to_angle(msg.orientation) self.imu_angle = angle self.imu_time = msg.header.stamp def main(): rospy.init_node('scan_to_angle') CalibrateRobot() if __name__ == '__main__': main() --- [INFO] [WallTime: 1508384569.829275] imu_angle:-12.3451541995 [INFO] [WallTime: 1508384569.930287] imu_angle:-12.3600526937 [INFO] [WallTime: 1508384570.031151] imu_angle:-12.3667251945 [INFO] [WallTime: 1508384570.132128] imu_angle:-12.3829488555 可见即使位置不变也是存在漂移的，这个偏移可以加回去。