AMCL Particle Weights Are Always Equal - Why?
I'm driving the Turtlebot around the turtlebot_world Gazebo environment. I'm printing out the AMCL particle filter poses and the weights assigned to the poses, and when I drive around for a while (as long as 5 minutes), the weights assigned to the poses in the particle filter are all set to the same value (particle weight = 0.00196, for 500 particles, so they sum to 1, of course).
I expected there to be some variety in the particles' weights - some higher, some lower. My theories as to why they're all equal:
1. I could be printing the weights at the wrong point of the AMCL code
2. It's a demo Gazebo world and a demo AMCL map - is the set-up too "perfect" for different hypotheses to result in different weights or something?
Here's a code snippet:
File: AMCL/src/amcl_node.cpp
Function: AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
~Line 1245, Added the following:
for(int i=0;i<set->sample_count;i++) {
ROS_DEBUG("Weight: %f",set->samples[i].weight);
}