Connor wool/update localization code documentation

src/localization/localization.cpp

@@ -5,61 +5,108 @@ using namespace Eigen;
 
 int main(int argc, char **argv)
 {
+    /* Initializes a new ROS node for the current process, and gives it the 
+    name "localization". This name must be unique across the whole ROS system */
     ros::init(argc, argv, "localization");
 
+    /* This call starts the ROS node that was created in the last step, and also
+    provides access to the services that the ROS node provides, like advertising
+    and subscribing to topics */
     ros::NodeHandle nh;
 
-    //set up a regular position publisher as a point
+    /* Creates a publisher which advertises the "position/point" topic. This
+    will be used to share our estimated position with the other ROS 
+    subsystems */
     ros::Publisher loc_point_pub =
     nh.advertise<geometry_msgs::PointStamped>("position/point", 1);
 
-    // Set up pose publisher. This is necessary for visualizing in rviz.
+    /* Set up pose publisher. This is necessary for visualizing in rviz. */
     ros::Publisher pose_pub =
         nh.advertise<geometry_msgs::PoseStamped>("rviz/cobalt/pose", 1);
 
-    // Wait for ros::Time to initialize and return good values.
+    /* Wait for ros::Time to initialize. Ros::Time::isValid will return true
+    if the time is non-zero. Helps with full-system synchronization. */
     while(!ros::Time::isValid())
     {
         usleep(10000);
     }
 
+    /* Creates instances of the RobosubSensors and LocalizationSystem classes.
+    RobosubSensors is designed to handle and sanitize input from outside topics,
+    and the LocalizationSystem class manages the particle filter and kalman
+    filter classes */
     RobosubSensors sensors;
     LocalizationSystem loc_system(nh, sensors);
 
+    /* This provides system-wide access to a Remote Procedure Call (RPC) that
+    will allow the filters to be reset to their initial configurations. This
+    could be useful if the control system detects an error and requires a full
+    system reset. */
     ros::ServiceServer reset_filter_service =
         nh.advertiseService("localization/reset",
                 &LocalizationSystem::ResetFilterCallback, &loc_system);
 
-    // RobosubSensors class handles all sensor callbacks
+    /* Create subscribers to the topics we want to consume to power the
+    kalman filters and particle filters. These subscribers take 4 arguments. 
+    First, a string with the topic name to subscribe to, then an int to define 
+    the subscriber's queue size, next a function pointer for the function we 
+    would like to call when new data arrives, and last a reference to the object
+    who's method we should call. */
+
+    //subscribe to depth sensor updates
     ros::Subscriber depth_sub = nh.subscribe("depth", 1,
             &RobosubSensors::InputDepth, &sensors);
+
+    //subscribe to hydrophone pinger bearing updates
     ros::Subscriber hydrophones_bearing_sub =
         nh.subscribe("hydrophones/bearing", 1,
                 &RobosubSensors::InputHydrophones, &sensors);
+
+    //subscribe to linear acceleration updates from the IMU
     ros::Subscriber accel_sub = nh.subscribe("acceleration/linear", 1,
             &RobosubSensors::InputRelLinAcl, &sensors);
+
+    //subscribe to orientation updates from the IMU
     ros::Subscriber orientation_sub = nh.subscribe("orientation", 1,
             &RobosubSensors::InputOrientation, &sensors);
 
-    // Load main loop update rate
+    /* Setting the rate defines the shortest amount of time a single loop should
+    take to run. This is used later via r.sleep() to make sure we run below this
+    rate. There is no guarantee that we will reach the specified rate. */
     double rate;
     ROS_ERROR_COND(!ros::param::getCached("rate/localization",
             rate), "Failed to load localization rate.");
     ros::Rate r(rate);
 
-    //this loop calls localization_system to run the code
+    /* checks for node shutdown. When node is shutdown, ros::ok() will return
+    false. This is required because the process running this code is tied to,
+    but is not, the ROS Node itself. By making this check, our code will stop
+    executing when the node is shutdown, either by ourselves or by an external
+    command.*/
     while(ros::ok())
     {
+        /* Calling ros::spinOnce fires all the callbacks that are pending from
+        updates that have reached subscribers. These callbacks are ONLY handled
+        when ros::spinOnce is called, or if ros::spin is called the callbacks
+        will be handled until the node is shutdown. ros::spinOnce provides more
+        control over the timing of the execution. */
         ros::spinOnce();
 
-        // Run localization system update. This will input sensor data and run
-        // the filters.
+        /* ros::spinOnce handled the topic callbacks to the sensor class, and 
+        the sensor class acts as a buffer for this incoming data. Now, the 
+        update() function makes the kalman filter and particle filter consume 
+        that buffered data, recalculate their internal states, and create a new
+        position estimate. */
         loc_system.Update();
 
-        // Finally, publish our data - both as a pose and the point itself.
+        /* these function calls retrieve data from the localization system that
+        is already properly formatted for the message topics we want to publish.
+        The publishers transparently handle making this data avaliable on their
+        topics to other subsystems. */
         pose_pub.publish(loc_system.GetPoseMessage());
         loc_point_pub.publish(loc_system.GetLocalizationPoint());
 
+        /* This consumes the rest of the time in our current cycle. */
         r.sleep();
     }
 

src/localization/localization_system.hpp

@@ -28,22 +28,32 @@ class LocalizationSystem
     LocalizationSystem(ros::NodeHandle &_nh, RobosubSensors &_sensors);
 
     bool ResetFilterCallback(std_srvs::Empty::Request &req,
-                             std_srvs::Empty::Response &rep);
+        std_srvs::Empty::Response &rep);
 
+    /* pulls position data from the particle filter, as vector message */
     geometry_msgs::Vector3Stamped GetLocalizationMessage();
+
+    /* pulls both position and orientation data from the particle filter, and 
+    provides it as pose message */
     geometry_msgs::PoseStamped GetPoseMessage();
+
+    /* pulls position data from the particle filter, as point message */
     geometry_msgs::PointStamped GetLocalizationPoint();
 
+    /* Checks for new data in sensors class, and pushes this data from the
+    sensors class into the filters. This in turn causes the filters to update
+    their internal state. */
     void Update();
 
 private:
     void publish_tf_message(tf::Vector3 pos);
 
-    // Objects inputted from localization main
+    /* references to the sensors and nodehandle objects created in
+    localization.cpp and passed in via the constructor. */
     RobosubSensors &sensors;
     ros::NodeHandle &nh;
 
-    // Filter objects
+    /* Storage for the kalman filter and particle filter object instances */
     LinAccelKalmanFilter kalman_filter;
     ParticleFilter particle_filter;
 

src/localization/robosub_sensors.cpp

@@ -63,7 +63,7 @@ void RobosubSensors::InputOrientation(const
         geometry_msgs::QuaternionStamped::ConstPtr &msg)
 {
     orientation = tf::Quaternion(msg->quaternion.x, msg->quaternion.y,
-                                 msg->quaternion.z, msg->quaternion.w);
+        msg->quaternion.z, msg->quaternion.w);
     orientation_dt = (msg->header.stamp - last_orientation_time).toSec();
     new_orientation = true;
     last_orientation_time = msg->header.stamp;

src/localization/robosub_sensors.h

@@ -9,18 +9,19 @@
 #include "robosub_msgs/Float32Stamped.h"
 #include "geometry_msgs/Vector3Stamped.h"
 
-// RobosubSensors is responsible for storing and handling all sensor input
-// (both from callbacks and derived info like linear velocity). It provides the
-// following functions for each set of sensor data:
-// Input Data
-// Get Data
-// Is New Data
-// Get DT
-// All data is stored as tf datatypes or primitive datatypes.
-
-// Additionally RobosubSensors provides one other function: Calculating
-// absolute linear acceleration whenever the relative linear acceleration
-// callback occurs.
+/*  RobosubSensors is responsible for storing and handling all sensor input
+    (both from callbacks and derived info like linear velocity). It provides the
+    following functions for each set of sensor data:
+    + Input Data (called via RPC by subscribers in localization.cpp)
+    + Get Data (called from the kalman filter and particle filter)
+    + Is New Data (do we have new data since last access?)
+    + Get DT (time since last data update via callback)
+    All data is stored as tf datatypes or primitive datatypes.
+
+    Additionally, RobosubSensors provides one other function: Calculating
+    absolute linear acceleration whenever the relative linear acceleration
+    callback occurs. */
+
 class RobosubSensors
 {
 public:
@@ -42,7 +43,7 @@ class RobosubSensors
     void InputPosition(const tf::Vector3 pos);
 
     // Getter functions.
-    // These set the new field to false when called.
+    // These set the `new data` field to false when called.
     tf::Vector3 GetRelLinAcl();
     double GetDepth();
     tf::Vector3 GetHydrophones();
@@ -51,7 +52,7 @@ class RobosubSensors
     tf::Vector3 GetAbsLinVel();
     tf::Vector3 GetPosition();
 
-    // Get new status
+    // check if there's new data avaliable
     bool NewRelLinAcl();
     bool NewDepth();
     bool NewHydrophones();
@@ -60,7 +61,7 @@ class RobosubSensors
     bool NewAbsLinVel();
     bool NewPosition();
 
-    // Get DTs
+    // Get time since last update, measured in seconds
     double GetRelLinAclDT();
     double GetDepthDT();
     double GetHydrophonesDT();
@@ -72,8 +73,8 @@ class RobosubSensors
 private:
     void calculate_absolute_lin_accel();
 
-    // Stores whether sensor is new. Set to false when a particular sensors Get
-    // method is called.
+    /* Stores whether sensor is new. Set to false when a particular sensor's Get
+    method is called. */
     bool new_rel_lin_acl;
     bool new_depth;
     bool new_hydrophones;