fixed time stamp issue in visual tracker

Author: wumanu

source/dbrt/tracker/visual_tracker_node.cpp

@@ -116,13 +116,12 @@ int main(int argc, char** argv)
     while (ros::ok())
     {
         visualization_rate.sleep();
-        auto current_state = tracker_ros.current_state();
+        State state;
+        ros::Time time;
+        tracker_ros.get_current_state(state, time);
 
-        // std::cout << "PUBLISHING ESTIMATED JONT ANGLES AND TF WITH"
-        //              "NOW() TIMESTAMP. THIS HAS TO BE FIXED!"
-        //           << std::endl;
-        ros::Time time = ros::Time::now();
-        tracker_publisher->publish_tf(current_state, time);
+        state[2] = 3;
+        tracker_publisher->publish_tf(state, time);
 
         ros::spinOnce();
     }

source/dbrt/tracker/visual_tracker_ros.cpp

@@ -38,6 +38,7 @@ void VisualTrackerRos::track(const sensor_msgs::Image& ros_image)
         ros_image, camera_data_->downsampling_factor());
 
     current_state_ = tracker_->track(image);
+    current_time_ = ros_image.header.stamp;
     // current_pose_.pose = ri::to_ros_pose(current_state_);
     // current_pose_.header.stamp = ros_image.header.stamp;
     // current_pose_.header.frame_id= ros_image.header.frame_id;
@@ -87,8 +88,10 @@ bool VisualTrackerRos::process()
     return true;
 }
 
-auto VisualTrackerRos::current_state() const -> const State&
+void VisualTrackerRos::get_current_state(State& state, ros::Time& time) const
 {
-    return current_state_;
+    state = current_state_;
+    time = current_time_;
 }
+
 }

source/dbrt/tracker/visual_tracker_ros.h

@@ -22,6 +22,7 @@
 #include <memory>
 #include <mutex>
 #include <dbrt/tracker/visual_tracker.h>
+#include <ros/time.h>
 
 namespace dbrt
 {
@@ -57,13 +58,14 @@ class VisualTrackerRos
      */
     void update_obsrv(const sensor_msgs::Image& ros_image);
 
-    const State& current_state() const;
+    void get_current_state(State& state, ros::Time& time) const;
     const std::shared_ptr<VisualTracker>& tracker() { return tracker_; }
 
 protected:
     bool obsrv_updated_;
     bool running_;
     State current_state_;
+    ros::Time current_time_;
     sensor_msgs::Image current_ros_image_;
     std::mutex obsrv_mutex_;
     std::mutex state_mutex_;