# How to use humanoid_localization with stereo cameras?

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="global_frame_id" value="/map" />
<param name="num_particles" value="2000" />
<param name="use_raycasting" value="true" />
<param name="use_imu" value="false" />
</node>


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 ...
edit retag close merge delete

Yes, points directly in the origin will be problematic! There is a filtering for min range in the code, but maybe that's not working properly in your case.

( 2014-02-20 21:23:13 -0600 )edit

The see the point clouds that will be used as measurements, you can subscribe to the topic humanoid_localization/filtered_cloud

( 2014-02-20 21:25:03 -0600 )edit

Sort by » oldest newest most voted

I managed to get rid of the issue.

I was using the following parameter setting:

<param name="base_frame_id" value="/base_link" />


By setting the base_frame_id to any other frame not coinciding with /base_link, the issue could be fixed.

For my system, the /base_link lies on the map ground level. However, the ground level does not seem to be the issue. I initialized the system with /base_link lying above and below ground level and I was still getting the error

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


It seems like another property of /base_link was causing the issue.

more

You should be able to use humanoid_localization code with point clouds without converting them to a laser scan (which will also be better). We've used the code as it is published for the paper you mention above, although with an Asus Xtion sensor. So you should first ensure your point clouds are all in order (no nans, invalid values or similar). The callback pointCloudCallback is then called, using the functions prepareGeneralPointCloud and localizeWithMeasurement. So laserCallbackshould not be used at all.

The error you describe should never happen: you're trying to raycast to an endpoint in direction "0,0,0" in the map, that could be a result of broken TF frames or broken values in the point cloud.

Otherwise you can trace what's happening e.g. with rxconsole setting the log level to "debug". Set num_particles to something small so you don't get thousands of errors in that case.

more