Reset start time when vehicle is not in offboard mode

Jaeyoung-Lim · Aug 23, 2022 · 8b3fff0 · 8b3fff0
1 parent a690bd5
commit 8b3fff0
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
diff --git 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,13 +79,15 @@ 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_;
   ros::Time start_time_, curr_time_;
 
   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
@@ -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_);