[pepper] 平面検出できない

[kochigami]

プログラムは後で貼る

send *pepper* :reset-pose

send *pepper* :head :neck-p :joint-angle 10

机に近づける

rosbag: https://drive.google.com/a/jsk.imi.i.u-tokyo.ac.jp/file/d/0B903DnmsYXiBZ0d3TFVNVTAzVWs/view?usp=sharing

• プログラムを書き換えたらどうなる？
• 姿勢の問題？
• センサの設定を変える

[k-okada]

よいですね．launch file を添付して、rosbagと合わせて実機がなくても再現できると嬉しいです．

[kochigami]

2週間も放置してしまいすみません。 ペッパーのせいではなく私のせいでした。

• ペッパーの点群がOrganizedではないのに、jsk_pcl/OrganizedMultiPlaneSegmentation しようとしていた
• Organizedではない点群を、Organizedにするにはどうすればいいのかはまだ分かりません・・・

以下メモ：

rostopic echo /pepper_robot/camera/depth_registered/points  -n 1
is_dense: False

jsk_pcl/NormalEstimationOMP とjsk_pcl/RegionGrowingMultiplePlaneSegmentation を使って、 ここまではできました。 机を検出したいので、jsk_pcl_utils/PlaneRejector を使いたいのですが、全くトピックが返ってこなくて、 多分Organized にしないといけないのかな、と思いました。 そうすると、初めからOrganizedにしたほうが良いのかな、と思いました。

send *pepper* :head :neck-p :joint-angle 10

send *pepper* :head :neck-p :joint-angle 20

send *pepper* :head :neck-p :joint-angle 30

<launch>
  <arg name="launch_manager" default="true" />
  <arg name="cloud_input" default="/pepper_robot/camera/depth_registered/points" />
  <arg name="sensor_frame" default="CameraDepth_optical_frame" />
  <arg name="base_frame_id" default="base_footprint" />
  <arg name="min_likelihood" default="0.3" />
  <arg name="vital_rate" default="0.2" />

  <!-- manager -->
  <arg unless="$(arg launch_manager)"
       name="manager" default="pepper_pointcloud_nodelet_manager" />
  <arg if="$(arg launch_manager)"
       name="manager" default="take_from_table_nodelet_manager" />

  <node if="$(arg launch_manager)"
        name="$(arg manager)" pkg="nodelet" type="nodelet"
        args="manager" output="screen" respawn="true"/>

  <!-- tabletop object detection -->
  <group ns="tabletop_object_detector">
    <node name="input_relay" pkg="nodelet" type="nodelet" respawn="true"
          args="load jsk_topic_tools/Relay /$(arg manager)">
      <remap from="~input" to="$(arg cloud_input)" />
    </node>
    <node pkg="nodelet" type="nodelet" name="passthrough"  respawn="true"
          args="load pcl/PassThrough /$(arg manager)" >
      <remap from="~input" to="input_relay/output" />
      <rosparam subst_value="true">                                                                                           
        filter_field_name: z                                                                                                  
        filter_limit_min: 0.08                                                                                                
        filter_limit_max: 1.20                                                                                                
        filter_limit_negative: False                                                                                          
        keep_organized: true                                                                                                  
        input_frame: $(arg base_frame_id)                                                                                     
      </rosparam>
    </node>
    <node pkg="nodelet" type="nodelet" name="normal_estimation"  respawn="true"
          args="load jsk_pcl/NormalEstimationOMP /$(arg manager)" >
      <remap from="~input" to="passthrough/output" />
    </node>
    <node pkg="nodelet" type="nodelet" name="multi_plane_estimate"  respawn="true"
          args="load jsk_pcl/RegionGrowingMultiplePlaneSegmentation /$(arg manager)" >
      <remap from="~input" to="passthrough/output" />
      <remap from="~input_normal" to="normal_estimation/output" />
    </node>
    <node name="table_extractor"
          pkg="nodelet" type="nodelet"  respawn="true"
          args="load jsk_pcl_utils/PlaneRejector /$(arg manager)">
      <remap from="~input_inliers" to="multi_plane_estimate/output/inliers" />
      <remap from="~input_polygons" to="multi_plane_estimate/output/polygons" />
      <remap from="~input_coefficients" to="multi_plane_estimate/output/coefficients" />
      <rosparam subst_value="true">                                                                                           
        processing_frame_id: base_footprint                                                                                   
        reference_axis: [0, 0, 1]                                                                                             
        angle: 0                                                                                                              
        use_inliers: true                                                                                                     
        vital_rate: $(arg vital_rate)                                                                                         
      </rosparam>
    </node>

 </group>

</launch>

[kochigami]

結局 #619 での議論に戻ってきた

• pepperの点群はorganized (heightの値が１ではないから)

  [http://133.11.216.124:11311][133.11.216.124] kochigami@kochigami-ThinkPad-T450:~/catkin_ws/src/pepper_robot/pepper_bringup/launch$ rostopic echo /pepper_robot/camera/depth_registered/points/width -n 1
  320
  ---
  [http://133.11.216.124:11311][133.11.216.124] kochigami@kochigami-ThinkPad-T450:~/catkin_ws/src/pepper_robot/pepper_bringup/launch$ rostopic echo /pepper_robot/camera/depth_registered/points/height -n 1
  240
  ---

• 点群がゆがんでいる

• 床と壁はパラメータの値を極端に大きく・小さくすると、平面らしきものが出てくる

• 見ているのはOrganizedMultiPlaneSegmentationの結果

• min_sizeを小さく、 max_curvatureを大きく

• 次は机とpepperの距離を離してみる

<!-- -*- mode: xml -*- -->
<launch>
  <arg name="INPUT" default="/pepper_robot/camera/depth_registered/points"/>
  <arg name="FILTER_NAME_SUFFIX" default=""/>
  <arg name="create_manager" default="true" />
  <arg name="manager" default="plane_extraction_manager" />

  <node if="$(arg create_manager)"
        pkg="nodelet" type="nodelet" name="$(arg manager)"
        args="manager" output="screen"/>

  <node name="input_relay" pkg="nodelet" type="nodelet"
        args="load jsk_topic_tools/Relay $(arg manager)">
      <remap from="~input" to="$(arg INPUT)" />
  </node>

  <node pkg="nodelet" type="nodelet" name="multi_plane_estimate"
        args="load jsk_pcl/OrganizedMultiPlaneSegmentation $(arg manager)"
        output="screen">
    <remap from="~input" to="/input_relay/output" />
    <rosparam>                                                                                                                                                 
      use_async: true                                                                                                                                          
      min_size: 10                                                                                                                                             
      max_curvature: 0.5                                                                                                                                       
      estimate_normal: true                                                                                                                                    
      publish_normal: true                                                                                                                                     
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="plane_extraction" args="load jsk_pcl/MultiPlaneExtraction $(arg manager)" output="screen">
    <rosparam>                                                                                                                                                 
      use_async: true                                                                                                                                          
      max_height: 1.0                                                                                                                                          
      use_indices: false                                                                                                                                       
    </rosparam>
    <remap from="~input" to="/input_relay/output" />
    <remap from="~indices" to="/multi_plane_estimate/output_refined" />
    <remap from="~input_polygons" to="/multi_plane_estimate/output_refined_polygon" />
    <remap from="~input_coefficients" to="/multi_plane_estimate/output_refined_coefficients" />
  </node>

</launch>

[kochigami]

memo

• put this rosbag under jsk_pepper_startup

• roslaunch plane_estimation_test.launch

• download this .rviz file

• rviz -d plane_estimation_test.rviz

Even /multi_plane_estimate/output_nonconnected_polygon, they didn't appear on the table. I also confirmed /multi_plane_estimate/output_polygon and /multi_plane_estimate/output_refined_polygon did not appear on the table.

plane_estimation_test.launch

<!-- -*- mode: xml -*- -->
<launch>
  <arg name="INPUT" default="/pepper_robot/camera/depth_registered/points"/>
  <arg name="FILTER_NAME_SUFFIX" default=""/>
  <arg name="create_manager" default="true" />
  <arg name="manager" default="plane_extraction_manager" />

  <param name="/use_sim_time" value="true" />
  <node name="rosbag_play"
        pkg="rosbag" type="play"
        args="$(find jsk_pepper_startup)/2017-06-28-17-48-25_for_table_detection.bag --clock --loop"/>

  <node if="$(arg create_manager)"
        pkg="nodelet" type="nodelet" name="$(arg manager)"
        args="manager" output="screen"/>

  <node name="input_relay" pkg="nodelet" type="nodelet"
        args="load jsk_topic_tools/Relay $(arg manager)">
    <remap from="~input" to="$(arg INPUT)" />
  </node>

  <node pkg="nodelet" type="nodelet" name="multi_plane_estimate"
        args="load jsk_pcl/OrganizedMultiPlaneSegmentation $(arg manager)"
        output="screen">
    <remap from="~input" to="/input_relay/output" />
    <rosparam>                                                                                                         
      use_async: true                                                                                                  
      min_size: 10                                                                                                     
      max_curvature: 0.5                                                                                               
      estimate_normal: true                                                                                            
      publish_normal: true                                                                                             
    </rosparam>
  </node>
</launch>