Merge pull request #62 from pascalau/feature/tf2

Migration to tf2

Author: rikba

package.xml

@@ -21,7 +21,9 @@
 
   <!-- Dependencies needed after this package is compiled. -->
   <depend>roscpp</depend>
-  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>vrpn_catkin</depend>

src/ros_vrpn_client.cpp

@@ -48,10 +48,10 @@
 #include <vrpn_Connection.h>
 #include <vrpn_Tracker.h>
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <glog/logging.h>
 
 #include "vicon_odometry_estimator.h"
@@ -155,7 +155,7 @@ class Rigid_Body {
   ros::Publisher measured_target_transform_pub_;
   ros::Publisher estimated_target_transform_pub_;
   ros::Publisher estimated_target_odometry_pub_;
-  tf::TransformBroadcaster br;
+  tf2_ros::TransformBroadcaster br;
   // Vprn object pointers
   vrpn_Connection* connection;
   vrpn_Tracker_Remote* tracker;
@@ -319,33 +319,29 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) {
   target_state->measured_transform.header.stamp = timestamp;
   target_state->measured_transform.header.frame_id = coordinate_system_string;
   target_state->measured_transform.child_frame_id = object_name;
-  tf::vectorEigenToMsg(position_measured_W,
+  tf2::toMsg(position_measured_W,
                        target_state->measured_transform.transform.translation);
-  tf::quaternionEigenToMsg(orientation_measured_B_W,
-                           target_state->measured_transform.transform.rotation);
+  target_state->measured_transform.transform.rotation = tf2::toMsg(orientation_measured_B_W);
 
   // Populate the estimated transform message. Published in main loop.
   target_state->estimated_transform.header.stamp = timestamp;
   target_state->estimated_transform.header.frame_id = coordinate_system_string;
   target_state->estimated_transform.child_frame_id = object_name;
-  tf::vectorEigenToMsg(position_estimate_W,
+  tf2::toMsg(position_estimate_W,
                        target_state->estimated_transform.transform.translation);
-  tf::quaternionEigenToMsg(
-      orientation_estimate_B_W,
-      target_state->estimated_transform.transform.rotation);
+  target_state->estimated_transform.transform.rotation = tf2::toMsg(
+      orientation_estimate_B_W);
 
   // Populate the estimated odometry message. Published in main loop.
   target_state->estimated_odometry.header.stamp = timestamp;
   target_state->estimated_odometry.header.frame_id = coordinate_system_string;
   target_state->estimated_odometry.child_frame_id = object_name;
-  tf::pointEigenToMsg(position_estimate_W,
-                      target_state->estimated_odometry.pose.pose.position);
-  tf::quaternionEigenToMsg(
-      orientation_estimate_B_W,
-      target_state->estimated_odometry.pose.pose.orientation);
-  tf::vectorEigenToMsg(velocity_estimate_B,
+  target_state->estimated_odometry.pose.pose.position = tf2::toMsg(position_estimate_W);
+  target_state->estimated_odometry.pose.pose.orientation = tf2::toMsg(
+      orientation_estimate_B_W);
+  tf2::toMsg(velocity_estimate_B,
                        target_state->estimated_odometry.twist.twist.linear);
-  tf::vectorEigenToMsg(rate_estimate_B,
+  tf2::toMsg(rate_estimate_B,
                        target_state->estimated_odometry.twist.twist.angular);
 
   // Indicating to the main loop the data is ready for publishing.

src/vicon_estimation_node.cpp

@@ -31,7 +31,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
 #include <Eigen/Geometry>
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 #include "vicon_odometry_estimator.h"
 
@@ -63,8 +63,8 @@ class ViconDataListener {
     // Extracting the relavent data from the message
     Eigen::Vector3d position_measured_W;
     Eigen::Quaterniond orientation_measured_B_W;
-    tf::vectorMsgToEigen(msg->transform.translation, position_measured_W);
-    tf::quaternionMsgToEigen(msg->transform.rotation, orientation_measured_B_W);
+    tf2::fromMsg(msg->transform.translation, position_measured_W);
+    tf2::fromMsg(msg->transform.rotation, orientation_measured_B_W);
     ros::Time timestamp = msg->header.stamp;
     // Passing the received data to the estimator
     vicon_odometry_estimator_->updateEstimate(position_measured_W,
@@ -85,21 +85,18 @@ class ViconDataListener {
     // Creating estimated transform message
     geometry_msgs::TransformStamped estimated_transform;
     estimated_transform.header = msg->header;
-    tf::vectorEigenToMsg(position_estimate_W,
+    tf2::toMsg(position_estimate_W,
                          estimated_transform.transform.translation);
-    tf::quaternionEigenToMsg(orientation_estimate_B_W,
-                             estimated_transform.transform.rotation);
+    estimated_transform.transform.rotation = tf2::toMsg(orientation_estimate_B_W);
     // Creating estimated odometry message
     nav_msgs::Odometry estimated_odometry;
     estimated_odometry.header = msg->header;
     estimated_odometry.child_frame_id = object_name_;
-    tf::pointEigenToMsg(position_estimate_W,
-                        estimated_odometry.pose.pose.position);
-    tf::quaternionEigenToMsg(orientation_estimate_B_W,
-                             estimated_odometry.pose.pose.orientation);
-    tf::vectorEigenToMsg(velocity_estimate_B,
+    estimated_odometry.pose.pose.position = tf2::toMsg(position_estimate_W);
+    estimated_odometry.pose.pose.orientation = tf2::toMsg(orientation_estimate_B_W);
+    tf2::toMsg(velocity_estimate_B,
                          estimated_odometry.twist.twist.linear);
-    tf::vectorEigenToMsg(rate_estimate_B,
+    tf2::toMsg(rate_estimate_B,
                          estimated_odometry.twist.twist.angular);
     // Publishing the estimates
     estimated_transform_pub_.publish(estimated_transform);

src/wrapper/vicon_odometry_estimator.cpp

@@ -19,7 +19,7 @@
  * limitations under the License.
  */
 
-#include <eigen_conversions/eigen_msg.h>
+#include <tf2_eigen/tf2_eigen.h>
 
 #include "vicon_odometry_estimator.h"
 
@@ -121,30 +121,27 @@ void ViconOdometryEstimator::publishIntermediateResults(ros::Time timestamp) {
   msg.header.stamp = timestamp;
 
   // Writing the measurement to the message object
-  tf::vectorEigenToMsg(translational_estimator_results.position_measured_,
+  tf2::toMsg(translational_estimator_results.position_measured_,
                        msg.pos_measured);
   // Writing the old estimates to the message object
-  tf::vectorEigenToMsg(translational_estimator_results.position_old_,
+  tf2::toMsg(translational_estimator_results.position_old_,
                        msg.pos_old);
-  tf::vectorEigenToMsg(translational_estimator_results.velocity_old_,
+  tf2::toMsg(translational_estimator_results.velocity_old_,
                        msg.vel_old);
   // Posteriori results
-  tf::vectorEigenToMsg(translational_estimator_results.position_estimate_,
+  tf2::toMsg(translational_estimator_results.position_estimate_,
                        msg.pos_est);
-  tf::vectorEigenToMsg(translational_estimator_results.velocity_estimate_,
+  tf2::toMsg(translational_estimator_results.velocity_estimate_,
                        msg.vel_est);
 
   // Writing the measurement to the message object
-  tf::quaternionEigenToMsg(rotational_estimator_results.orientation_measured_,
-                           msg.quat_measured);
+  msg.quat_measured = tf2::toMsg(rotational_estimator_results.orientation_measured_);
   // Writing the old estimates to the message object
-  tf::quaternionEigenToMsg(rotational_estimator_results.orientation_old_,
-                           msg.quat_old);
-  tf::vectorEigenToMsg(rotational_estimator_results.rate_old_, msg.omega_old);
+  msg.quat_old = tf2::toMsg(rotational_estimator_results.orientation_old_);
+  tf2::toMsg(rotational_estimator_results.rate_old_, msg.omega_old);
   // Posteriori results
-  tf::quaternionEigenToMsg(rotational_estimator_results.orientation_estimate_,
-                           msg.quat_est);
-  tf::vectorEigenToMsg(rotational_estimator_results.rate_estimate_,
+  msg.quat_est = tf2::toMsg(rotational_estimator_results.orientation_estimate_);
+  tf2::toMsg(rotational_estimator_results.rate_estimate_,
                        msg.omega_est);
 
   // Data to do with the orientation measurement outlier detection