From 221bd86a3c003bed7c356d3d1f9c72aaaa380cb6 Mon Sep 17 00:00:00 2001
From: Michael Boulet <boulet@ll.mit.edu>
Date: Tue, 2 Feb 2016 10:17:09 -0500
Subject: [PATCH] add (optional) tf publisher to vesc_to_odom

---
 vesc_ackermann/CMakeLists.txt                 | 13 ++++++++++-
 .../include/vesc_ackermann/vesc_to_odom.h     |  4 ++++
 vesc_ackermann/package.xml                    |  4 ++++
 vesc_ackermann/src/vesc_to_odom.cpp           | 23 ++++++++++++++++++-
 4 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt
index fc1a45a..5d99968 100644
--- a/vesc_ackermann/CMakeLists.txt
+++ b/vesc_ackermann/CMakeLists.txt
@@ -7,13 +7,24 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   nav_msgs
   std_msgs
+  geometry_msgs
+  tf
   ackermann_msgs
   vesc_msgs
 )
 
 catkin_package(
   INCLUDE_DIRS include
-  CATKIN_DEPENDS nodelet pluginlib roscpp nav_msgs std_msgs ackermann_msgs vesc_msgs
+  CATKIN_DEPENDS
+    nodelet
+    pluginlib
+    roscpp
+    nav_msgs
+    std_msgs
+    geometry_msgs
+    tf
+    ackermann_msgs
+    vesc_msgs
 )
 
 ###########
diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
index 66a23ec..7866a5a 100644
--- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
+++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h
@@ -6,6 +6,8 @@
 #include <ros/ros.h>
 #include <vesc_msgs/VescStateStamped.h>
 #include <std_msgs/Float64.h>
+#include <boost/shared_ptr.hpp>
+#include <tf/transform_broadcaster.h>
 
 namespace vesc_ackermann
 {
@@ -26,6 +28,7 @@ class VescToOdom
   double speed_to_erpm_gain_, speed_to_erpm_offset_;
   double steering_to_servo_gain_, steering_to_servo_offset_;
   double wheelbase_;
+  bool publish_tf_;
 
   // odometry state
   double x_, y_, yaw_;
@@ -36,6 +39,7 @@ class VescToOdom
   ros::Publisher odom_pub_;
   ros::Subscriber vesc_state_sub_;
   ros::Subscriber servo_sub_;
+  boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;
 
   // ROS callbacks
   void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);
diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml
index 0c9ea6c..ae97b77 100644
--- a/vesc_ackermann/package.xml
+++ b/vesc_ackermann/package.xml
@@ -21,6 +21,8 @@
   <build_depend>roscpp</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
   <build_depend>ackermann_msgs</build_depend>
   <build_depend>vesc_msgs</build_depend>
 
@@ -29,6 +31,8 @@
   <run_depend>roscpp</run_depend>
   <run_depend>nav_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf</run_depend>
   <run_depend>ackermann_msgs</run_depend>
   <run_depend>vesc_msgs</run_depend>
 
diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp
index 9fceab4..f7fd86e 100644
--- a/vesc_ackermann/src/vesc_to_odom.cpp
+++ b/vesc_ackermann/src/vesc_to_odom.cpp
@@ -5,6 +5,7 @@
 #include <cmath>
 
 #include <nav_msgs/Odometry.h>
+#include <geometry_msgs/TransformStamped.h>
 
 namespace vesc_ackermann
 {
@@ -14,7 +15,7 @@ inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& val
 
 VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
   odom_frame_("odom"), base_frame_("base_link"),
-  use_servo_cmd_(true), x_(0.0), y_(0.0), yaw_(0.0)
+  use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0)
 {
   // get ROS parameters
   private_nh.param("odom_frame", odom_frame_, odom_frame_);
@@ -32,10 +33,16 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
     if (!getRequiredParam(nh, "wheelbase", wheelbase_))
       return;
   }
+  private_nh.param("publish_tf", publish_tf_, publish_tf_);
 
   // create odom publisher
   odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 10);
 
+  // create tf broadcaster
+  if (publish_tf_) {
+    tf_pub_.reset(new tf::TransformBroadcaster);
+  }
+
   // subscribe to vesc state and. optionally, servo command
   vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this);
   if (use_servo_cmd_) {
@@ -106,6 +113,20 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr&
   // Velocity uncertainty
   /** @todo Think about velocity uncertainty */
 
+  if (publish_tf_) {
+    geometry_msgs::TransformStamped tf;
+    tf.header.frame_id = odom_frame_;
+    tf.child_frame_id = base_frame_;
+    tf.header.stamp = ros::Time::now();
+    tf.transform.translation.x = x_;
+    tf.transform.translation.y = y_;
+    tf.transform.translation.z = 0.0;
+    tf.transform.rotation = odom->pose.pose.orientation;
+    if (ros::ok()) {
+      tf_pub_->sendTransform(tf);
+    }
+  }
+
   if (ros::ok()) {
     odom_pub_.publish(odom);
   }