ROS 学习系列 -- 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

打开另一个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>

版权声明：本文为博主原创文章，未经博主允许不得转载。