SpeedTracker: implemented hover service

ctu-mrs · Jun 20, 2024 · 2ddfdd4 · 2ddfdd4
1 parent b7b70e9
commit 2ddfdd4
Showing 1 changed file with 19 additions and 1 deletion.
diff --git a/src/speed_tracker/speed_tracker.cpp b/src/speed_tracker/speed_tracker.cpp
@@ -472,7 +472,25 @@ const std_srvs::TriggerResponse::ConstPtr SpeedTracker::switchOdometrySource([[m
 
 const std_srvs::TriggerResponse::ConstPtr SpeedTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
 
-  return std_srvs::TriggerResponse::Ptr();
+  ROS_WARN("[SpeedTracker]: initiating hover");
+
+  getting_cmd_ = false;
+
+  mrs_msgs::String srv;
+  srv.request.value = _backup_tracker_;
+
+  switch_tracker_future_ = sch_switch_tracker_.callAsync(srv);
+
+  switch_tracker_called_ = true;
+
+  std::stringstream ss;
+  ss << "initiating hover";
+
+  std_srvs::TriggerResponse res;
+  res.success = true;
+  res.message = ss.str();
+
+  return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
 }
 
 //}