Created Kalman Filter velocity publisher

src/localization/CMakeLists.txt

@@ -1,3 +1,4 @@
+#create and add librarires for all files in this system
 add_library(localization_system localization_system.cpp)
 add_dependencies(localization_system ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
@@ -13,5 +14,9 @@ add_dependencies(robosub_sensors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
 add_library(obstacle_map obstacle_map.cpp)
 add_dependencies(obstacle_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+#creates the localization system node
 add_ros_node(localization localization.cpp)
 target_link_libraries(localization localization_system particle_filter lin_accel_kalman_filter robosub_sensors ${catkin_LIBRARIES})
+
+#add the test directory
+add_subdirectory(tests)

src/localization/localization.cpp

@@ -13,6 +13,13 @@ int main(int argc, char **argv)
     ros::Publisher loc_point_pub =
     nh.advertise<geometry_msgs::PointStamped>("position/point", 1);
 
+    /* Establishes a publisher for the Kalman filter internal state.
+    This is to be used for debugging, so that we can understand the internals
+    of the Kalman filter during program execution. */
+    ros::Publisher kalman_filter_velocity_pub =
+        nh.advertise<geometry_msgs::Vector3Stamped>
+        ("localization/debug/kf/linVel", 1);
+
     // Set up pose publisher. This is necessary for visualizing in rviz.
     ros::Publisher pose_pub =
         nh.advertise<geometry_msgs::PoseStamped>("rviz/cobalt/pose", 1);
@@ -59,6 +66,7 @@ int main(int argc, char **argv)
         // Finally, publish our data - both as a pose and the point itself.
         pose_pub.publish(loc_system.GetPoseMessage());
         loc_point_pub.publish(loc_system.GetLocalizationPoint());
+        kalman_filter_velocity_pub.publish(loc_system.GetKFVelocityEstimate());
 
         r.sleep();
     }

src/localization/localization_system.cpp

@@ -34,6 +34,20 @@ geometry_msgs::Vector3Stamped LocalizationSystem::GetLocalizationMessage()
     return msg;
 }
 
+geometry_msgs::Vector3Stamped LocalizationSystem::GetKFVelocityEstimate()
+{
+    geometry_msgs::Vector3Stamped msg;
+
+    tf::Vector3 velocity = kalman_filter.GetAbsLinVel();
+
+    msg.vector.x = velocity[0];
+    msg.vector.y = velocity[1];
+    msg.vector.z = velocity[2];
+    msg.header.stamp = ros::Time::now();
+
+    return msg;
+}
+
 geometry_msgs::PointStamped LocalizationSystem::GetLocalizationPoint()
 {
     geometry_msgs::PointStamped msg;

src/localization/localization_system.hpp

@@ -33,10 +33,11 @@ class LocalizationSystem
     geometry_msgs::Vector3Stamped GetLocalizationMessage();
     geometry_msgs::PoseStamped GetPoseMessage();
     geometry_msgs::PointStamped GetLocalizationPoint();
+    geometry_msgs::Vector3Stamped GetKFVelocityEstimate();
 
     void Update();
 
-private:
+  private:
     void publish_tf_message(tf::Vector3 pos);
 
     // Objects inputted from localization main

src/localization/robosub_sensors.h

@@ -32,7 +32,7 @@ class RobosubSensors
     void InputDepth(const robosub::Float32Stamped::ConstPtr &msg);
     void InputHydrophones(const geometry_msgs::Vector3Stamped::ConstPtr &msg);
     void InputOrientation(const geometry_msgs::QuaternionStamped::ConstPtr
-                          &msg);
+        &msg);
 
     // Inputs for derived data
     // Since we have no stamp for these inputs, DT is calculated using the time

src/localization/tests/CMakeLists.txt

@@ -0,0 +1,6 @@
+add_rostest_gtest(test_localization_system
+    localizationSystem.test
+    test_localization_system.cpp)
+#add_dependencies(test_localization_system)
+target_link_libraries(test_localization_system test_tools ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+

src/localization/tests/localizationSystem.test

@@ -0,0 +1,6 @@
+<launch>
+    <test test-name="test_localization_system" pkg="robosub" type="test_localization_system" time-limit="240.0 />
+
+    <rosparam command="load" file="$(find robosub)/param/cobalt.yaml" />
+
+</launch>

src/localization/tests/test_localization_system.cpp

@@ -0,0 +1,21 @@
+/* 
+file: test_localization_system.cpp
+A file that uses the gtest suite inside of ros to run unit tests
+and integration tests.
+*/
+
+//System includes
+#include <gtest/gtest.h>
+#include <vector>
+#include <string>
+
+//Project includes
+#include "ros/ros.h"
+#include "../particle_filter.h"
+#include "../lin_accel_kalman_filter.h"
+#include "../robosub_sensors.h"
+
+TEST(ParticleFilter, VerifyTestingSuite)
+{
+    EXPECT_TRUE(true) << "This trivial test should never fail.";
+}