Skip to content

euclidean_clusteringにおいて,該当するclusterが1個もないときにcluster_indicesに空のindicesの配列1個が格納される状態は正しいのか #2858

@mqcmd196

Description

@mqcmd196

再現方法

  1. rosbagの再生
    https://drive.google.com/file/d/1mn5Xa6DNrsXqwjxzSlBCVux5sDsEJAvh/view?usp=sharingを,rosbag play ./tsubaki.bag -l で再生する

  2. launchの立ち上げ

<launch>

        <group ns="recognize_wound">

          <node name="image_decompresser"
                pkg="image_transport" type="republish"
                args="compressed raw" respawn="true">
            <remap from="in" to="/kinect_head/rgb/image_rect_color"/>
            <remap from="out" to="decompressed_image"/>
          </node>

          <node name="depth_decompresser"
                pkg="image_transport" type="republish"
                args="compressedDepth raw" respawn="true">
            <remap from="in" to="/kinect_head/depth_registered/image"/>
            <remap from="out" to="decompressed_depth"/>
          </node>

          <node pkg="jsk_topic_tools" type="synchronize_republish.py" name="synchronize_republish">
            <param name="topics" value="[/recognize_wound/decompressed_image, /recognize_wound/decompressed_depth]" type="yaml"/>
            <param name="approximate_sync" value="true" />
          </node>

          <arg name="manager" value="recognize_wound_manager" />

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

          <node pkg="nodelet" type="nodelet" name="decompress_points"
                args="load depth_image_proc/point_cloud_xyzrgb $(arg manager)">
            <remap from="rgb/camera_info" to="/kinect_head/depth_registered/camera_info"/>
            <remap from="rgb/image_rect_color" to="synchronize_republish/pub_00"/>
            <remap from="depth_registered/image_rect" to="synchronize_republish/pub_01"/>
            <rosparam>
              queue_size: 100
            </rosparam>
          </node>

  <node name="transformable_interactive_server"
        pkg="jsk_interactive_marker" type="transformable_server_sample">
    <rosparam subst_value="true">
      display_interactive_manipulator: true
      display_interactive_manipulator_only_selected: true
      display_description_only_selected: true
    </rosparam>
  </node>
  <node name="transformable_table_markers"
        pkg="jsk_interactive_marker" type="transformable_markers_client.py">
    <remap from="~server" to="transformable_interactive_server" />
    <rosparam subst_value="true">
      config_file: $(find pr2_surgery)/config/table_marker.yaml
      config_auto_save: true  <!-- Use true to save config updated on rviz -->
    </rosparam>
  </node>
  <node name="bbox_array_to_bbox"
          pkg="nodelet" type="nodelet"
          args="load jsk_pcl_utils/BoundingBoxArrayToBoundingBox $(arg manager)">
      <remap from="~input" to="transformable_table_markers/output/boxes" />
      <rosparam>
        index: 0
      </rosparam>
    </node>
    <node name="attention_clipper_table"
          pkg="nodelet" type="nodelet"
          args="load jsk_pcl/AttentionClipper $(arg manager)">
      <remap from="~input" to="/kinect_head/depth_registered/camera_info" />
      <remap from="~input/points" to="depth_registered/points" />
      <remap from="~input/box" to="bbox_array_to_bbox/output" />
      <rosparam subst_value="true">
        use_multiple_attention: false
      </rosparam>
    </node>
    <node name="extract_indices_table"
          pkg="nodelet" type="nodelet"
          args="load jsk_pcl/ExtractIndices $(arg manager)">
      <remap from="~input" to="depth_registered/points" />
      <remap from="~indices" to="attention_clipper_table/output/point_indices" />
      <rosparam>
        keep_organized: true
        approximate_sync: true
        max_queue_size: 100
      </rosparam>
    </node>

          <node pkg="nodelet" type="nodelet" name="hsi_filter"
                args="load jsk_pcl/HSIColorFilter $(arg manager)" output="screen">
            <remap from="~input" to="extract_indices_table/output" />
            <rosparam>
              use_indices: false
              keep_organized: true
              h_limit_max: 11
              h_limit_min: -8
              s_limit_max: 255
              s_limit_min: 104
              i_limit_max: 255
              i_limit_min: 0
            </rosparam>
          </node>

          <node pkg="nodelet" type="nodelet" name="euclidean_clustering"
                args="load jsk_pcl/EuclideanClustering $(arg manager)" output="screen">
            <remap from="~input" to="hsi_filter/output" />
            <rosparam>
              tolerance: 0.02
              min_size: 10 
            </rosparam>
          </node>

          <node pkg="nodelet" type="nodelet"
                name="cluster_decomposer"
                args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg manager)"
                output="screen" clear_params="true">
            <remap from="~input" to="hsi_filter/output" />
            <remap from="~target" to="euclidean_clustering/output" />
            <rosparam>
              align_boxes: true
              align_boxes_with_plane: false
              force_to_flip_z_axis: false
              use_pca: false
              target_frame_id: base_footprint
            </rosparam>
          </node>

        </group>
</launch>
  1. topic 確認
    rostopic info /recognize_wound/euclidean_clustering/output

rostopic info /recognize_wound/euclidean_clustering/output の出力が

❯ rostopic echo /recognize_wound/euclidean_clustering/output
header:
  seq: 23
  stamp:
    secs: 1733916386
    nsecs: 366280000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 24
  stamp:
    secs: 1733916386
    nsecs: 366280000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 25
  stamp:
    secs: 1733916386
    nsecs: 634271000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 26
  stamp:
    secs: 1733916386
    nsecs: 634271000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 0
  stamp:
    secs: 1733916386
    nsecs: 634271000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 1
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 27
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 2
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 3
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 28
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 4
  stamp:
    secs: 1733916386
    nsecs: 898343000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 5
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 6
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 7
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 29
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 30
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---
header:
  seq: 8
  stamp:
    secs: 1733916387
    nsecs: 166288000
  frame_id: "head_mount_kinect_rgb_optical_frame"
cluster_indices:
  -
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs:         0
      frame_id: ''
    indices: []
---

というtopicになっているのは正しいのか

Metadata

Metadata

Assignees

Labels

No labels
No labels

Type

No type

Projects

No projects

Milestone

No milestone

Relationships

None yet

Development

No branches or pull requests

Issue actions