TF2

.gitignore

@@ -0,0 +1 @@
+.vscode/*

CMakeLists.txt

@@ -11,7 +11,9 @@ link_directories(${BFL_LIBRARY_DIRS})
 find_package(catkin REQUIRED
         COMPONENTS
             roscpp
-            tf
+            tf2
+            tf2_ros
+            tf2_geometry_msgs
             nav_msgs
             std_msgs
             geometry_msgs
@@ -50,15 +52,18 @@ include_directories(
     )
 
 add_executable(robot_pose_ekf 
-                       src/odom_estimation.cpp 
-                       src/nonlinearanalyticconditionalgaussianodo.cpp 
-                       src/odom_estimation_node.cpp)
+                src/odom_estimation.cpp 
+                src/nonlinearanalyticconditionalgaussianodo.cpp 
+                src/odom_estimation_node.cpp
+)
 target_link_libraries(robot_pose_ekf
     ${catkin_LIBRARIES}
     ${Boost_LIBRARIES}
     ${BFL_LIBRARIES}
     )
-add_dependencies(robot_pose_ekf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(robot_pose_ekf 
+    ${${PROJECT_NAME}_EXPORTED_TARGETS} 
+    ${catkin_EXPORTED_TARGETS})
 
 install(
     TARGETS

include/robot_pose_ekf/odom_estimation.h

@@ -45,14 +45,21 @@
 #include <bfl/pdf/linearanalyticconditionalgaussian.h>
 #include "nonlinearanalyticconditionalgaussianodo.h"
 
-// TF
-#include <tf/tf.h>
+// TF2
+#include <tf2/transform_datatypes.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 // msgs
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
 
 // log files
 #include <fstream>
+#include <memory>
 
 namespace estimation
 {
@@ -61,7 +68,9 @@ class OdomEstimation
 {
 public:
   /// constructor
-  OdomEstimation();
+  OdomEstimation(
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener);
 
   /// destructor
   virtual ~OdomEstimation();
@@ -81,7 +90,7 @@ class OdomEstimation
    * \param prior the prior robot pose
    * \param time the initial time of the ekf
    */
-  void initialize(const tf::Transform& prior, const ros::Time& time);
+  void initialize(const tf2::Transform& prior, const ros::Time& time);
 
   /** check if the filter is initialized
    * returns true if the ekf has been initialized already
@@ -97,13 +106,13 @@ class OdomEstimation
    * \param time the time of the filter posterior
    * \param estimate the filter posterior as a tf transform
    */
-  void getEstimate(ros::Time time, tf::Transform& estimate);
+  void getEstimate(ros::Time time, tf2::Transform& estimate);
 
   /** get the filter posterior
    * \param time the time of the filter posterior
    * \param estimate the filter posterior as a stamped tf transform
    */
-  void getEstimate(ros::Time time, tf::StampedTransform& estimate);
+  void getEstimate(ros::Time time, geometry_msgs::TransformStamped& estimate);
 
   /** get the filter posterior
    * \param estimate the filter posterior as a pose with covariance
@@ -113,32 +122,30 @@ class OdomEstimation
   /** Add a sensor measurement to the measurement buffer
    * \param meas the measurement to add
    */
-  void addMeasurement(const tf::StampedTransform& meas);
+  void addMeasurement(const geometry_msgs::TransformStamped& meas);
 
   /** Add a sensor measurement to the measurement buffer
    * \param meas the measurement to add
    * \param covar the 6x6 covariance matrix of this measurement, as defined in the PoseWithCovariance message
    */
-  void addMeasurement(const tf::StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar);
-
-  /** set the output frame used by tf
-   * \param output_frame the desired output frame published on /tf (e.g., odom_combined)
-   */
-  void setOutputFrame(const std::string& output_frame);
+  void addMeasurement(const geometry_msgs::TransformStamped& meas, const MatrixWrapper::SymmetricMatrix& covar);
 
   /** set the base_footprint frame of the robot used by tf
    * \param base_frame the desired base frame from which to transform when publishing the combined odometry frame (e.g., base_footprint)
    */
   void setBaseFootprintFrame(const std::string& base_frame);
 
+  void setOutputFrame(const std::string& output_frame);
+
+
 private:
   /// correct for angle overflow
   void angleOverflowCorrect(double& a, double ref);
 
   // decompose Transform into x,y,z,Rx,Ry,Rz
-  void decomposeTransform(const tf::StampedTransform& trans,
+  void decomposeTransform(const geometry_msgs::TransformStamped& trans,
 			  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
-  void decomposeTransform(const tf::Transform& trans,
+  void decomposeTransform(const tf2::Transform& trans,
 			  double& x, double& y, double&z, double&Rx, double& Ry, double& Rz);
 
 
@@ -159,20 +166,33 @@ class OdomEstimation
 
   // vars
   MatrixWrapper::ColumnVector vel_desi_, filter_estimate_old_vec_;
-  tf::Transform filter_estimate_old_;
-  tf::StampedTransform odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_;
+  tf2::Transform filter_estimate_old_;
+  geometry_msgs::TransformStamped odom_meas_, odom_meas_old_, imu_meas_, imu_meas_old_, vo_meas_, vo_meas_old_, gps_meas_, gps_meas_old_;
   ros::Time filter_time_old_;
   bool filter_initialized_, odom_initialized_, imu_initialized_, vo_initialized_, gps_initialized_;
 
   // diagnostics
   double diagnostics_odom_rot_rel_, diagnostics_imu_rot_rel_;
 
   // tf transformer
-  tf::Transformer transformer_;
-
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // tf2_ros::Buffer tf_buffer_;
+  // tf2_ros::TransformListener tf_listener_;
+
   std::string output_frame_;
   std::string base_footprint_frame_;
 
+
+  std::string hidden_output_frame_ = "hidden_output";
+  std::string hidden_odom_frame_ = "hidden_odom";
+  std::string hidden_imu_frame_ = "hidden_imu";
+  std::string hidden_vo_frame_ = "hidden_vo";
+  std::string hidden_gps_frame_ = "hidden_gps";
+
+
+
 }; // class
 
 }; // namespace

include/robot_pose_ekf/odom_estimation_node.h

@@ -38,9 +38,11 @@
 
 // ros stuff
 #include <ros/ros.h>
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
+
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
 #include "odom_estimation.h"
 #include <robot_pose_ekf/GetStatus.h>
 
@@ -55,6 +57,7 @@
 
 // log files
 #include <fstream>
+#include <memory>
 
 namespace estimation
 {
@@ -110,20 +113,25 @@ class OdomEstimationNode
   ros::ServiceServer state_srv_;
 
   // ekf filter
-  OdomEstimation my_filter_;
+  std::shared_ptr<OdomEstimation> my_filter_;
 
   // estimated robot pose message to send
   geometry_msgs::PoseWithCovarianceStamped  output_; 
 
   // robot state
-  tf::TransformListener    robot_state_;
-  tf::TransformBroadcaster odom_broadcaster_;
+  // tf transformer
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+
+  // tf::TransformListener    robot_state_;
+  // tf::TransformBroadcaster odom_broadcaster_;
 
   // vectors
-  tf::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_;
-  tf::Transform base_vo_init_;
-  tf::Transform base_gps_init_;
-  tf::StampedTransform camera_base_;
+  tf2::Transform odom_meas_, imu_meas_, vo_meas_, gps_meas_;
+  tf2::Transform base_vo_init_;
+  tf2::Transform base_gps_init_;
+  geometry_msgs::TransformStamped camera_base_;
   ros::Time odom_time_, imu_time_, vo_time_, gps_time_;
   ros::Time odom_stamp_, imu_stamp_, vo_stamp_, gps_stamp_, filter_stamp_;
   ros::Time odom_init_stamp_, imu_init_stamp_, vo_init_stamp_, gps_init_stamp_;

package.xml

@@ -21,7 +21,9 @@
     <build_depend>geometry_msgs</build_depend>
     <build_depend>sensor_msgs</build_depend>
     <build_depend>nav_msgs</build_depend>
-    <build_depend>tf</build_depend>
+    <build_depend>tf2</build_depend>
+    <build_depend>tf2_ros</build_depend>
+    <build_depend>tf2_geometry_msgs</build_depend>
     <build_depend>message_generation</build_depend>
 
     <exec_depend>message_runtime</exec_depend>
@@ -32,7 +34,9 @@
     <exec_depend>geometry_msgs</exec_depend>
     <exec_depend>sensor_msgs</exec_depend>
     <exec_depend>nav_msgs</exec_depend>
-    <exec_depend>tf</exec_depend>
+    <exec_depend>tf2</exec_depend>
+    <exec_depend>tf2_ros</exec_depend>
+    <exec_depend>tf2_geometry_msgs</exec_depend>
 
     <test_depend>rosbag</test_depend>
     <test_depend>rostest</test_depend>