ROS 学习系列 -- iRobot 第二代机座 Roomba 作为Turtlebot使用时无法开关机


iRobot 推出了第二代机座 Roomba来取代Create. 这是一个绿脸的机座。

如果使用在turtlebot上，几乎是完全兼容的，不用该什么代码，但是波特率提高了一倍，所以需要更改环境变量。但是在停止所以turtlebot的进程后，发现Roomba不再有任何反应，灯灭了，按任何按钮都不管用，也无法充电，但是电池一直在放电。原因是退出进程时没有进入到passive模式。在full模式和safe模式下，它就是这个反应。

1. 启动turtlebot

apt-get install turtlebot之后，执行下面的脚本来设置Roomba为机座：

[cpp]
view plain
copy
print?

export TURTLEBOT_BASE=roomba export TURTLEBOT_STACKS=circles export TURTLEBOT_3D_SENSOR=asus_xtion_pro export TURTLEBOT_SIMULATION=false export TURTLEBOT_SERIAL_PORT=/dev/ttyUSB0 export ROS_IP=192.168.1.16

export TURTLEBOT_BASE=roomba
export TURTLEBOT_STACKS=circles
export TURTLEBOT_3D_SENSOR=asus_xtion_pro
export TURTLEBOT_SIMULATION=false
export TURTLEBOT_SERIAL_PORT=/dev/ttyUSB0
export ROS_IP=192.168.1.16

按下 Roomba中间的最大按钮 “ＣLEＡＮ”, 执行下面的脚本启动turtlebot

[cpp]
view plain
copy
print?

$ roslaunch turtlebot_bringup minimal.launch

$ roslaunch turtlebot_bringup minimal.launch

此时发现，连接Roomba的USB口开始持续狂闪，一直不停。但是“ＣＬＥＡＮ”按钮的灯灭了。这时按CTRＬ＋Ｃ终止，发现USB灯灭了，但是“ＣＬＥＡＮ”按钮没有恢复亮灯。无论怎么操作都不管用，Roomba成了废铁。

2. 修改代码

下载了turtlebot的代码后，打开代码文件 src/turtlebot_create/create_node/nodes/turtlebot_node.py, 增加一下几行代码。为 class TurtlebotNode 添加一个函数：

[python]
view plain
copy
print?

def stop(self): self.robot.passive()

def stop(self):
self.robot.passive()

在 turtlebot_main 函数中添加最后一行 c.stop()：

[python]
view plain
copy
print?

def turtlebot_main(argv): c = TurtlebotNode() while not rospy.is_shutdown(): try: # This sleep throttles reconnecting of the driver. It # appears that pyserial does not properly release the file # descriptor for the USB port in the event that the Create is # unplugged from the laptop. This file desecriptor prevents # the create from reassociating with the same USB port when it # is plugged back in. The solution, for now, is to quickly # exit the driver and let roslaunch respawn the driver until # reconnection occurs. However, it order to not do bad things # to the Create bootloader, and also to keep relaunching at a # minimum, we have a 3-second sleep. time.sleep(3.0) c.start() c.spin() except Exception as ex: msg = "Failed to contact device with error: [%s]. Please check that the Create is powered on and that the connector is plugged into the Create."%(ex) c._diagnostics.node_status(msg,"error") rospy.logerr(msg) finally: # Driver no longer connected, delete flag from disk try: os.remove(connected_file()) except Exception: pass c.stop()

def turtlebot_main(argv):
c = TurtlebotNode()
while not rospy.is_shutdown():
try:
# This sleep throttles reconnecting of the driver.  It
# appears that pyserial does not properly release the file
# descriptor for the USB port in the event that the Create is
# unplugged from the laptop.  This file desecriptor prevents
# the create from reassociating with the same USB port when it
# is plugged back in.  The solution, for now, is to quickly
# exit the driver and let roslaunch respawn the driver until
# reconnection occurs.  However, it order to not do bad things
# to the Create bootloader, and also to keep relaunching at a
# minimum, we have a 3-second sleep.
time.sleep(3.0)

c.start()
c.spin()

except Exception as ex:
msg = "Failed to contact device with error: [%s]. Please check that the Create is powered on and that the connector is plugged into the Create."%(ex)
c._diagnostics.node_status(msg,"error")
rospy.logerr(msg)

finally:
# Driver no longer connected, delete flag from disk
try:
os.remove(connected_file())
except Exception: pass

c.stop()

再次启动turtlebot，然后关闭。Roomba就还可以正常使用了。

ROS 学习系列 -- rocon_app_manager 中 'unegister service [/Cybernetic Pirate/XXX]' 错误处理方法

1. 错误描述

今天在PC和开发板上下载了 rocon_app_manager_tutorials，并尝试在安卓系统上遥控远程ROS进程，执行下面命令启动 rocon app 和 interaction集合：

[cpp]
view plain
copy
print?

roslaunch rocon_app_manager_tutorials pairing.launch --screen

roslaunch rocon_app_manager_tutorials pairing.launch --screen

结果出现了下面的红惨惨的错误:

[plain]
view plain
copy
print?

/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py:709: UserWarning: '/Cybernetic Pirate/platform_info' is not a legal ROS graph resource name. This may cause problems with other ROS tools
error_handler)
[FATAL] [WallTime: 1435499530.814710] unable to register service [/Cybernetic Pirate/platform_info] with master: ERROR: parameter [service] contains illegal chars

/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py:709: UserWarning: '/Cybernetic Pirate/platform_info' is not a legal ROS graph resource name. This may cause problems with other ROS tools
error_handler)
[FATAL] [WallTime: 1435499530.814710] unable to register service [/Cybernetic Pirate/platform_info] with master: ERROR: parameter [service] contains illegal chars

2. 原因分析

这个问题十分诡异，看似许多错误，但仔细一看都是重复的，就一个错误：/Cybernetic Pirate/ＸＸＸＸ 这个service 名字是不合法的。中间的那个空格十分可疑，打开ROS官网命名规范：

[cpp]
view plain
copy
print?

A valid name has the following characteristics:

1. First character is an alpha character ([a-z|A-Z]), tilde (~) or forward slash (/)

2. Subsequent characters can be alphanumeric ([0-9|a-z|A-Z]), underscores (_), or forward slashes (/)

A valid name has the following characteristics:

1. First character is an alpha character ([a-z|A-Z]), tilde (~) or forward slash (/)

2. Subsequent characters can be alphanumeric ([0-9|a-z|A-Z]), underscores (_), or forward slashes (/)

空格没有被标记为允许使用的符号！问题是怎么把它解决掉, 这几个service的名字在哪里？？我们看看执行的 pairing.launch ：

[python]
view plain
copy
print?

<!--
An example that loads up some pairing interactions with a standalone app manager.

The turtle teleop pairing is a bit mucked up, we bypass the relay that gets wierdly
dropped in because we can't configure the args going in to the rapp yet (will be
able to be cleanly do this once we have rapps parsing parameters).
-->
<launch>
<include file="$(find rocon_app_manager)/launch/standalone.launch">
<arg name="interactions" value="true"/>
<arg name="interactions_list" value="[rocon_app_manager_tutorials/pairing]"/>
<arg name="rapp_package_whitelist" value="[rocon_apps, turtle_concert]"/>
<arg name="robot_name" value="Cybernetic Pirate"/>
<arg name="robot_type" value="pc"/>
<arg name="robot_icon" value="rocon_icons/cybernetic_pirate.png"/>
<arg name="robot_description" value="A tutorial environment for demonstrating pairing interactions." />
<arg name="zeroconf" value="true"/>
</include>
<node pkg="turtlesim" type="turtlesim_node" name="turtlesim" required="true"/>
</launch>

<!--
An example that loads up some pairing interactions with a standalone app manager.

The turtle teleop pairing is a bit mucked up, we bypass the relay that gets wierdly
dropped in because we can't configure the args going in to the rapp yet (will be
able to be cleanly do this once we have rapps parsing parameters).
-->
<launch>
<include file="$(find rocon_app_manager)/launch/standalone.launch">
<arg name="interactions"           value="true"/>
<arg name="interactions_list"      value="[rocon_app_manager_tutorials/pairing]"/>
<arg name="rapp_package_whitelist" value="[rocon_apps, turtle_concert]"/>
<arg name="robot_name"             value="Cybernetic Pirate"/>
<arg name="robot_type"             value="pc"/>
<arg name="robot_icon"             value="rocon_icons/cybernetic_pirate.png"/>
<arg name="robot_description"      value="A tutorial environment for demonstrating pairing interactions." />
<arg name="zeroconf"               value="true"/>
</include>
<node pkg="turtlesim" type="turtlesim_node" name="turtlesim" required="true"/>
</launch>

这里的 “robot_name" 作为参数被传递到 rocon_app_manager 包的程序使用了，再没有其它程序的调用了。rocon_app_manager 肯定不会硬编码这个关键字，这里99％就是错误根源了。

3. 解决方案

把 ”Cybernetic Pirate" 改成 "Cybernetic_Pirate", 重新启动pairing.launch， 问题果然解决！

ROS 学习系列 -- image_transport 发布jpg图片文件流在image_view中看视频效果

目标：

目录/home/camera 中保存了2000张照相机连续拍照的JPG文件，名字按数字排序 1.jpg 2.jpg 3.jpg ......... 现在需要在ROS中建立一个node按频率发布所有照片，并可以在image_view中看视频效果。

详细见链接：http://wiki.ros.org/image_transport/Tutorials/PublishingImages

首先安装image_transport,一般在安装ROS时都安装过了。执行

[html]
view plain
copy
print?

sudo apt-get install ros-indigo-image-transport

sudo apt-get install ros-indigo-image-transport

发布node的代码：

[cpp]
view plain
copy
print?

#include <ros/ros.h>
#include <stdlib.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("camera/image", 1);

ros::Rate loop_rate(5);
for(int i = 0; i < 2000; i++)
{
if(!nh.ok())
break;
std::ostringstream stringStream;
stringStream << "/home/camero/" << i << ".jpg";
cv::Mat image = cv::imread(stringStream.str(), CV_LOAD_IMAGE_COLOR);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}

#include <ros/ros.h>
#include <stdlib.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub = it.advertise("camera/image", 1);

ros::Rate loop_rate(5);
for(int i = 0; i < 2000; i++)
{
if(!nh.ok())
break;
std::ostringstream stringStream;
stringStream << "/home/camero/" << i << ".jpg";
cv::Mat image = cv::imread(stringStream.str(), CV_LOAD_IMAGE_COLOR);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}

使用image_view 查看视频流

上面的node发布视频流到 /camera/image 上，但是image_transport同时还发布了其它的topic用来兼容image_view的功能，就是说上面的代码自动帮助发布了有压缩格式的视频流：

[html]
view plain
copy
print?

/camera/image /camera/image/compressed /camera/image/compressed/parameter_descriptions /camera/image/compressed/parameter_updates /camera/image/compressedDepth /camera/image/compressedDepth/parameter_descriptions /camera/image/compressedDepth/parameter_updates /camera/image/theora /camera/image/theora/parameter_descriptions /camera/image/theora/parameter_updates

/camera/image
/camera/image/compressed
/camera/image/compressed/parameter_descriptions
/camera/image/compressed/parameter_updates
/camera/image/compressedDepth
/camera/image/compressedDepth/parameter_descriptions
/camera/image/compressedDepth/parameter_updates
/camera/image/theora
/camera/image/theora/parameter_descriptions
/camera/image/theora/parameter_updates

使用下面的命令查看图片流效果：

[html]
view plain
copy
print?

rosrun image_view image_view image:=/camera/image

rosrun image_view image_view image:=/camera/image

如果要查看压缩视频流效果，需要增加一个参数就可以了：

[html]
view plain
copy
print?

rosrun image_view image_view image:=/camera/image/ image_transport:='compressed'

rosrun image_view image_view image:=/camera/image/ image_transport:='compressed'

ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法

ROS
AMCL 算法根据订阅到的地图数据配合激光扫描特征，使用粒子滤波获取最佳定位点，该点称为Mp (point on map), 它是相对于地图map上的坐标,也就是base_link相对map上的坐标。

odom 的原点是机器人启动时刻的位置，它在map上的位置或转换矩阵是未知的。但是AMCL可以根据最佳粒子的位置推算出 odom->map的tf转换信息并发布到
tf主题上。因为base_link->odom的tf转换信息是每时每刻都在发布的，所以它是已知的。

已知tf转换：

map->base_link

base_link->odom

下面的公式就可以推算:

map->odom = map->base_link - base_link->odom



看一下代码是怎么实现的：

[cpp]
view plain
copy
print?

ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);

// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}

latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;

if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}

ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);

// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}

latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;

if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}

hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp

第10行 tmp_tf = 从map原点看base_link的位置

第14行 tmp_tf.inverse() = 以为Mp为原点，map原点的位置，就是在base_link坐标系下map 原点的位置

第17行 进行 base_link坐标系下的点转换到odom坐标系，也就是把map原点转换到odom坐标系下，等于从odom原点看map原点的位置。放到latest_tf_变量里面。

第37行 发布tf转换关系，latest_tf_.inverse() 得到的是从map原点看odom原点的位置，也就是 odom->map的转换关系

总结

从W的原点看A所在的位置坐标 p，就是A->W转换矩阵，称为WAT. Ap为A坐标系的点， 它在W上的坐标 Wp
= WAT*Ap

卡尔曼滤波算法实例剖析--机器人足球赛场中的定位算法

卡尔曼滤波（Kalman filtering）最早在阿波罗飞船的导航电脑中使用，它对阿波罗计划的轨道预测很有用。
卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。

卡尔曼滤波本身是贝叶斯滤波体系的，建立在马尔科夫概率模型基础上。大多数关于卡尔曼滤波的文章都是大量的数学概率公式，看的晕头转向。本文通过机器人足球场上的定位来通俗易懂的讲述该滤波算法原理。

1. 机器人足球场景介绍

如图1所示，踢球的机器人为差分轮式机器人，它可以直线或者弧线行走，也就是可以同时以线速度v和角速度ω行动。v和ω都是从传感器测量得到的，这里认为是已知的。

如图2，足球场为矩形，内部填充灰色的黑色圆圈代表机器人，它在场内的位置可以表示为(x,y)，左下角为原点; 圆圈的黑色直线代表机器人的角度为 θ，以X轴正向为0度，逆时针旋转。
为了便于定位，四个角落和中间两边分别放置一个ROS的AR标签，这些标签完全不同，编号为 1-6, 位置在系统内已知固定。无论在场内任何地方，机器人使用Kinnect或XTionPRO
均可以探测到至少一个AR标签，ROS可以检测到距离AR标签的距离 r 和角度 φ. 它们也是实时获取的已知量。

很明显，角度θ+φ 是可以从机器人位置(x,y)和AR标签位置(mx,my)同反正切
atan2(mx - x ,my - y)
轻松计算出来。



图1 机器人足球场地实景图



图2 机器人足球场逻辑图

2. 卡尔曼滤波模型

在任何一个时间点t-1, 机器人的位置为(x, y,θ)
, 它以线速度和角速度(vt,ωt)以弧线轨迹移动到下一个时间点t时，新的位置(x',
y', θ')通过简单的三角函数数学公式是可以算出来的：



公式1 坐标转移矩阵

这个过程叫做状态转移，状态x=(x,y) 代表机器人的坐标（状态x 和坐标里面的x只是巧合同一个字母，因为卡尔曼滤波里面的状态就是用x表示的，不要混淆）。

按照公式1不停地累计计算从t 到 t-1，再到t+2,......,t+n，貌似可以得到机器人的实际位置。但是实际效果是什么样子呢？来看一下图3，机器人从左下角开始移动6个时间点，依次经过AR标签2-6, 它的轨迹为实线，白色圆圈代表每个时间点的位置。 而根据公式1的方法计算得到的轨迹为虚线所示，灰色圆圈代表计算机算出的位置。可以清楚的看到，计算误差很大，从一开始的位置计算就有有误差，这种误差不断积累，最终‘失之毫厘，谬以千里’。



图3 移动机器人的计算轨迹（虚线）和实际轨迹（实线）有很大误差

这是为毛呢？原因就是目前所有人类制造的各种仪器都是有误差的。就像GPS定位一样，它也不是能够精确的给出一个坐标，它有个5米到10米的误差。同样机器人传感器得到的线速度v和角速度ω都是不准确的，有噪音。如图4所示，灰色圆圈为公式1计算出来的坐标，但是噪音范围为椭圆形所示，真实位置在里面的任何一点都是有可能的，只是每个点的概率不一样，中间的概率是最高的。



图4 测量误差--椭圆区域

[b]2.1
卡尔曼滤波能做什么[/b]

看到这里会问了，不是还有AR标签帮助测量定位吗？根据标签位置，距离和角度也可以算出坐标啊! 没错，但是还是那个老问题，这个测量结果也是有误差的，误差大小完全取决于传感器。
我们总结一下，现在有两个已知量：

移动的速度和起点坐标 根据公式1算出来的坐标 x = (x,y) 有很大误差噪音
AR标签测量的值 z=(r, φ)
有很大误差噪音

卡尔曼滤波能做什么？它可以根据上面的两个有噪音的测量值融合到一起获得最优的坐标，也就是最优状态。这就是一个降噪的过程，滤波出来的坐标也是有噪音的，但是很小。如图5所示，滤波后的轨迹和真实轨迹非常接近了。灰色椭圆是测量位置的噪音范围，黑色椭圆是卡尔曼滤波后的噪音范围，确实降噪了不少。

这个最优的坐标和噪音到了下一个时刻t+1，就作为滤波的输入结合t+1的AR标签观测量和噪音再次推导一遍。机器人不断行走，该算法不断的循环执行就推导出了最优的移动轨迹。每次间隔Δt越小，滤波的精度就越高。所以计算机性能越好，就可以越精确的估计移动轨迹。



图5 卡尔曼滤波效果图 移动机器人的计算轨迹（虚线）和实际轨迹（实线）非常接近

********接下来开始讲概率矩阵数学了，东方赤龙提醒您：不想看可以不看了*******

[b]2.2
卡尔曼滤波模型参数定义[/b]
卡尔曼滤波是为了获取每个时刻t最优的状态，它有许多物理意义的参数：

状态 x =
坐标 (x,y,θ)
状态噪音Σ = 状态 x 的精确度
最优状态 μ 每个时刻卡尔曼滤波估算的最优坐标
状态转移控制量 u =
线速度和角速度(v, ω)
状态转移噪音 R 因为移动所增加的噪音
观测量 z = AR标签测量值 (r, φ)
观测量噪音 Q =
AR标签传感器的噪音

还有几个状态转移方程:

xt =At xt-1
+Bt ut
根据t-1的坐标和移动速度估计出在t时刻的坐标，也就是上面的公式一。这里At =1，Bt 就是时间间隔
Δt
z =C x
状态量x 到观测量z 的映射关系。这里就是根据下面的三角函数计算出来的， C代表了状态映射矩阵。



[b]2.3
卡尔曼滤波过程[/b]
有了数据，接下来就是具体操作了。科尔曼滤波或者贝叶斯滤波体系的过程都包含两个步骤：

t-1 到 t时刻的状态预测，得到前验概率
根据观察量对预测状态进行修正，得到后验概率，也就是最优值

在卡尔曼滤波里面，每个状态都是一个正态分布概率，也就是高斯分布概率:
p(x) = N( μ, σ )
这里,[b]μ
就是高斯分布的均值，也就是概率最高的值，称为最优值。[b]σ
就是高斯分布的方差，也就是噪音。所以卡尔曼滤波最终算出来的不是一个精确的状态值，而是一个概率最高的最优状态值。这个状态值在我们的例子里面就是坐标。[/b][/b]

[b]2.3.1
状态预测[/b]
根据t-1时刻的最优状态值[b][b]xt-1 [/b][/b]，带入文章开头的状态转移方程公式1，根据控制量u得到t时刻的新状态μ。就是根据机械运动的线速度和角速度推算出新的坐标

。为了便于记忆，我们一般化它为公式一：



公式一 状态转移变换方程

上面已经说了，这个预测是有误差噪音的，在t-1时刻的噪音 Σt-1 经过状态转移后被扩大了，也就是说经过移动后，坐标的不确定性增大了。新的噪音

。为了便于记忆，我们一般化它为公式二：



公式二 状态噪音转移变换方程

[b]2.3.2
卡尔曼增益 K[/b]



公式三 卡尔曼增益K

卡尔曼增益 K 相当于一个系数变换矩阵，它包含了两个信息：

观测量 z 到状态量 x 的的变换矩阵，就是说 x = K z . 相当于2.1节提到的状态转移方程 z = C x 中 C 的逆矩阵。本文例子就相当于从地标AR标签的距离和角度反推出机器人的位置。
对观测量的可信度百分比。观测量可信度越高，也就是测量噪音Q相对于与状态预测噪音

越小，该百分比就越高。反之则越低。

[b]2.3.3
后验值修正[/b]
既然预测不准确，那就用测量后验值去修正它。先修正状态值x，就是公式四：



公式四 状态修正方程
这个方程直观上很容易理解，就是假设预测状态

正确，那它观测到AR标签时的距离和角度对应的值就是

，可视为预测观测值。而

则代表预测观测值与实际观测值的差距。乘上卡尔曼增益K，就得到了修正量。修正量加上预测值就是修正值或最优值。假设

为0，就是说预测AR标签位置和观测到的AR标签位置完全一样，那公式四返回就是预测状态坐标值，和直观常识吻合的非常完美。假设

的值比较大，就是说预测AR标签位置和观测到的AR标签位置差别很大，那根据卡尔曼增益K中的百分比信息的小或大，公式四就返回值靠近预测值或靠近测量值多一些，和实际物理意义吻合的还是非常完美。

有了修正后的状态坐标值，就需要得到该状态坐标的修正后的噪音大小 Σt 就是公式五：



公式五 状态误差噪音修正方程

修正后的噪音Σt
其值要比预测噪音

和感测噪音Q都要小。这样直观上也容易理解，滤波后的状态噪音最小，所以最优。

[b]2.4
卡尔曼滤波总结[/b]
以上公式一二三四五，就是卡尔曼滤波的五项公式。前面说过，在卡尔曼滤波里面，每个状态都是一个正态分布概率，也就是高斯分布概率:

p(x) = N( μ, σ )

整个滤波过程从高斯曲线上看就如下图6所示：



图6 卡尔曼滤波的高斯分布动态过程

a） 初始时刻t-1的状态概率高斯分布，分布图的宽度越宽，误差越大，最优均值概率就越低。
b） t时刻观测到观测量概率高斯分布图，明显观测量噪音低，可信度高。但是两个高斯的均值不一致，就是说观测和预估不一致
c） 卡尔曼滤波后的高斯分布图，该高斯图的噪音比其它两个都小，最优均值概率高于其它两个高斯图，而均值恰好在其它两个高斯均值之间
d） t+1时刻发生的状态转移，状态向右移动，这个过程增加了新的噪音，所以新的状态高斯图噪音增大，最优均值概率下降
e） t+1时刻的观测量高斯分布图
f） t+1时刻应用卡尔曼滤波融合出来噪音低的最优状态高斯图

[b]3
科尔曼滤波的条件和扩展卡尔曼滤波[/b]

本文的例子其实是个扩展卡尔曼滤波。标准的卡尔曼滤波要求2.2小节中的两个状态转移方程为线性方程，也就是要求A、B和C矩阵都为线性矩阵。

xt = At xt-1 +
Bt ut
根据t-1的坐标和移动速度估计出在t时刻的坐标，也就是上面的公式一。这里At =1，Bt 就是时间间隔
Δt
z = C x 状态量x 到观测量z 的映射关系。这里就是根据下面的三角函数计算出来的， C代表了状态映射矩阵。

本文的例子明显不符合这个要求，因为涉及到了三角函数等计算。标准卡尔曼滤波在现实环境中是很难找到的，大部分情况下都是非线性状态转移。而扩展卡尔曼滤波就是应用在非线性状态转移的环境中的。它的算法和标准算法一样，只是把上面的A、B和C矩阵线性化，也就是用一个线性方程最大程度的逼近非线性方程。常用的方法有UT变换( Unscented Transformation) 和UKF变换

ROS 学习系列 -- Roomba, Xtion Pro live 实现360度全景照片panorama 无法启动的解决方案

turtlebot 有个实现iPhone360全景照相功能的应用
panorama. 官方使用Create底座和Kinnect, 在使用Roomba底座和Xtion Pro Live配套时发现，按照教程的方式启动不了。



1. 启动

[cpp]
view plain
copy
print?

roslaunch turtlebot_bringup minimal.launch \\加载轮子驱动 <pre>roslaunch turtlebot_panorama panorama.launch \\启动 panorama

roslaunch turtlebot_bringup minimal.launch       \\加载轮子驱动
<pre>roslaunch turtlebot_panorama panorama.launch     \\启动 panorama

打开另一个shell窗口

[cpp]
view plain
copy
print?

rosservice call turtlebot_panorama/take_pano 0 360.0 30.0 0.3 \\下令照相转圈

rosservice call turtlebot_panorama/take_pano 0 360.0 30.0 0.3   \\下令照相转圈

这个时候发现，Roomba 驱动的turtlebot根本没有任何反应,接下来一步剖析问题。

2. 问题分析

打开源代码panorama.cpp, 看到有大量的 log("...") 输出调试信息:

[cpp]
view plain
copy
print?

//*************
// Logging
//*************
void PanoApp::log(std::string log)
{
std_msgs::String msg;
msg.data = log;
pub_log.publish(msg);
ROS_INFO_STREAM(log);
}

PanoApp::PanoApp() : nh(), priv_nh("~")
{
std::string name = ros::this_node::getName();

ros::param::param<int>("~default_mode", default_mode, 1);
ros::param::param<double>("~default_pano_angle", default_pano_angle, (2 * M_PI));
ros::param::param<double>("~default_snap_interval", default_snap_interval, 2.0);
ros::param::param<double>("~default_rotation_velocity", default_rotation_velocity, 0.3);

ros::param::param<std::string>("~camera_name", params["camera_name"], "/camera/rgb");
ros::param::param<std::string>("~bag_location", params["bag_location"], "/home/turtlebot/pano.bag");

pub_log = priv_nh.advertise<std_msgs::String>("log", 100);
}

//*************
// Logging
//*************
void PanoApp::log(std::string log)
{
std_msgs::String msg;
msg.data = log;
pub_log.publish(msg);
ROS_INFO_STREAM(log);
}

PanoApp::PanoApp() : nh(), priv_nh("~")
{
  std::string name = ros::this_node::getName();

  ros::param::param<int>("~default_mode", default_mode, 1);
  ros::param::param<double>("~default_pano_angle", default_pano_angle, (2 * M_PI));
  ros::param::param<double>("~default_snap_interval", default_snap_interval, 2.0);
  ros::param::param<double>("~default_rotation_velocity", default_rotation_velocity, 0.3);

  ros::param::param<std::string>("~camera_name", params["camera_name"], "/camera/rgb");
  ros::param::param<std::string>("~bag_location", params["bag_location"], "/home/turtlebot/pano.bag");

  pub_log = priv_nh.advertise<std_msgs::String>("log", 100);
}

从这里可以看出所有的信息都被发送到了 turtlebot\log 这个topic里面了，那就监视一些这个topic：

[cpp]
view plain
copy
print?

rostopic echo turtlebot\log

rostopic  echo turtlebot\log

再执行一遍启动命令:

[cpp]
view plain
copy
print?

rosservice call turtlebot_panorama/take_pano 0 360.0 30.0 0.3 \\下令照相转圈

rosservice call turtlebot_panorama/take_pano 0 360.0 30.0 0.3   \\下令照相转圈

这时候可以看到turtlebot\log 输出的日志：

[cpp]
view plain
copy
print?

［well time xxxxx]: Starting panorama creation. ［well time xxxxx]: Pano ROS action goal sent. <pre name="code" class="cpp">［well time xxxxx]: Pano action goal just went active.

［well time xxxxx]: Starting panorama creation.
［well time xxxxx]: Pano ROS action goal sent.
<pre name="code" class="cpp">［well time xxxxx]: Pano action goal just went active.

[cpp]
view plain
copy
print?

<pre name="code" class="cpp">［well time xxxxx]: snap

<pre name="code" class="cpp">［well time xxxxx]: snap

很明显，事情卡在了snap拍快照上面了，我们来看看这块代码：

[cpp]
view plain
copy
print?

void PanoApp::snap()
{
log("snap");
pub_action_snap.publish(empty_msg);
}

void PanoApp::init()
{
......................

//***************************
// pano_ros API
//***************************
pano_ros_client = new actionlib::SimpleActionClient<pano_ros::PanoCaptureAction>("pano_server", true);
log("Waiting for Pano ROS server ...");
pano_ros_client->waitForServer(); // will wait for infinite time
log("Connected to Pano ROS server.");
pub_action_snap = nh.advertise<std_msgs::Empty>("pano_server/snap", 100);
pub_action_stop = nh.advertise<std_msgs::Empty>("pano_server/stop", 100);
image_transport::ImageTransport it_pano(nh);
sub_stitched = it_pano.subscribe("pano_server/stitch", 1, &PanoApp::stitchedImageCb, this);

..............................
}

void PanoApp::snap()
{
log("snap");
pub_action_snap.publish(empty_msg);
}

void PanoApp::init()
{
  ......................

  //***************************
  // pano_ros API
  //***************************
  pano_ros_client = new actionlib::SimpleActionClient<pano_ros::PanoCaptureAction>("pano_server", true);
  log("Waiting for Pano ROS server ...");
  pano_ros_client->waitForServer(); // will wait for infinite time
  log("Connected to Pano ROS server.");
  pub_action_snap = nh.advertise<std_msgs::Empty>("pano_server/snap", 100);
  pub_action_stop = nh.advertise<std_msgs::Empty>("pano_server/stop", 100);
  image_transport::ImageTransport it_pano(nh);
  sub_stitched = it_pano.subscribe("pano_server/stitch", 1, &PanoApp::stitchedImageCb, this);

  ..............................
}

抓图的功能放到了另外一个进程。"pano_server/snap" 的实现在 turtlebot_apps/software/pano/pano_ros/nodes/capture_server.py 里面：

[python]
view plain
copy
print?

def pano_capture(self, goal):
if self._capture_job is not None:
raise Exception("cannot run multile capture jobs. TODO: pre-eempt existing job")

rospy.loginfo('%s: Pano Capture Goal: \n\tCamera [%s]'%(self._action_name, goal.camera_topic))

pano_id = int(rospy.get_time())
#TODO make this a parameter, bag, publisher, etc...
capturer = self._capture_interface()#= BagCapture(pano_id,goal.bag_filename or None)
capturer.start(pano_id,goal)

self._snap_requested = False #reset
capture_job = self._capture_job = PanoCaptureJob(pano_id, capturer )
camera_topic = goal.camera_topic or rospy.resolve_name('camera')
#TODO: FIX ONCE OPENNI API IS FIXED
image_topic = rospy.names.ns_join(camera_topic, 'image_color')
camera_info_topic = rospy.names.ns_join(camera_topic, 'camera_info')

rospy.loginfo('%s: Starting capture of pano_id %d.\n\tImage [%s]\n\tCamera Info[%s]'
%(self._action_name, pano_id, image_topic, camera_info_topic) )
grabber = ImageGrabber(image_topic, camera_info_topic, self.capture_fn)

# local vars
server = self._server
preempted = False

rospy.loginfo('%s: Executing capture of pano_id %d' % (self._action_name, pano_id))
# this will become true
while capture_job.result is None and not preempted and not rospy.is_shutdown():
if server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
server.set_preempted()
capture_job.cancel()
preempted = True
else:
rospy.sleep(0.001) #let the node rest a bit

result = capture_job.result
grabber.stop()

if result:
rospy.loginfo('%s: Succeeded, [%s] images.\nResult: %s' % (self._action_name, result.n_captures, result))
server.set_succeeded(result)

self._capture_job = None

def pano_capture(self, goal):
if self._capture_job is not None:
raise Exception("cannot run multile capture jobs. TODO: pre-eempt existing job")

rospy.loginfo('%s: Pano Capture Goal: \n\tCamera [%s]'%(self._action_name, goal.camera_topic))

pano_id = int(rospy.get_time())
#TODO make this a parameter, bag, publisher, etc...
capturer = self._capture_interface()#= BagCapture(pano_id,goal.bag_filename or None)
capturer.start(pano_id,goal)

self._snap_requested = False #reset
capture_job = self._capture_job = PanoCaptureJob(pano_id, capturer )
camera_topic = goal.camera_topic or rospy.resolve_name('camera')
#TODO: FIX ONCE OPENNI API IS FIXED
image_topic = rospy.names.ns_join(camera_topic, 'image_color')
camera_info_topic = rospy.names.ns_join(camera_topic, 'camera_info')

rospy.loginfo('%s: Starting capture of pano_id %d.\n\tImage [%s]\n\tCamera Info[%s]'
%(self._action_name, pano_id, image_topic, camera_info_topic) )
grabber = ImageGrabber(image_topic, camera_info_topic, self.capture_fn)

# local vars
server = self._server
preempted = False

rospy.loginfo('%s: Executing capture of pano_id %d' % (self._action_name, pano_id))
# this will become true
while capture_job.result is None and not preempted and not rospy.is_shutdown():
if server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
server.set_preempted()
capture_job.cancel()
preempted = True
else:
rospy.sleep(0.001) #let the node rest a bit

result = capture_job.result
grabber.stop()

if result:
rospy.loginfo('%s: Succeeded, [%s] images.\nResult: %s' % (self._action_name, result.n_captures, result))
server.set_succeeded(result)

self._capture_job = None

第16, 17 行 显示 同时监听等待 “image_color” “camera_info”两个topic, 它们都是OPenNI2发布的图像数据，看ImageGrabber的代码发现，里面就是在同步等待两个 topic同时有数据时才触发拍照操作( message_filters.TimeSynchronizer() ）

这个时候我们查看一下这两个主题是否有图像和信息：

[cpp]
view plain
copy
print?

$ rosrun image_view image_view image:=/camera/rgb/image_color $ rostopic echo /camera/rgb/camera_info

$ rosrun image_view image_view image:=/camera/rgb/image_color

$ rostopic echo /camera/rgb/camera_info

可以看到在image_view里面是没有图像的，因此可以判断是图像的主题在kinnect和XtionPro的不一致造成的.

真正的图像主题在这里:

[cpp]
view plain
copy
print?

$ rosrun image_view image_view image:=/camera/rgb/image_raw

$ rosrun image_view image_view image:=/camera/rgb/image_raw

可以看到在image_view里面是有图像的。

结论

Xtion Pro Live 的图像数据被发布在 /camera/rgb/image_raw， 但是turtlebot_apps/software/pano/pano_ros/nodes/capture_server.py 里面一直在监听/camera/rgb/image_color主题，导致一直没有图像出现。

解决方案

remap topic 映射主题, 修改turtlebot_panorama/launch/panorama.launch。 如下，在第15行增加了remap, 映射两个主题，将程序指向/camera/rgb/image_raw

[cpp]
view plain
copy
print?

<launch>
<!-- 3d sensor; we just need RGB images -->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
<arg name="rgb_processing" value="true"/>
<arg name="ir_processing" value="false"/>
<arg name="depth_processing" value="false"/>
<arg name="depth_registered_processing" value="false"/>
<arg name="depth_registration" value="false"/>
<arg name="disparity_processing" value="false"/>
<arg name="disparity_registered_processing" value="false"/>
<arg name="scan_processing" value="false"/>
</include>

<node name="pano_server" pkg="pano_ros" type="capture_server.py" output="screen">
<remap from="camera/rgb/image_color" to="camera/rgb/image_raw"/>
</node>

<node name="turtlebot_panorama" pkg="turtlebot_panorama" type="panorama" output="screen">
<param name="default_mode" value="1"/>
<param name="default_pano_angle" value="6.28318530718"/> <!-- 2 * Pi -->
<param name="default_snap_interval" value="2.0"/>
<param name="default_rotation_velocity" value="0.3"/>
<param name="camera_name" value="camera/rgb"/>
<param name="bag_location" value="/tmp/turtlebot_panorama.bag"/>
<remap from="cmd_vel" to="/cmd_vel_mux/input/navi"/>
<remap from="odom" to="/odom"/>
</node>
</launch>

关闭

ROS学习 —— qt_gui_main() found no plugin matching ‘xxx' 解决方法

在使用
rocon_app_manager 控制 ROS进程时，打开某个QT的控制程序，出现以下错误：

[cpp]
view plain
copy
print?

qt_gui_main() found no plugin matching "rocon_qt_teleop"

qt_gui_main() found no plugin matching "rocon_qt_teleop"

rocon_qt_teleop 已经安装完成，问题出在 qt 的缓存没有更新安装插件。解决办法：

[cpp]
view plain
copy
print?

$ rm ~/.config/ros.org/rqt_gui.ini $ rqt

$ rm ~/.config/ros.org/rqt_gui.ini
$ rqt