RalphA's profile - activity

Hi,

I started using the humanoid_localization node for localizing my robot using stereo cameras. I give the node a sensor_msgs/PointCloud2 as input, but I don't provide any sensor_msgs/LaserScan. According to the humanoid_localization wiki page, this setup should be fine.

I am able to run the node and the particle filter is working, but for every particle filter update, the following error gets printed on the console hundreds of times:

ERROR: Raycasting in direction (0,0,0) is not possible!

The error message is generated when calling the castRay function in the file humanoid_localization/src/RaycastingModel.cpp.

What am I missing to have the node running without errors? Do I need to create a "fake" LaserScan message from my stereo image data to get rid of the errors? Do I need to change the parameter setting in my launch file?

Reading a paper from the authors of the humanoid_localization algorithms, I found out that they implemented a setup similar to mine using a depth camera. So the code has been used with cameras only, but I could not find out which adaptations were necessary. See:

Maier, D.; Hornung, A.; Bennewitz, M., "Real-time navigation in 3D environments based on depth camera data," Humanoid Robots (Humanoids), 2012 12th IEEE-RAS International Conference on , vol., no., pp.692,697, Nov. 29 2012-Dec. 1 2012

My setup:

• ROS fuerte
• Ubuntu 12.04
• stereo cameras fixed on wheeled robot

Extract of my launch file:

        <!-- Load map from octomap_server -->
    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"
    args="$(find nav)/maps/3D/track3_2014-02-18_image_proc_pc_tallmap.bt" output="screen" >
    </node>

    <!-- Localization -->
    <node pkg="humanoid_localization" type="localization_node" name="humanoid_localization" output="screen" >
        <remap from="point_cloud" to="/stereo_odometer/point_cloud" />      
        <param name="odom_frame_id" value="/odom" />
        <param name="base_frame_id" value="/base_link" />
        <param name="global_frame_id" value="/map" />
        <param name="num_particles" value="2000" />
        <param name="use_raycasting" value="true" />    
        <param name="use_imu" value="false" />
        <param name="base_footprint_id" value="/base_link" />
    </node>

EDIT: (for AHornung's answer)

I was using humanoid_navigation code downloaded from the APT repo ros-fuerte-humanoid-navigation. Now, I downloaded the recent version of humanoid_navigation from GitHub, ran the humanoid_localization node and had the same issues as before.

The point cloud input for the humanoid_localization node has no NaNs. I am not sure what other values could be invalid as input... could points in the origin (0,0,0) cause the error?

The width of my point cloud is 1 and the height is unequal to 1, so it seems to be ordered.

An extract of my console output:

core service [/rosout] found
process[saved_map/octomap_server-1]: started with pid [26692]
[ INFO] [1392965933.527639300]: Publishing latched (single publish will take longer, all topics are prepared)
[ INFO] [1392965933.601608621]: Octomap file /home/aes002/fuerte_workspace/sandbox/trunk/gator/nav/maps/3D/track1_2014-02-18_image_proc_pc.bt loaded (772778 nodes).
process[saved_map/humanoid_localization-2]: started with pid [26763]
[ INFO] [1392965933.779107102]: Requesting the map from /saved_map/humanoid_localization/octomap_binary...
[ INFO] [1392965933.848516881, 1392354124.777533329]: Sending binary map data on service request
process[saved_map/rviz-3]: started with pid [26867]
[ INFO] [1392965933.963886245]: Occupancy map initialized with 772778 nodes ...