From a8d240eba2bfd6e579affd7c5af798d0127b9f90 Mon Sep 17 00:00:00 2001 From: connor wool Date: Thu, 22 Feb 2018 00:44:05 -0800 Subject: [PATCH 1/3] Created Kalman Filter velocity publisher --- src/localization/localization.cpp | 8 ++++++++ src/localization/localization_system.cpp | 14 ++++++++++++++ src/localization/localization_system.hpp | 3 ++- src/localization/robosub_sensors.h | 2 +- 4 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/localization/localization.cpp b/src/localization/localization.cpp index 7c59b4f8..b9e38105 100644 --- a/src/localization/localization.cpp +++ b/src/localization/localization.cpp @@ -13,6 +13,13 @@ int main(int argc, char **argv) ros::Publisher loc_point_pub = nh.advertise("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 + ("localization/debug/kf/linVel", 1); + // Set up pose publisher. This is necessary for visualizing in rviz. ros::Publisher pose_pub = nh.advertise("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(); } diff --git a/src/localization/localization_system.cpp b/src/localization/localization_system.cpp index 825a8103..36a4c2b4 100644 --- a/src/localization/localization_system.cpp +++ b/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; diff --git a/src/localization/localization_system.hpp b/src/localization/localization_system.hpp index 3c97df63..a8f674b8 100644 --- a/src/localization/localization_system.hpp +++ b/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 diff --git a/src/localization/robosub_sensors.h b/src/localization/robosub_sensors.h index b93caeef..04d217dd 100644 --- a/src/localization/robosub_sensors.h +++ b/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 From 8eccf01d07d61644d6ec51e73f405a8c61dd6069 Mon Sep 17 00:00:00 2001 From: connor wool Date: Thu, 22 Feb 2018 15:47:16 -0800 Subject: [PATCH 2/3] Created testing directory for localization system, integrated with Cmake --- src/localization/CMakeLists.txt | 5 +++++ src/localization/tests/CMakeLists.txt | 6 ++++++ .../tests/localizationSystem.test | 6 ++++++ .../tests/test_localization_system.cpp | 21 +++++++++++++++++++ 4 files changed, 38 insertions(+) create mode 100644 src/localization/tests/CMakeLists.txt create mode 100644 src/localization/tests/localizationSystem.test create mode 100644 src/localization/tests/test_localization_system.cpp diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 59bc6aa4..6f8b0f42 100644 --- a/src/localization/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/src/localization/tests/CMakeLists.txt b/src/localization/tests/CMakeLists.txt new file mode 100644 index 00000000..af5bf718 --- /dev/null +++ b/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}) + diff --git a/src/localization/tests/localizationSystem.test b/src/localization/tests/localizationSystem.test new file mode 100644 index 00000000..bd4882e7 --- /dev/null +++ b/src/localization/tests/localizationSystem.test @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/src/localization/tests/test_localization_system.cpp b/src/localization/tests/test_localization_system.cpp new file mode 100644 index 00000000..8f5a7a6d --- /dev/null +++ b/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 +#include +#include + +//Project includes +#include "ros/ros.h" +#include "../particle_filter.h" +#include "../lin_accel_kalman_filter.h" +#include "../robosub_sensors.h" + +TEST(ParticleFilter, HandlesGettersAndSetters) +{ + EXPECT_TRUE(true) << "This trivial test should never fail."; +} From cf21d8a4ea0c2a309a1e9a8ffc3265e30509cbfb Mon Sep 17 00:00:00 2001 From: connor wool Date: Mon, 26 Feb 2018 15:55:59 -0800 Subject: [PATCH 3/3] Created directory for localization system testing code --- src/localization/tests/test_localization_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/localization/tests/test_localization_system.cpp b/src/localization/tests/test_localization_system.cpp index 8f5a7a6d..af40c3ad 100644 --- a/src/localization/tests/test_localization_system.cpp +++ b/src/localization/tests/test_localization_system.cpp @@ -15,7 +15,7 @@ and integration tests. #include "../lin_accel_kalman_filter.h" #include "../robosub_sensors.h" -TEST(ParticleFilter, HandlesGettersAndSetters) +TEST(ParticleFilter, VerifyTestingSuite) { EXPECT_TRUE(true) << "This trivial test should never fail."; }