Stereo image proc point cloud is all NaN

edit

I'm trying to extract a point cloud from my stereo cameras using stereo_image_proc. I managed to get two picameras to work simultaneously. I got my disparity image and it looks great. For some reason my point cloud is all NaN's. I used PCL to check if anything was coming through. In rviz, the points are all bunched up at (0, 0, 0). The rgb values of the image are in the point cloud, but the xyz values are either 0 or NaN. I'm using the parameters found in Ubiquity's raspicam node for the picamera calibration because my own calibration resulted in a poor image. I can't see what stereo image proc is doing so I don't know what's going on.

<launch>
<!-- IP camera node -->
<node type="naboris_ip_cam_node" name="naboris_ip_cam_node" pkg="naboris_ip_cam" required="true" output="screen">
    <param name="~camera_info_url" value="package://naboris_ip_cam/camera_info/naboris_ip_cam_410x308.yaml"/>
    <!-- <param name="~camera_info_url" value="package://naboris_ip_cam/camera_info/naboris_ip_cam_720x480.yaml"/> -->

    <param name="~camera_name" value="naboris_stereo/right"/>
    <param name="~fps" value="10.0" type="double"/>
</node>

<!-- Raspi cam nodes -->
<node type="raspicam_node" pkg="raspicam_node" name="raspicam_node" output="screen">
    <param name="camera_info_url" value="package://raspicam_node/camera_info/camerav2_410x308.yaml"/>
    <!-- <param name="camera_info_url" value="package://raspicam_node/camera_info/camerav2_720x480.yaml"/> -->
    <param name="~camera_name" value="naboris_stereo/left"/>
    <param name="~width" value="410"/>
    <param name="~height" value="308"/>
    <!-- <param name="~width" value="720"/>
    <param name="~height" value="480"/> -->

    <param name="~framerate" value="5.0"/>
    <param name="camera_frame_id" value="naboris_left_cam"/>
</node>

<node name="dynamic_reconfigure_load_raspicam" pkg="dynamic_reconfigure" type="dynparam"
    args="load /raspicam_node $(find raspicam_node)/params/raspicam_node_defaults.yaml"/>

<!-- Intermediate stereo node -->
<node type="naboris_stereo_node" name="naboris_stereo_node" pkg="naboris_stereo" required="true" output="screen">
    <param name="~right_cam_sub_topic" value="/naboris_ip_cam/image_raw"/>
    <param name="~left_cam_sub_topic" value="/raspicam_node/image"/>
</node>

<!-- Stereo proc nodes -->
<!-- <node name="dynamic_reconfigure_load_stereo_proc" pkg="dynamic_reconfigure" type="dynparam"
    args="load /naboris_stereo/stereo_image_proc $(find naboris_stereo)/params/stereo_params-410x308.yaml"/> -->
<node name="dynamic_reconfigure_load_stereo_proc" pkg="dynamic_reconfigure" type="dynparam"
    args="load /naboris_stereo/stereo_image_proc $(find naboris_stereo)/params/stereo_params-720x480.yaml"/>

<group ns="/naboris_stereo" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc" output="screen">
        <param name="~approximate_sync" value="True" />
        <param name="~queue_size" value="25" />
    </node>
</group>

<!-- ORB_SLAM2 -->
<!-- <node pkg="ORB_SLAM2" type="Stereo" name="orbslam2" output="screen" required="true"
    args="/home/naboris/Documents/ORB_SLAM2/Vocabulary/ORBvoc.bin /home/naboris/Documents/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Naboris.yaml true">
</node> -->
</launch>