From 8b3fff0327b56c415aa24708bea5f37d76307404 Mon Sep 17 00:00:00 2001
From: Jaeyoung Lim <jalim@ethz.ch>
Date: Mon, 22 Aug 2022 12:33:47 +0200
Subject: [PATCH] Reset start time when vehicle is not in offboard mode

---
 .../include/trajectory_publisher/trajectoryPublisher.h     | 4 ++++
 trajectory_publisher/src/trajectoryPublisher.cpp           | 7 +++++++
 2 files changed, 11 insertions(+)

diff --git a/trajectory_publisher/include/trajectory_publisher/trajectoryPublisher.h b/trajectory_publisher/include/trajectory_publisher/trajectoryPublisher.h
index 334fe02..8aba58d 100644
--- a/trajectory_publisher/include/trajectory_publisher/trajectoryPublisher.h
+++ b/trajectory_publisher/include/trajectory_publisher/trajectoryPublisher.h
@@ -50,6 +50,7 @@
 #include <mavconn/mavlink_dialect.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <mavros_msgs/PositionTarget.h>
+#include <mavros_msgs/State.h>
 #include <nav_msgs/Path.h>
 #include <ros/ros.h>
 #include <std_msgs/Int32.h>
@@ -78,6 +79,7 @@ class trajectoryPublisher {
   ros::Subscriber motionselectorSub_;
   ros::Subscriber mavposeSub_;
   ros::Subscriber mavtwistSub_;
+  ros::Subscriber mavstate_sub_;
   ros::ServiceServer trajtriggerServ_;
   ros::Timer trajloop_timer_;
   ros::Timer refloop_timer_;
@@ -85,6 +87,7 @@ class trajectoryPublisher {
 
   nav_msgs::Path refTrajectory_;
   nav_msgs::Path primTrajectory_;
+  mavros_msgs::State current_state_;
 
   int trajectory_type_;
   Eigen::Vector3d p_targ, v_targ, a_targ;
@@ -121,6 +124,7 @@ class trajectoryPublisher {
   void motionselectorCallback(const std_msgs::Int32& selector);
   void mavposeCallback(const geometry_msgs::PoseStamped& msg);
   void mavtwistCallback(const geometry_msgs::TwistStamped& msg);
+  void mavstateCallback(const mavros_msgs::State::ConstPtr& msg);
 };
 
 #endif
diff --git a/trajectory_publisher/src/trajectoryPublisher.cpp b/trajectory_publisher/src/trajectoryPublisher.cpp
index cf0b4af..b81fb9d 100644
--- a/trajectory_publisher/src/trajectoryPublisher.cpp
+++ b/trajectory_publisher/src/trajectoryPublisher.cpp
@@ -54,6 +54,8 @@ trajectoryPublisher::trajectoryPublisher(const ros::NodeHandle& nh, const ros::N
                               ros::TransportHints().tcpNoDelay());
   mavtwistSub_ = nh_.subscribe("mavros/local_position/velocity", 1, &trajectoryPublisher::mavtwistCallback, this,
                                ros::TransportHints().tcpNoDelay());
+  mavstate_sub_ = nh_.subscribe("mavros/state", 1, &trajectoryPublisher::mavstateCallback, this,
+                                ros::TransportHints().tcpNoDelay());
 
   trajloop_timer_ = nh_.createTimer(ros::Duration(0.1), &trajectoryPublisher::loopCallback, this);
   refloop_timer_ = nh_.createTimer(ros::Duration(0.01), &trajectoryPublisher::refCallback, this);
@@ -109,6 +111,9 @@ trajectoryPublisher::trajectoryPublisher(const ros::NodeHandle& nh, const ros::N
 
 void trajectoryPublisher::updateReference() {
   curr_time_ = ros::Time::now();
+  if (current_state_.mode != "OFFBOARD") {  /// Reset start_time_ when not in offboard
+    start_time_ = ros::Time::now();
+  }
   trigger_time_ = (curr_time_ - start_time_).toSec();
 
   p_targ = motionPrimitives_.at(motion_selector_)->getPosition(trigger_time_);
@@ -216,6 +221,8 @@ void trajectoryPublisher::pubrefSetpointRawGlobal() {
   global_rawreferencePub_.publish(msg);
 }
 
+void trajectoryPublisher::mavstateCallback(const mavros_msgs::State::ConstPtr& msg) { current_state_ = *msg; }
+
 void trajectoryPublisher::loopCallback(const ros::TimerEvent& event) {
   // Slow Loop publishing trajectory information
   pubrefTrajectory(motion_selector_);