http://blog.csdn.net/crazyquhezheng/article/details/47065811Roomba, Xtion Pro机器人制作地图在Android手机无法实时观测地图
Roomba, Xtion Pro live 实现360度全景照片panorama 无法启动的解决方案
turtlebot 有个实现iPhone360全景照相功能的应用 panorama. 官方使用Create底座和Kinnect, 在使用Roomba底座和Xtion Pro Live配套时发现，按照教程的方式启动不了。
1. 启动
- 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窗口
- 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("...") 输出调试信息:
- //*************
- // 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);
}
- rostopic echo turtlebot\log
rostopic echo turtlebot\log 再执行一遍启动命令:
- 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 输出的日志：
- ［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.- <pre name="code" class="cpp">［well time xxxxx]: snap
<pre name="code" class="cpp">［well time xxxxx]: snap
- 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);
..............................
}
- 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() ）
这个时候我们查看一下这两个主题是否有图像和信息：
- $ 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的不一致造成的.
真正的图像主题在这里:
- $ rosrun image_view image_view image:=/camera/rgb/image_raw
$ rosrun image_view image_view image:=/camera/rgb/image_raw
结论
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
- <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>
turtlebot 配合Android应用Make a Map可以允许用户通过手机操纵机器人扫描室内地图并在手机上显示出来,效果图如下：
ROS机器人与安卓设备的通讯协作是通过rocon实现的，细节这里不作描述。本文适合已经对该技术非常熟悉的人阅读。
1. 问题描述
下载安卓rocon remocon和Ｍake a map并安装，在turtlebot上启动机
该命令可以启动turtlebot的轮子驱动并加在Android interaction 应用rapps. 该命令可以成功启动的前提条件是正确配置的硬件环境变量，把roomba和XTion pro Live指定给ROS.
接下来在安卓终端打开Rocon remocon应用，添加一个master URI,把turtlebot的IP输入，这时就可以看到该节点信息了，点击进入后选择角色”Android pair”. 就可以看到大量的应用，启动”Make a map”就可以看到操作界面了，摇控行走可以看到室内图片信息显示出来了，但是地图上一片空白。
2. 问题分析
首先查看地图数据到底生产没有，地图应该是由 /map 主题发布出来的，这时候我们看一些该主题：
上来就看到warning: No map received, 点击topic下拉框，看到/turtlebot/map被自动列出来了，选中后就看到地图确实正确显示出来了。
3. 问题确认
安卓的Make a map到底订阅了那个主题？我们可以通过rqt_graph来查看究竟：
我们可以看出来，android那边只使用了一个主题”/teleop/compressed_image”,也就是图像数据，没有map数据被订阅的痕迹，看来就是这个”/turtlebot”前缀搞的鬼了。
怀疑是rocon通过 .rapp启动一个.launch文件的时候，这个缀搞被自动加上了，试一下手工启动. 这需要禁止ros在点击安卓应用后启动.launch文件。
这个时候在安卓终端打开rocon remocon启动Map a map,地图出来了，房间图像不见了!
再次使用rqt_graph,可以看到 /map被Android节点成功订阅到了，而且不止一个主题，还有/scan
4. 结论
安卓应用Make a map 需要订阅的主题为：
图像数据：/teleop/compressed_image
地图数据：/map
激光扫描: /scan
而通过remocon启动程序后真正的数据主题为：
图像数据：/teleop/compressed_image
地图数据：/turtlebot/map
激光扫描: /turtlebot/scan
5. 解决方案
需要完成下面的主题映射:
/turtlebot/map –> /map
/turtlebot/scan –> /scan
在 turtlebot_rapp 的make_a_map.launch 文件里面添加映射命令. 先找到该文件nano编辑
注意这里的参数千万不要带前缀’turtlebot’,而是使用相对路径’map’ ‘scan’ ，而映射目标主题要写明绝对路径’/map’ ‘/sacn’.
因为这个launch文件就被放到了’turtlebot’命名空间里面了，相对路径会被自动补全－－前面加上命名空间
6. 后记－－’turtlebot’命名空间哪里来的？
ROS默认就有一个环境变量 TURTLEBOTNAME＝turtlebot.该环境变量被经由minimal.launch−−>standalone.launch−−> (find rocon_app_manager)/launch/includes/_app_manager.xml 一直传递进 rapp_manager.py 里面。该程序负责启动romocon对应的rapp launch文件,查看函数_process_start_app(self,
每次启动的所有程序都被放到了命名空间里面，这里的self._application_namespace就是robot_name参数的值。
rospy.loginfo()的内容可以通过查看 /rosout 这个主题观察,验证每次启动rapp所用的命名空间.
扫一扫
778
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
