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 (61.10 x 39.00 x 2.70 m), 0.100000 m res.
[ WARN] [1392965933.977203542]: Frame ID of "initialpose" () is different from the global frame /map
[ INFO] [1392965933.977417430]: Pose reset around mean (0.000000 0.000000 0.320000)
[ WARN] [1392965933.983373819]: Failed to subtract base to odom transform, will not publish pose estimate: Frame id /odom does not exist! Frames (1):
[ INFO] [1392965933.983481518]: Initialization of 50 particles took 0.00628427s (=0.000125685s/particle)
[ INFO] [1392965933.991816628]: NaoLocalization initialized with 50 particles.
[ WARN] [1392965934.152648624, 1392354124.958635337]: Message from [/stereo_odometer] has a non-fully-qualified frame_id [camera]. Resolved locally to [/camera]. This is will likely not work in multi-robot systems. This message will only print once.
[ INFO] [1392965935.092718408, 1392354125.526207845]: PointCloudGroundFiltering done. Added 248 non-ground points and 14 ground points (from 18). Cloud size is 262
[ INFO] [1392965935.092938381, 1392354125.526207845]: OdomTransformTemporal took 0.00014625s
ERROR: Raycasting in direction (0,0,0) is not possible!ERROR: Raycasting in direction (0,0,0) is not possible!ERROR: Raycasting in direction (0,0,0) is not possible!
There are three warnings in the console output, the second one could be critical:
[ WARN] [1392965933.983373819]: Failed to subtract base to odom transform, will not publish pose estimate: Frame id /odom does not exist! Frames (1):
The error is created in humanoid_localization/src/HumanoidLocalization.cpp. The warning is printed once at the beginning of launching the node. I assume the warning is printed because baseToMapTFis not available while launching the node and therefore odomToMapTF cannot be calculated.
I checked the TF tree using tf view_frames and visualized the frames in RViz. It turned out to be fine. So the warning mentioned above does not seem to be critical...
Originally posted by RalphA on ROS Answers with karma: 36 on 2014-02-19
Post score: 0
Original comments
Comment by AHornung on 2014-02-20:
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.
Comment by AHornung on 2014-02-20:
The see the point clouds that will be used as measurements, you can subscribe to the topic humanoid_localization/filtered_cloud