jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

[pepper] 平面検出できない #797

Open kochigami opened 7 years ago

kochigami commented 7 years ago

プログラムは後で貼る

send *pepper* :reset-pose screenshot from 2017-06-28 17 29 07

send *pepper* :head :neck-p :joint-angle 10 screenshot from 2017-06-28 17 29 55

机に近づける screenshot from 2017-06-28 17 30 52

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

k-okada commented 7 years ago

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

kochigami commented 7 years ago

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

以下メモ:

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

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

send *pepper* :head :neck-p :joint-angle 10 screenshot from 2017-07-12 19 54 40

send *pepper* :head :neck-p :joint-angle 20 screenshot from 2017-07-12 19 55 53

send *pepper* :head :neck-p :joint-angle 30 screenshot from 2017-07-12 19 56 38

<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 commented 7 years ago

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

<!-- -*- 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 commented 5 years ago

memo

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>