Open kochigami opened 7 years ago
よいですね.launch file を添付して、rosbagと合わせて実機がなくても再現できると嬉しいです.
2週間も放置してしまいすみません。 ペッパーのせいではなく私のせいでした。
jsk_pcl/OrganizedMultiPlaneSegmentation
しようとしていた以下メモ:
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>
結局 #619 での議論に戻ってきた
pepperの点群はorganized (heightの値が1ではないから)
[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>
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>
プログラムは後で貼る
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