Pointcloud 2 publishing problem

Question

Pointcloud 2 publishing problem

egeldres opened this issue 8 years ago · comments

Hi Rmonica,

I'm using ros_kinfu to extract the pointcloud2 that can be extracted, however my response message with the pointcloud2 doesnt appear to have any point, even when rviz shows the model and the kinfulaunch terminal shows that there are many points on the map being converted to pointcloud2.
I'm sending:
the kinfulaunch info terminal
the echo to the response terminal
the publisher node built

MAIN TERMINAL

...
[ INFO] [1488578630.917639316]: Avg frame time = 37.85 ms (26.42 fps)
[ INFO] [1488578632.362444687]: Avg frame time = 43.09 ms (23.21 fps)
//---------(request)
[ INFO] [1488578632.977390169]: kinfu: Extract Cloud Worker started.
[ INFO] [1488578632.977449758]: kinfu: Locking kinfu...
[ INFO] [1488578633.027745258]: kinfu: Locked.
Extracting current volume...The old cube's metric origin was (0.000000, 0.000000, 0.000000).
The new cube's metric origin is now (-0.018683, -0.007586, 0.006940).
SIZE IS 607195
shift: downloading weights...
shift: weights downloaded.
world contains 0 points after update
world contains 0 points after cleaning
[ INFO] [1488578636.757929727]: kinfu: retrieveOldCube: scheduled old cube.
shift: calling weight listener...
shift: done.
[ INFO] [1488578636.758035622]: kinfu: RetrieveCubeWorker: working...
Done
[ INFO] [1488578636.777796456]: kinfu: Marching cubes...
Getting world as cubes. World contains 607195 points.
World contains 607195 points after nan removal.
cube size is set to 512
[ INFO] [1488578637.150566945]: kinfu: RetrieveCubeWorker: retrieved 0 voxels.
[ INFO] [1488578637.150618302]: kinfu: NewCubeWorker: Setting known voxels...

[ INFO] [1488578637.154779249]: kinfu: retrieveOldCube: returned old cube.
known: uploading weights...
known: upload complete.
Bounding box for the world:
[66.000000 - 511.000000]
[27.000000 - 366.000000]
[29.000000 - 511.000000]
Extracting cube at: [66.000000, 27.000000, 29.000000].
[ INFO] [1488578637.619580363]: Avg frame time = 44.91 ms (22.27 fps)
returning 1 cubes
Processing world with volume size set to 3meters
There are 1 cubes to be processed
Processing cube number 0
[ INFO] [1488578639.121338163]: Avg frame time = 44.79 ms (22.33 fps)
VOLUME SIZE IS 3
Done!
[ INFO] [1488578640.722270426]: kinfu: Extracting only points from mesh...
[ INFO] [1488578640.722412967]: kinfu: Merging points...
[ INFO] [1488578640.722471226]: kinfu: Applying transformation...
[ INFO] [1488578640.722556896]: kinfu: Publishing...
[ INFO] [1488578640.722610838]: kinfu_output:
Received response with
0 cloud points
0 mesh cloud points
0 triangles
0 pixels
0 uint64 values
0 float32 values

[ INFO] [1488578640.724894327]: kinfu: Extract Cloud Worker complete.
[ INFO] [1488578640.909571324]: kinfu_output: Connection detected for topic /response.
[ INFO] [1488578641.141319865]: Avg frame time = 60.48 ms (16.53 fps)
[ INFO] [1488578641.909543631]: kinfu_output: Message to topic /response sent.
[ INFO] [1488578683.377241191]: Avg frame time = 40.58 ms (24.65 fps)
...
short time repeating
...
[ INFO] [1488578679.269798066]: Avg frame time = 44.09 ms (22.68 fps)
[ INFO] [1488578679.767178154]: kinfu: NewCubeWorker: Done: set 4555063 leaves.
[ INFO] [1488578679.767252487]: kinfu: NewCubeWorker: points are 4555063, bitmasks are 14361, full bitmasks are 0.
[ INFO] [1488578680.635297378]: Avg frame time = 40.64 ms (24.61 fps)
[ INFO] [1488578682.014967543]: Avg frame time = 41.03 ms (24.37 fps)
[ INFO] [1488578683.377241191]: Avg frame time = 40.58 ms (24.65 fps)
...
then it keeps repeating

The "empty" pointcloud2 I'm getting when echoing the topic is:

RESPONSE MESSAGE

header:
seq: 0
stamp:
secs: 1488578632
nsecs: 977435053
frame_id: /kinfu_first_frame
height: 1
width: 0
fields:

name: x
offset: 0
datatype: 7
count: 1
name: y
offset: 4
datatype: 7
count: 1
name: z
offset: 8
datatype: 7
count: 1
name: normal_x
offset: 16
datatype: 7
count: 1
name: normal_y
offset: 20
datatype: 7
count: 1
name: normal_z
offset: 24
datatype: 7
count: 1
name: curvature
offset: 32
datatype: 7
count: 1

is_bigendian: False
point_step: 48
row_step: 0
data: []
is_dense: True

PUBLISHER

I've build a publisher sending these specs to publish:

msg.tsdf_header.request_type = kinfu_msgs::KinfuRequestHeader::REQUEST_TYPE_GET_CLOUD;
msg.tsdf_header.request_id = 0;
msg.tsdf_header.request_source_name = "/response";

msg.request_reset = false;

msg.request_transformation = false;
msg.request_bounding_box=false;
msg.request_sphere=false;
msg.request_subsample=false;
msg.request_bounding_box_view=false;
msg.tsdf_center_distance=0.0;
msg.request_camera_intrinsics=false;

msg.request_view_poses=false;
msg.request_remove_duplicates=false;

I'll really appreciate your help again
And a short question, how long take a pointcloud 2 to build from a small capture meaning just a view without volume shift

Best regards

RMonica · Answer 1 · Thu Mar 09 2017 20:54:46 GMT+0800 (China Standard Time)

Hi egeldres.

I managed to find a laptop with a GTX 950M, in the hope that it would behave similarly to your 960M.
Nonetheless, I was unable to reproduce your issue.
Sorry.

Suggestions:

Try to discover where in the code the points disappear. Is it in standalone marching cubes?
Tinker with the CTA_SIZE_X and CTA_SIZE_Y parameters in templated_extract.cuh.
Maybe, if there is a race condition, something will change.
They must always be a power of two, and LOG_CTA_SIZE must be the sum of the exponents.
Use REQUEST_TYPE_GET_VOXELGRID with a bounding box. The result is less accurate, but it skips marching cubes.
See if you can use the kinfu_voxelgrid_conversions package to obtain a point cloud from the voxel grid.

Time needed to build a point cloud with marching cubes is (at least) a few seconds, depending on your system.

egeldres · Answer 2 · Fri Mar 10 2017 11:24:01 GMT+0800 (China Standard Time)

Hi Rmonica,

Thanks again for your response, and for sharing the experience with the similar laptop.

I have another little question before: Using this project with the "action-based interface", means that this project launches the action server, and it's required to build and use an action client in order to make the requests?

Continuing with my problem, It makes sense what you say about the marching cubes, I'll try to get a little more into that process

Tinkering the values and keeping the consistence from 0 to 128 isn't helping, over 128 it hangs.
But when using TSDF, REQUEST_TYPE_GET_VOXELGRID, or REQUEST_TYPE_GET_VIEW_CLOUD, it publishes the data.

Here are the view cloud kinfu launch terminal info

[ INFO] [1489115000.754683598]: kinfu: Extract View Worker started.
[ INFO] [1489115000.754772826]: kinfu: Locking kinfu...
[ INFO] [1489115000.802629531]: kinfu: initializing raycaster...
[ INFO] [1489115000.803159718]: kinfu: shiftNear...
The old cube's metric origin was (0.000000, 0.000000, 0.000000).
The new cube's metric origin is now (0.000000, 0.000000, -1.800000).
SIZE IS 30875
shift: downloading weights...
shift: weights downloaded.
world contains 0 points after update
world contains 0 points after cleaning
[ INFO] [1489115002.566684522]: kinfu: retrieveOldCube: scheduled old cube.
shift: calling weight listener...
shift: done.
[ INFO] [1489115002.566779621]: kinfu: RetrieveCubeWorker: working...
[ INFO] [1489115002.867738709]: kinfu: RetrieveCubeWorker: retrieved 0 voxels.
[ INFO] [1489115002.867781952]: kinfu: NewCubeWorker: Setting known voxels...

[ INFO] [1489115002.967108643]: kinfu: retrieveOldCube: returned old cube.
known: uploading weights...
known: upload complete.
[ INFO] [1489115003.033275486]: kinfu: generating scene...
[ INFO] [1489115003.033890648]: kinfu: downloading data...
[ INFO] [1489115003.038516532]: kinfu: Applying transform...
The old cube's metric origin was (0.000000, 0.000000, -1.800000).
The new cube's metric origin is now (0.021312, 0.008733, -0.001730).
SIZE IS 0
shift: downloading weights...
[ INFO] [1489115004.474876309]: kinfu: Sending message...
[ INFO] [1489115004.478677403]: kinfu_output:
Received response with
307200 cloud points
0 mesh cloud points
0 triangles
0 pixels
0 uint64 values
0 float32 values
[ INFO] [1489115004.482059060]: kinfu: Extract View Worker complete.
shift: weights downloaded.
world contains 30875 points after update
world contains 30875 points after cleaning
[ INFO] [1489115005.013773876]: kinfu: retrieveOldCube: scheduled old cube.
shift: calling weight listener...
shift: done.
SHIFTING
[ INFO] [1489115005.086013320]: Avg frame time = 98.79 ms (10.12 fps)
[ INFO] [1489115006.440584807]: Avg frame time = 40.55 ms (24.66 fps)

Many thanks again!

RMonica · Answer 3 · Sat Mar 11 2017 01:25:53 GMT+0800 (China Standard Time)

Hi egeldres.

Yes, you have to create a node with an action client to connect to the action server.
http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionClient
I suspect it won't fix your issue, however.

REQUEST_TYPE_GET_VIEW_CLOUD always publishes width*height points, since it simulates a view from a specific pose.
You still have to look at them and see if they make sense.

Afterthought: if you have time, you could also try to revert the marching cubes code to the original PCL code.
The marching_cubes.* and the standalone_marching_cubes.* files should be restored.
Standalone marching cubes is called here: https://github.com/RMonica/ros_kinfu/blob/master/kinfu/src/worlddownloadutils.cpp#L304

RMonica · Answer 4 · Tue Mar 14 2017 01:47:06 GMT+0800 (China Standard Time)

Hi egeldres.

Finally, I was able to reproduce the issue. If I compile in debug mode, I get an empty cloud as well.

Enable optimizations and see if the problem disappears:
With catkin_make:

  catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo

With catkin build:

  catkin build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

This suggests either undefined behavior in my code or a compiler bug.
I will look into it, if I have time.

egeldres · Answer 5 · Tue Mar 14 2017 04:17:18 GMT+0800 (China Standard Time)

Thanks!!!!!!
yesterday I've tried to revert to pcl largescalekinfu without any success, today I planned to merge both, but this saved me!
Now it works fine and it's really really fast!
thanks again.

egeldres · Answer 6 · Wed Mar 29 2017 11:20:13 GMT+0800 (China Standard Time)

Rmonica thanks and feel free to close this issue.

egeldres · Answer 7 · Wed Mar 29 2017 11:20:38 GMT+0800 (China Standard Time)

Thanks Rmonica,
please feel free to close this issue.

lion · Answer 8 · Wed Mar 29 2017 17:06:43 GMT+0800 (China Standard Time)

I am launch the kinfu2.launch, like this:

roslaunch kinfu kinfu2.launch 
... logging to /home/turtlebot/.ros/log/c0292d1c-145c-11e7-a666-d0577bf4a92a/roslaunch-turtlebot-OMEN-by-HP-Laptop-11892.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.1.102:60892/

SUMMARY
========

PARAMETERS
 * /kinect2_bridge/base_name: kinect2
 * /kinect2_bridge/base_name_tf: kinect2
 * /kinect2_bridge/bilateral_filter: True
 * /kinect2_bridge/calib_path: /home/turtlebot/c...
 * /kinect2_bridge/depth_device: -1
 * /kinect2_bridge/depth_method: default
 * /kinect2_bridge/edge_aware_filter: True
 * /kinect2_bridge/fps_limit: -1.0
 * /kinect2_bridge/jpeg_quality: 90
 * /kinect2_bridge/max_depth: 12.0
 * /kinect2_bridge/min_depth: 0.1
 * /kinect2_bridge/png_level: 1
 * /kinect2_bridge/publish_tf: False
 * /kinect2_bridge/queue_size: 5
 * /kinect2_bridge/reg_device: -1
 * /kinect2_bridge/reg_method: default
 * /kinect2_bridge/sensor: 
 * /kinect2_bridge/use_png: False
 * /kinect2_bridge/worker_threads: 4
 * /kinect2_points_xyzrgb_hd/queue_size: 5
 * /kinect2_points_xyzrgb_qhd/queue_size: 5
 * /kinect2_points_xyzrgb_sd/queue_size: 5
 * /kinect_kinfu1/autostart: True
 * /kinect_kinfu1/camera_info_topic: /kinect2/sd/camer...
 * /kinect_kinfu1/depth_height: 424
 * /kinect_kinfu1/depth_image_topic: /kinect2/sd/image...
 * /kinect_kinfu1/depth_width: 512
 * /kinect_kinfu1/extract_known_points: False
 * /kinect_kinfu1/prefix_topic: /kinect2
 * /kinect_kinfu1/request_topic: /kinfu_request_topic
 * /kinect_kinfu1/response_topic: /kinfu_response_t...
 * /rosdistro: indigo
 * /rosversion: 1.11.21

NODES
  /
    kinect2 (nodelet/nodelet)
    kinect2_bridge (nodelet/nodelet)
    kinect2_points_xyzrgb_hd (nodelet/nodelet)
    kinect2_points_xyzrgb_qhd (nodelet/nodelet)
    kinect2_points_xyzrgb_sd (nodelet/nodelet)
    kinect_kinfu1 (kinfu/kinfu)

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

setting /run_id to c0292d1c-145c-11e7-a666-d0577bf4a92a
process[rosout-1]: started with pid [11917]
started core service [/rosout]
process[kinect2-2]: started with pid [11932]
process[kinect2_bridge-3]: started with pid [11935]
process[kinect2_points_xyzrgb_sd-4]: started with pid [11936]
process[kinect2_points_xyzrgb_qhd-5]: started with pid [11937]
process[kinect2_points_xyzrgb_hd-6]: started with pid [11944]
[ INFO] [1490777425.538226078]: Loading nodelet /kinect2_bridge of type kinect2_bridge/kinect2_bridge_nodelet to manager kinect2 with the following remappings:
[ INFO] [1490777425.542021849]: waitForService: Service [/kinect2/load_nodelet] has not been advertised, waiting...
process[kinect_kinfu1-7]: started with pid [11966]
[ INFO] [1490777425.579610899]: Initializing nodelet with 8 worker threads.
[ INFO] [1490777425.583518730]: waitForService: Service [/kinect2/load_nodelet] is now available.
[ INFO] [1490777425.636883072]: [Kinect2Bridge::initialize] parameter:
        base_name: kinect2
           sensor: default
        fps_limit: -1
       calib_path: /home/turtlebot/catkin_ws/src/iai_kinect2/kinect2_bridge/data/
          use_png: false
     jpeg_quality: 90
        png_level: 1
     depth_method: default
     depth_device: -1
       reg_method: default
       reg_device: -1
        max_depth: 12
        min_depth: 0.1
       queue_size: 5
 bilateral_filter: true
edge_aware_filter: true
       publish_tf: false
     base_name_tf: kinect2
   worker_threads: 4
[Info] [CudaDepthPacketProcessorImpl] device 0: GeForce GTX 1070 @ 1645MHz Memory 8105MB
[pcl::gpu::printShortCudaDeviceInfo] : Device 0:  "GeForce GTX 1070"  8105Mb, sm_61, 2048 cores, Driver/Runtime ver.8.0/8.0
[Info] [CudaDepthPacketProcessorImpl] selected device 0
[ INFO] [1490777425.762206758]: Running KinFu without texture extraction
[Info] [Freenect2Impl] enumerating devices...
[Info] [Freenect2Impl] 8 usb devices connected
[Info] [Freenect2Impl] found valid Kinect v2 @2:7 with serial 021468743547
[Info] [Freenect2Impl] found 1 devices
[ INFO] [1490777425.854566514]: [Kinect2Bridge::initDevice] Kinect2 devices found: 
[ INFO] [1490777425.854672038]: [Kinect2Bridge::initDevice]   0: 021468743547 (selected)
[Info] [Freenect2DeviceImpl] opening...
[Info] [Freenect2DeviceImpl] transfer pool sizes rgb: 20*16384 ir: 60*8*33792
[Info] [Freenect2DeviceImpl] opened
[ INFO] [1490777425.950207106]: [Kinect2Bridge::initDevice] starting kinect2
[Info] [Freenect2DeviceImpl] starting...
[ INFO] [1490777426.054441689]: kinfu_output: main thread started.
[ INFO] [1490777426.058729688]: --- CURRENT SETTINGS ---

[ INFO] [1490777426.058765963]: Volume size is set to 3.00 meters

[ INFO] [1490777426.058794028]: Volume will shift when the camera target point is farther than 1.50 meters from the volume center

[ INFO] [1490777426.058832852]: The target point is located at [0, 0, 1.80] in camera coordinates

[ INFO] [1490777426.058858207]: ------------------------

in reset function!
[Info] [Freenect2DeviceImpl] submitting rgb transfers...
[Info] [Freenect2DeviceImpl] submitting depth transfers...
[Info] [Freenect2DeviceImpl] started
[ INFO] [1490777426.298858877]: [Kinect2Bridge::initDevice] device serial: 021468743547
[ INFO] [1490777426.298887253]: [Kinect2Bridge::initDevice] device firmware: 4.0.3911.0
[Info] [Freenect2DeviceImpl] stopping...
[Info] [Freenect2DeviceImpl] canceling rgb transfers...
[Info] [Freenect2DeviceImpl] canceling depth transfers...
[Info] [Freenect2DeviceImpl] stopped
[ INFO] [1490777426.575424627]: [DepthRegistration::New] Using CPU registration method!
[ INFO] [1490777426.575515952]: [DepthRegistration::New] Using CPU registration method!
[ INFO] [1490777426.641613736]: [Kinect2Bridge::main] waiting for clients to connect
[ INFO] [1490777426.753208011]: [Kinect2Bridge::callbackStatus] client connected. starting device...
[Info] [Freenect2DeviceImpl] starting...
[Info] [Freenect2DeviceImpl] submitting depth transfers...
[Info] [Freenect2DeviceImpl] started
[Info] [DepthPacketStreamParser] -11 packets were lost
[Info] [DepthPacketStreamParser] 7 packets were lost
[ INFO] [1490777429.869191387]: [Kinect2Bridge::main] depth processing: ~3.73527ms (~267.718Hz) publishing rate: ~27.9454Hz

How to get pointcloud2? "Readme" tell that kinfu_voxelgrid_conversions could convert the std_msgs/Float32MultiArray to pointCloud2. But I find that the main thread no such topic "/kinfu_voxelgrid_to_collision_map/input" published.

RMonica · Answer 9 · Thu Mar 30 2017 15:28:06 GMT+0800 (China Standard Time)

Hi QinZiwen.

Please do not pollute other unrelated issues with your problems.
I'm closing this issue, as requested.

Please read more carefully the KINFU section in the README.
You can extract the point cloud by sending requests, either through the action-based interface or the message-based one. You don't need kinfu_voxelgrid_conversions.