turtlebot 有个实现iPhone360全景照相功能的应用 panorama. 官方使用Create底座和Kinnect, 在使用Roomba底座和Xtion Pro Live配套时发现，按照教程的方式启动不了。
1. 启动
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 \\下令照相转圈
这个时候发现，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);
}
从这里可以看出所有的信息都被发送到了 turtlebot\log 这个topic里面了，那就监视一些这个topic：
rostopic echo turtlebot\log
再执行一遍启动命令:
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.
<pre name="code" class="cpp">［well time xxxxx]: snap
很明显，事情卡在了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);
..............................
}
抓图的功能放到了另外一个进程。"pano_server/snap" 的实现在 turtlebot_apps/software/pano/pano_ros/nodes/capture_server.py 里面：
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
可以看到在image_view里面是没有图像的，因此可以判断是图像的主题在kinnect和XtionPro的不一致造成的.
真正的图像主题在这里:
$ 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
<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>
版权声明：本文为博主原创文章，未经博主允许不得转载。
时间： 2024-08-08 09:40:33