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>
版權聲明:本文爲博主原創文章,未經博主允許不得轉載。