Merge pull request #10 from ctu-mrs/status

Added a `state` field to the `TrackerStatus` message which is now filled out by the trackers
ctu-mrs · May 9, 2024 · ace6362 · ace6362
2 parents d3bf135 + 4cf4bc6
commit ace6362
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 6 deletions.
diff --git a/src/landoff_tracker/landoff_tracker.cpp b/src/landoff_tracker/landoff_tracker.cpp
@@ -56,7 +56,7 @@ typedef enum
 
 } States_t;
 
-const char* state_names[7] = {
+const std::array<const char*, 7> state_names = {
 
     "IDLING", "LANDED", "STOPPING_MOTION", "HOVERING", "ACCELERATING", "DECELERATING", "STOPPING"};
 
@@ -551,8 +551,17 @@ const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
   tracker_status.active            = is_active_;
   tracker_status.callbacks_enabled = callbacks_enabled_;
 
-  bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
-  bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+  const bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+  const bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+
+  if (idling)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+  else if (taking_off_)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_TAKEOFF;
+  else if (hovering)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+  else if (landing_)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_LAND;
 
   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
 
@@ -820,7 +829,7 @@ void LandoffTracker::changeStateHorizontal(States_t new_state) {
     }
   }
 
-  ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+  ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names.at(previous_state_horizontal_), state_names.at(current_state_horizontal_));
 }
 
 //}
@@ -844,7 +853,7 @@ void LandoffTracker::changeStateVertical(States_t new_state) {
     }
   }
 
-  ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+  ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names.at(previous_state_vertical_), state_names.at(current_state_vertical_));
 }
 
 //}

diff --git a/src/line_tracker/line_tracker.cpp b/src/line_tracker/line_tracker.cpp
@@ -579,7 +579,12 @@ const mrs_msgs::TrackerStatus LineTracker::getStatus() {
   tracker_status.active            = is_active_;
   tracker_status.callbacks_enabled = callbacks_enabled_;
 
-  bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+  const bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+
+  if (idling)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+  else
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
 
   tracker_status.have_goal = !idling;
 

diff --git a/src/mpc_tracker/mpc_tracker.cpp b/src/mpc_tracker/mpc_tracker.cpp
@@ -1139,6 +1139,16 @@ const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
 
   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
 
+  if (!is_active_)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+  else if (tracker_status.tracking_trajectory)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_TRAJECTORY;
+  else if (tracker_status.have_goal)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+  else
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+
+
   int trajectory_tracking_idx = getCurrentTrajectoryIdx();
 
   tracker_status.trajectory_length = trajectory_size;

diff --git a/src/speed_tracker/speed_tracker.cpp b/src/speed_tracker/speed_tracker.cpp
@@ -370,6 +370,11 @@ const mrs_msgs::TrackerStatus SpeedTracker::getStatus() {
   tracker_status.active            = is_active_;
   tracker_status.callbacks_enabled = callbacks_enabled_;
 
+  if (!tracker_status.active || first_iteration_)
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+  else
+    tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+
   return tracker_status;
 }