From ffcbfbf87528f21c02a180fc8cced66c4a11db71 Mon Sep 17 00:00:00 2001 From: connor wool Date: Fri, 23 Mar 2018 23:37:01 -0700 Subject: [PATCH 1/4] Wrote comments to explain execution of localization.cpp --- src/localization/localization.cpp | 65 ++++++++++++++++++++++++++----- 1 file changed, 56 insertions(+), 9 deletions(-) diff --git a/src/localization/localization.cpp b/src/localization/localization.cpp index 7c59b4f8..9b1701fc 100644 --- a/src/localization/localization.cpp +++ b/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("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("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(); } From 34be7a99fc3640d8837ec237e1b4322289d97b68 Mon Sep 17 00:00:00 2001 From: connor wool Date: Fri, 23 Mar 2018 23:46:15 -0700 Subject: [PATCH 2/4] made clarifying comments to robosub_sensors.h --- src/localization/robosub_sensors.h | 35 +++++++++++++++--------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/src/localization/robosub_sensors.h b/src/localization/robosub_sensors.h index 4231da01..fccc0f9b 100644 --- a/src/localization/robosub_sensors.h +++ b/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 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; From a9d42e5d33da3106b5c1400c3da2a47a8ee1ae4d Mon Sep 17 00:00:00 2001 From: connor wool Date: Fri, 23 Mar 2018 23:56:01 -0700 Subject: [PATCH 3/4] updated comments and code clarity for robosub_sensors class --- src/localization/robosub_sensors.cpp | 2 +- src/localization/robosub_sensors.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/localization/robosub_sensors.cpp b/src/localization/robosub_sensors.cpp index 2b3d6448..832d852f 100644 --- a/src/localization/robosub_sensors.cpp +++ b/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; diff --git a/src/localization/robosub_sensors.h b/src/localization/robosub_sensors.h index fccc0f9b..e8bf1fe9 100644 --- a/src/localization/robosub_sensors.h +++ b/src/localization/robosub_sensors.h @@ -61,7 +61,7 @@ class RobosubSensors bool NewAbsLinVel(); bool NewPosition(); - // Get time since last update + // Get time since last update, measured in seconds double GetRelLinAclDT(); double GetDepthDT(); double GetHydrophonesDT(); From b820e7e37f2f1c524cfc6214d0d1432b647a30a5 Mon Sep 17 00:00:00 2001 From: connor wool Date: Sat, 24 Mar 2018 00:18:30 -0700 Subject: [PATCH 4/4] added function description comments to localization_system.hpp --- src/localization/localization_system.hpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/localization/localization_system.hpp b/src/localization/localization_system.hpp index 3c97df63..caa9c8cd 100644 --- a/src/localization/localization_system.hpp +++ b/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;