Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[jsk_pcl_ros] ExtractIndicesに同じnamespaceのpointcloudとindicesを入れるとbond brokenする #2626

Closed
shmpwk opened this issue Sep 17, 2021 · 4 comments
Closed

[jsk_pcl_ros] ExtractIndicesに同じnamespaceのpointcloudとindicesを入れるとbond brokenする #2626

shmpwk opened this issue Sep 17, 2021 · 4 comments

Comments

@shmpwk
Copy link
Contributor

shmpwk commented Sep 17, 2021 •
edited

ExtractIndicesで入力~input (sensor_msgs/PointCloud2)と~indices (pcl_msgs/PointIndices)で，同じnamespaceのトピックを入れるとbond brokenします．

環境: Ubuntu18.04+melodic + pcl 1.8.1

例えば，OrganizedEdgeDetectorから出てきたトピックを ExtractIndicesに次のように入力します．

<remap from="~input" to="/organized_edge_detector/output" />
<remap from="~indices" to="/organized_edge_detector/output_rgb_edge_indices" />

launchしてもすぐにエラーは出ないのですが，
/extract_edge_indices/outputをrvizで可視化したり，rostopic echo や rostopic hz で読んだりすると，落ちます．

次は再現launchファイルです．rosbagの点群をOrganizedEdgeDetectorを通してさらにExtractIndicesに入力します．
jsk_recognition系の環境を手元でbuildしていればそのまま立ち上げることができるかと思います．

<launch>
  <arg name="manager" default="test_manager" />
  <arg name="points" default="/right_hand_camera/depth_registered/points" />
  <include file="$(find jsk_pcl_ros_utils)/sample/include/play_rosbag_shelf_bin.xml"/>

  <node name="$(arg manager)"
        pkg="nodelet" type="nodelet"
        args="manager"/>
  
  <node name="organized_edge_detector"
        pkg="nodelet" type="nodelet"
        args="load jsk_pcl/OrganizedEdgeDetector $(arg manager)" >
    <remap from="~input" to="$(arg points)"/>
    <rosparam>
      publish_normal: true
      use_nan_boundary: true
      use_rgb: true
    </rosparam>
  </node>

  <!--
  <node name="border_estimator"
        pkg="nodelet" type="nodelet"
        args="load jsk_pcl/BorderEstimator $(arg manager)">
    <remap from="~input" to="$(arg points)"/>
    <remap from="~input_camera_info" to="/kinect_head/rgb/camera_info"/>
  </node> 
  -->

  <node name="extract_edge_indices"
      pkg="nodelet" type="nodelet" 
      args="load jsk_pcl/ExtractIndices $(arg manager)">
    <remap from="~input" to="/organized_edge_detector/output" />
    <!--remap from="~input" to="/border_estimator/output_cloud" /-->
    <remap from="~indices" to="/organized_edge_detector/output_rgb_edge_indices" />
    <!--remap from="~indices" to="/border_estimator/output_veil_indices" /-->
  </node>
   <node name="rviz"
         pkg="rviz" type="rviz"
         args="-d $(find jsk_pcl_ros)/sample/rviz/organized_edge_detector.rviz"/>
</launch>

launch 結果です．
プログラムを走らせて何もしなければそのままですが，/extract_edge_indices/output を呼ぶとprocess has diedしました．

... logging to /home/shmpwk/.ros/log/ae7723ec-16f2-11ec-afe8-7085c2dd7af3/roslaunch-shmpwk-GX9A-14863.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://shmpwk-GX9A:37517/

SUMMARY
========

PARAMETERS
 * /organized_edge_detector/publish_normal: True
 * /organized_edge_detector/use_nan_boundary: True
 * /organized_edge_detector/use_rgb: True
 * /right_hand_camera/depth_rectify_depth/interpolation: 0
 * /right_hand_camera/depth_registered_rectify_depth/interpolation: 0
 * /right_hand_camera/right_hand_camera_nodelet_manager/num_worker_threads: 4
 * /rosdistro: melodic
 * /rosversion: 1.14.11
 * /use_sim_time: True

NODES
  /
    extract_edge_indices (nodelet/nodelet)
    organized_edge_detector (nodelet/nodelet)
    right_hand_camera_base_link (tf2_ros/static_transform_publisher)
    right_hand_camera_base_link1 (tf2_ros/static_transform_publisher)
    right_hand_camera_base_link2 (tf2_ros/static_transform_publisher)
    right_hand_camera_base_link3 (tf2_ros/static_transform_publisher)
    rosbag_play (rosbag/play)
    rviz (rviz/rviz)
    test_manager (nodelet/nodelet)
  /right_hand_camera/
    depth_metric (nodelet/nodelet)
    depth_metric_rect (nodelet/nodelet)
    depth_points (nodelet/nodelet)
    depth_rectify_depth (nodelet/nodelet)
    depth_registered_hw_metric_rect (nodelet/nodelet)
    depth_registered_metric (nodelet/nodelet)
    depth_registered_rectify_depth (nodelet/nodelet)
    points_xyzrgb_hw_registered (nodelet/nodelet)
    rgb_rectify_color (nodelet/nodelet)
    right_hand_camera_nodelet_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [14896]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to ae7723ec-16f2-11ec-afe8-7085c2dd7af3
process[rosout-1]: started with pid [14930]
started core service [/rosout]
process[rosbag_play-2]: started with pid [14936]
process[right_hand_camera/right_hand_camera_nodelet_manager-3]: started with pid [14938]
process[right_hand_camera/rgb_rectify_color-4]: started with pid [14944]
process[right_hand_camera/depth_rectify_depth-5]: started with pid [14945]
process[right_hand_camera/depth_metric_rect-6]: started with pid [14947]
process[right_hand_camera/depth_metric-7]: started with pid [14957]
process[right_hand_camera/depth_points-8]: started with pid [14963]
process[right_hand_camera/depth_registered_rectify_depth-9]: started with pid [14972]
[ INFO] [1631799211.537256159]: Initializing nodelet with 4 worker threads.
process[right_hand_camera/points_xyzrgb_hw_registered-10]: started with pid [14984]
process[right_hand_camera/depth_registered_hw_metric_rect-11]: started with pid [14991]
process[right_hand_camera/depth_registered_metric-12]: started with pid [14992]
process[right_hand_camera_base_link-13]: started with pid [14999]
process[right_hand_camera_base_link1-14]: started with pid [15008]
process[right_hand_camera_base_link2-15]: started with pid [15014]
process[right_hand_camera_base_link3-16]: started with pid [15020]
process[test_manager-17]: started with pid [15049]
process[organized_edge_detector-18]: started with pid [15057]
process[extract_edge_indices-19]: started with pid [15058]
process[rviz-20]: started with pid [15059]
[ WARN] [1631799214.520991944, 1486278999.834764891]: Detected jump back in time of 2.75989s. Clearing TF buffer.
[ WARN] [1631799216.873090111, 1486279002.189037446]: '/extract_edge_indices' subscribes topics only with child subscribers.
[ WARN] [1631799217.300984173, 1486278999.831723040]: Detected jump back in time of 2.75987s. Clearing TF buffer.
[ WARN] [1631799220.080969301, 1486278999.838270832]: Detected jump back in time of 2.75988s. Clearing TF buffer.
[ WARN] [1631799222.860973069, 1486278999.834528740]: Detected jump back in time of 2.75989s. Clearing TF buffer.
[ WARN] [1631799223.983586171, 1486279000.961046864]: Messages of type 0 arrived out of order (will print only once)
[ WARN] [1631799225.640998838, 1486278999.832475183]: Detected jump back in time of 2.75989s. Clearing TF buffer.
[ WARN] [1631799228.421073646, 1486278999.838469863]: Detected jump back in time of 2.75989s. Clearing TF buffer.
[ WARN] [1631799231.201087274, 1486278999.834674341]: Detected jump back in time of 2.75993s. Clearing TF buffer.
[test_manager-17] process has died [pid 15049, exit code -11, cmd /opt/ros/melodic/lib/nodelet/nodelet manager __name:=test_manager __log:=/home/shmpwk/.ros/log/ae7723ec-16f2-11ec-afe8-7085c2dd7af3/test_manager-17.log].
log file: /home/shmpwk/.ros/log/ae7723ec-16f2-11ec-afe8-7085c2dd7af3/test_manager-17*.log
[ WARN] [1631799233.981300893, 1486278999.831030748]: Detected jump back in time of 2.75989s. Clearing TF buffer.
[extract_edge_indices-19] process has finished cleanly
log file: /home/shmpwk/.ros/log/ae7723ec-16f2-11ec-afe8-7085c2dd7af3/extract_edge_indices-19*.log

GDBで見ました．
Screenshot from 2021-09-16 22-36-14

ExtractIndicesはpcl_rosのextract_indicesノードが元になっているので，issueになってないか調べたのですがありませんでした．

OrganizedEdgeDetectorの代わりに，BorderEstimator
から出力されるものを，ExtractIndicesに次のように入力しても同様に落ちます．

<remap from="~input" to="/border_estimator/output_cloud" />
<remap from="~indices" to="/border_estimator/output_veil_indices" />

そもそもですが，この問題は，ExtractIndicesで
<remap from="~input" to="/organized_edge_detector/output" />
の代わりに
<remap from="~input" to="/right_hand_camera/depth_registered/points" />
のように，カメラのトピックを入れれば，正常に動きます．
前者と後者は同じ挙動を期待しています．
なので，そもそもこのissueは回避できるものなのですが，不思議な問題だなと思い投稿しました．
@shmpwk
Copy link
Contributor Author

shmpwk commented Sep 17, 2021 •
edited

すみません，めちゃくちゃ初歩的な勘違いでした．．．
問題のコードは~inputのpointcloudのindixのsizeが~indicesより小さいので，ExtraIndices実行すると~indicesで指定したindex外のpointcloudを抜き取ろうとしてエラーが起こっていました．

@shmpwk shmpwk closed this as completed Sep 17, 2021
@knorth55
Copy link
Member

それならinputとindicesの長さをチェックして，Errorを表示してみたらいいんじゃないでしょうか？

@knorth55
Copy link
Member

難しく考えずにただlengthを比較して違えばros_errorを出せばいいという話です
ros_errorを出した後にsegmentationfault回避でreturnして後の処理を全てスキップするというのもありです

@shmpwk shmpwk mentioned this issue Sep 18, 2021
Merged
@shmpwk
Copy link
Contributor Author

shmpwk commented Sep 18, 2021

遅くなってすみません，PR作りましたのでご確認ください．#2628

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@knorth55 @shmpwk