search

ros-naoqi / pepper_robot

Meta-package for basic Pepper robot-related packages
25 stars 47 forks source link

[pepper_bringup] add perception.launch.xml #27

Closed furushchev closed 8 years ago

furushchev commented 8 years ago

enables rectify and depth registration needs https://github.com/ros-drivers/rgbd_launch/pull/28

kochigami commented 8 years ago

Hi, I tried this pull-request with rgbd_launch pull-request because I'd like to use this !

I executed 

roslaunch pepper_bringup pepper_full.launch network_interface:=eth3
roscd pepper_bringup/config & rosrun rviz rviz -d pepper.rviz

New topics are as follows:

/pepper_robot/camera/bottom/image_rect_color
/pepper_robot/camera/bottom/image_rect_color/compressed
/pepper_robot/camera/bottom/image_rect_color/compressed/parameter_descriptions
/pepper_robot/camera/bottom/image_rect_color/compressed/parameter_updates
/pepper_robot/camera/bottom/image_rect_color/compressedDepth
/pepper_robot/camera/bottom/image_rect_color/compressedDepth/parameter_descriptions
/pepper_robot/camera/bottom/image_rect_color/compressedDepth/parameter_updates
/pepper_robot/camera/bottom/image_rect_color/theora
/pepper_robot/camera/bottom/image_rect_color/theora/parameter_descriptions
/pepper_robot/camera/bottom/image_rect_color/theora/parameter_updates
/pepper_robot/camera/bottom_rectify_color/parameter_descriptions
/pepper_robot/camera/bottom_rectify_color/parameter_updates
/pepper_robot/camera/depth/image
/pepper_robot/camera/depth/image/compressed
/pepper_robot/camera/depth/image/compressed/parameter_descriptions
/pepper_robot/camera/depth/image/compressed/parameter_updates
/pepper_robot/camera/depth/image/compressedDepth
/pepper_robot/camera/depth/image/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth/image/compressedDepth/parameter_updates
/pepper_robot/camera/depth/image/theora
/pepper_robot/camera/depth/image/theora/parameter_descriptions
/pepper_robot/camera/depth/image/theora/parameter_updates
/pepper_robot/camera/depth/image_rect
/pepper_robot/camera/depth/image_rect/compressed
/pepper_robot/camera/depth/image_rect/compressed/parameter_descriptions
/pepper_robot/camera/depth/image_rect/compressed/parameter_updates
/pepper_robot/camera/depth/image_rect/compressedDepth
/pepper_robot/camera/depth/image_rect/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth/image_rect/compressedDepth/parameter_updates
/pepper_robot/camera/depth/image_rect/theora
/pepper_robot/camera/depth/image_rect/theora/parameter_descriptions
/pepper_robot/camera/depth/image_rect/theora/parameter_updates
/pepper_robot/camera/depth/image_rect_raw
/pepper_robot/camera/depth/image_rect_raw/compressed
/pepper_robot/camera/depth/image_rect_raw/compressed/parameter_descriptions
/pepper_robot/camera/depth/image_rect_raw/compressed/parameter_updates
/pepper_robot/camera/depth/image_rect_raw/compressedDepth
/pepper_robot/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/pepper_robot/camera/depth/image_rect_raw/theora
/pepper_robot/camera/depth/image_rect_raw/theora/parameter_descriptions
/pepper_robot/camera/depth/image_rect_raw/theora/parameter_updates
/pepper_robot/camera/depth_rectify_depth/parameter_descriptions
/pepper_robot/camera/depth_rectify_depth/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressed
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressed/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressed/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressedDepth
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect/compressedDepth/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect/theora
/pepper_robot/camera/depth_registered/hw_registered/image_rect/theora/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect/theora/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressed
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressed/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressed/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_updates
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/theora
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/theora/parameter_descriptions
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw/theora/parameter_updates
/pepper_robot/camera/depth_registered/image
/pepper_robot/camera/depth_registered/image/compressed
/pepper_robot/camera/depth_registered/image/compressed/parameter_descriptions
/pepper_robot/camera/depth_registered/image/compressed/parameter_updates
/pepper_robot/camera/depth_registered/image/compressedDepth
/pepper_robot/camera/depth_registered/image/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth_registered/image/compressedDepth/parameter_updates
/pepper_robot/camera/depth_registered/image/theora
/pepper_robot/camera/depth_registered/image/theora/parameter_descriptions
/pepper_robot/camera/depth_registered/image/theora/parameter_updates
/pepper_robot/camera/depth_registered/points
/pepper_robot/camera/depth_registered/sw_registered/camera_info
/pepper_robot/camera/depth_registered/sw_registered/image_rect
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressed
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressed/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressed/parameter_updates
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressedDepth
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_updates
/pepper_robot/camera/depth_registered/sw_registered/image_rect/theora
/pepper_robot/camera/depth_registered/sw_registered/image_rect/theora/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect/theora/parameter_updates
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressed
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_updates
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_updates
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/theora
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/theora/parameter_descriptions
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw/theora/parameter_updates
/pepper_robot/camera/depth_registered_rectify_depth/parameter_descriptions
/pepper_robot/camera/depth_registered_rectify_depth/parameter_updates
/pepper_robot/camera/front/image_rect_color
/pepper_robot/camera/front/image_rect_color/compressed
/pepper_robot/camera/front/image_rect_color/compressed/parameter_descriptions
/pepper_robot/camera/front/image_rect_color/compressed/parameter_updates
/pepper_robot/camera/front/image_rect_color/compressedDepth
/pepper_robot/camera/front/image_rect_color/compressedDepth/parameter_descriptions
/pepper_robot/camera/front/image_rect_color/compressedDepth/parameter_updates
/pepper_robot/camera/front/image_rect_color/theora
/pepper_robot/camera/front/image_rect_color/theora/parameter_descriptions
/pepper_robot/camera/front/image_rect_color/theora/parameter_updates
/pepper_robot/camera/front_rectify_color/parameter_descriptions
/pepper_robot/camera/front_rectify_color/parameter_updates
/pepper_robot/camera/ir/image_rect_ir
/pepper_robot/camera/ir/image_rect_ir/compressed
/pepper_robot/camera/ir/image_rect_ir/compressed/parameter_descriptions
/pepper_robot/camera/ir/image_rect_ir/compressed/parameter_updates
/pepper_robot/camera/ir/image_rect_ir/compressedDepth
/pepper_robot/camera/ir/image_rect_ir/compressedDepth/parameter_descriptions
/pepper_robot/camera/ir/image_rect_ir/compressedDepth/parameter_updates
/pepper_robot/camera/ir/image_rect_ir/theora
/pepper_robot/camera/ir/image_rect_ir/theora/parameter_descriptions
/pepper_robot/camera/ir/image_rect_ir/theora/parameter_updates
/pepper_robot/camera/ir_rectify_ir/parameter_descriptions
/pepper_robot/camera/ir_rectify_ir/parameter_updates

I also checked outputs by using rviz Image.

/pepper_robot/camera/bottom/image_rect_color
/pepper_robot/camera/depth/image_rect
/pepper_robot/camera/depth/image_rect_raw
/pepper_robot/camera/depth_registered/points
/pepper_robot/camera/depth_registered/sw_registered/image_rect
/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw
/pepper_robot/camera/front/image_rect_color
/pepper_robot/camera/ir/image_rect_ir

show output.

However, 

/pepper_robot/camera/depth_registered/image
/pepper_robot/camera/depth_registered/hw_registered/image_rect
/pepper_robot/camera/depth_registered/hw_registered/image_rect_raw

showed nothing. (I don't think it is a problem though.)

I found the terminal of pepper_full.launch shows

[ WARN] [1473339319.233269310]: TF2 exception:
Lookup would require extrapolation into the future.  Requested time 1473339319.232140798 but the latest data is at time 1473339318.744720039, when looking up transform from frame [CameraDepth_optical_frame] to frame [CameraTop_optical_frame]

It seems that depth_registered processing requires static_tf_publisher. (I'm not sure. It may be only my PC problem.)

furushchev commented 8 years ago

@kochigami Nice catch! Currently tf publication from CameraDepth_optical_frame to CameraTop_optical_frame is too slow for depth registration

$ rosrun tf tf_monitor CameraDepth_optical_frame CameraTop_optical_frame
RESULTS: for CameraDepth_optical_frame to CameraTop_optical_frame
Chain is: CameraDepth_optical_frame -> CameraDepth_frame -> Head -> Neck -> torso -> Neck -> Head -> CameraTop_frame
-> CameraTop_optical_frame
Net delay     avg = 0.040272: max = 0.640725

Frames:
Frame: CameraDepth_frame published by unknown_publisher Average Delay: 0.00310782 Max Delay: 0.0437475
Frame: CameraDepth_optical_frame published by unknown_publisher Average Delay: 0.00310642 Max Delay: 0.0437458
Frame: CameraTop_frame published by unknown_publisher Average Delay: 0.00311052 Max Delay: 0.0437512
Frame: CameraTop_optical_frame published by unknown_publisher Average Delay: 0.00310918 Max Delay: 0.0437492
Frame: Head published by unknown_publisher Average Delay: 0.0030323 Max Delay: 0.0436482
Frame: Head published by unknown_publisher Average Delay: 0.0030323 Max Delay: 0.0436482
Frame: Neck published by unknown_publisher Average Delay: 0.00303538 Max Delay: 0.0436521
Frame: Neck published by unknown_publisher Average Delay: 0.00303538 Max Delay: 0.0436521
Frame: torso published by unknown_publisher Average Delay: 0.00314292 Max Delay: 0.0437953

All Broadcasters:
Node: unknown_publisher 15.0153 Hz, Average Delay: 0.00305912 Max Delay: 0.0432073

This frequency is originated from naoqi_driver (https://github.com/ros-naoqi/naoqi_driver/blob/master/share/boot_config.json#L61), and after I increased value to 100, the warning you pointed out disappeared, and all topics are published more frequently. I'm now creating another PR to naoqi_driver.

furushchev commented 8 years ago

BTW, the chain from depth to front color camera is too long, I think...

kochigami commented 8 years ago

@furushchev Thank you very much for your help ! I could clear my head.

k-okada commented 8 years ago

this is interesting, can we use approx sync for color and depth? > @furushchev

furushchev commented 8 years ago

@k-okada OK, I will take a look into soon.

furushchev commented 8 years ago

Now ros-drivers/rgbd_launch#28 was merged, and waiting https://github.com/ros-naoqi/naoqi_driver/pull/72

mikaelarguedas commented 8 years ago

ros-naoqi/naoqi_driver#72 is now merged too so the transforms should be published often enough to perform registration

furushchev commented 8 years ago

@mikaelarguedas Thank you for summarizing, and yes. To pass a test, changes on ros-drivers/rgbd_launch#28 need to be relesaed ( I also created the issue: https://github.com/ros-drivers/rgbd_launch/issues/33 ).

furushchev commented 8 years ago

@k-okada

this is interesting, can we use approx sync for color and depth? > @furushchev

I found that depth registration nodelet uses approximate sync by default and queue size are set to 5. We may register even if tf is slow by increasing this queue size. (but we will have to write own launch file to do so)

furushchev commented 8 years ago

confirmed that changes on ros-drivers/rgbd_launch#28 was already released. Could you restart testing? cc: @mikaelarguedas @k-okada

k-okada commented 8 years ago

ok, restarted travis, do we also need https://github.com/kochigami/naoqi_driver/commit/d8ce96404251adf236ebe14084037a7a1a053112 ?

◉ Kei Okada

On Tue, Sep 13, 2016 at 5:39 PM, Furushchev notifications@github.com wrote:

confirmed that changes on ros-drivers/rgbd_launch#28 https://github.com/ros-drivers/rgbd_launch/pull/28 was already released. Could you restart testing? cc: @mikaelarguedas https://github.com/mikaelarguedas @k-okada https://github.com/k-okada

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/ros-naoqi/pepper_robot/pull/27#issuecomment-246614304, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3K_TGMG0IYauxyHmD6zTXKFoeWu_ks5qpmFEgaJpZM4J3jJ2 .

furushchev commented 8 years ago

@k-okada To just publish registered point cloud, it is not necessary, but to work SLAM, yes.

@kochigami Anyway, at almost case compressed depth image is too noisy to use, so you can create pull request of this change on your branch :+1: kochigami/naoqi_driver@d8ce964

kochigami commented 8 years ago

Hi, I created a pull request for kRawDepthColorSpacein https://github.com/ros-naoqi/naoqi_driver/pull/74 Thank you very much for your advice!

and I think pepper_perception.launch seems to be pepper_perception.launch."xml" in pepper_bringup/launch/pepper_full_py.launch.

furushchev commented 8 years ago

@kochigami thx :+1:

furushchev commented 8 years ago

waiting https://github.com/ros-drivers/rgbd_launch/pull/35 is released on jade.

furushchev commented 8 years ago

released on jade https://github.com/ros/rosdistro/pull/12687 and now available on ros-shadow-fixed.

$ sudo apt-cache policy ros-jade-rgbd-launch 
ros-jade-rgbd-launch:
  Installed: (none)
  Candidate: 2.2.2-0trusty-20160913-224143-0700
  Version table:
     2.2.2-0trusty-20160913-224143-0700 0
        500 http://packages.ros.org/ros-shadow-fixed/ubuntu/ trusty/main amd64 Packages

@k-okada could you please restart test once more for testing jade?

mikaelarguedas commented 8 years ago

@suryaambrose do you agree to release naoqi-bridge to be able to test this PR ?

mikaelarguedas commented 8 years ago

@suryaambrose FYI: released naoqi-bridge in indigo, jade, kinetic I'll try to monitor the jobs but if you receive any buildfarm failure email please let me know

furushchev commented 8 years ago

@mikaelarguedas @suryaambrose any update?

mikaelarguedas commented 8 years ago

lgtm thanks @furushchev

furushchev commented 8 years ago

@mikaelarguedas @severin-lemaignan Thank you for reviewing and merging!

furushchev commented 8 years ago

This PR also seems to resolve #12