Publishing Point Cloud Weights

After some confusion with ROS messages and C++ float/double and array/vector properties, I've finally put the code together that publishes the weights of each particle in the particle filter as the robot drives around.  I had to use a custom ROS message type I had originally created for the filter's covariance, because for some reason the ROS weights message I made was not being copied to the proper dependency folder in the build process.  I suspect this is related to the CMakeLists file getting blown away.

The code that I wrote is just a few lines added to amcl_node.cpp:

cloud_weight_pub_ = nh_.advertise<filter_covariance_msg>("cloud_weight",2,true);
...
 if (!m_force_update) {
      geometry_msgs::PoseArray cloud_msg;
     
      filter_covariance_msg weights_msg;
      std::vector<float> weights(set->sample_count);
      cloud_msg.header.stamp = ros::Time::now();
      cloud_msg.header.frame_id = global_frame_id_;
      cloud_msg.poses.resize(set->sample_count);
      for(int i=0;i<set->sample_count;i++)
      {
        tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                                 tf::Vector3(set->samples[i].pose.v[0],
                                           set->samples[i].pose.v[1], 0)),
                        cloud_msg.poses[i]);
  weights[i] = set->samples[i].weight;
 
      }
      particlecloud_pub_.publish(cloud_msg);
      weights_msg.cov = weights;     
      cloud_weight_pub_.publish(weights_msg);
    }
  }


And the result is that there's now a /cloud_weights topic published by my AMCL overlay with data that looks like this:

From here, I'm going to run trials and collect data. There are a few new details I'm going to incorporate:

-Data routes should be 5 minutes long
-Drive with teleop and record/playback the teleop commands to replicate the exact route
-Script anything I can (for reproducibility)
-For every route, try the same route but with kidnapping occurrences at different times (locations) to see what happens.

The ultimate direction is that I'm going to add this particle weight data into my Matlab print-outs and color the particles by weight. I'm crossing my fingers that this will reveal something.