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.

Small Victories

It looks like there's a problem with the cloud_weight_msg ROS message that I made a while back in order to publish the particle cloud's particle weights.  Even though the file declares the message type as float64[], the
rosmsg show cloud_weight_msg  prints out the message type as int32[].  Not sure what the deal is.

Fortunately, I had an existing message of type float32[], so I'm using that one for the cloud_weights message declaration.  The only difference is that my cloud weights are stored as doubles, and this float32[] will require them to be converted to floats.  I don't think it will cause a problem, so I should be good to go.

Work Plan:
- Put code in for publishing particle cloud weights tonight.
- Tomorrow: collect a bunch of data, over 5-minute trials and kidnapping events at different timesteps.

Maybe ROS Isn't So Bad

I'm still waiting for the other shoe to drop, but I ran catkin_make in my amcl_overlay workspace, and I was able to successfully compile the code and start the amcl_demo code with the overlaid version.  Then, I went over to the turtlebot plugin workspace, and ran catkin_make, and everything compiled.

I may have been gunshy about changing the code in order to get the particle cloud weights for no reason at all.  Whoops.

Graduate Research Seminar Speaker

Yesterday I had the opportunity to be the speaker for the weekly Graduate Research Seminar at Parks College.  There were about 15 students in attendance, spaced out among all levels and corners of the auditorium of 200 seats.  I spoke for about 40 minutes about my research, and in particular, the steps we had to take to simulate robot fault.  While I was preparing the presentation, I was thinking, "You know, this robot fault thing could really be a paper or something."  Unfortunately, a RosCon conference proposal was not accepted over the summer, and it's made me think either the topic isn't as original as I think, or I didn't write a good proposal.

The most enjoyable part of the talk was the last 2 minutes, when I took questions.  I got questions about automating the tests, whether I'll be able to differentiate between major and minor kidnapping event classes, and how to improve the Roomba's ability to make it around the whole room like it's supposed to.  Getting to free-style and apply my knowledge was fun.