From 582dc20522486a307f702c733d277010d068e1df Mon Sep 17 00:00:00 2001
From: niklastkl <niklas.trekel@tuhh.de>
Date: Mon, 22 Jan 2024 12:00:45 +0100
Subject: [PATCH] check availability of pose information when requesting
 movement to start

---
 fav/src/scenario/scenario_node.cpp | 8 ++++++++
 fav/src/scenario/scenario_node.hpp | 1 +
 2 files changed, 9 insertions(+)

diff --git a/fav/src/scenario/scenario_node.cpp b/fav/src/scenario/scenario_node.cpp
index b5f14ca..16c4fbd 100644
--- a/fav/src/scenario/scenario_node.cpp
+++ b/fav/src/scenario/scenario_node.cpp
@@ -114,6 +114,11 @@ void ScenarioNode::ServeStart(
     response->message = "Already running.";
     return;
   }
+  if (!initial_position_information_received_){
+    response->success = false;
+    response->message = "Current vehicle position still unknown.";
+    return;
+  }
   Reset();
   running_ = true;
   using hippo_common::convert::EigenToRos;
@@ -300,6 +305,9 @@ void ScenarioNode::OnPose(
   using hippo_common::convert::RosToEigen;
   RosToEigen(_msg->pose.pose.position, vehicle_position_);
   RosToEigen(_msg->pose.pose.orientation, vehicle_orientation_);
+  if (!initial_position_information_received_){
+    initial_position_information_received_ = true;
+  }
 
   UpdateViewpointProgress();
   viewpoint_index_.current = FindViewpointInTolerance();
diff --git a/fav/src/scenario/scenario_node.hpp b/fav/src/scenario/scenario_node.hpp
index 1ae10f9..97918ff 100644
--- a/fav/src/scenario/scenario_node.hpp
+++ b/fav/src/scenario/scenario_node.hpp
@@ -66,6 +66,7 @@ class ScenarioNode : public rclcpp::Node {
   bool running_{false};
   bool start_position_reached_{false};
   bool viewpoints_finished_{false};
+  bool initial_position_information_received_{false};
   Eigen::Vector3d vehicle_position_;
   Eigen::Quaterniond vehicle_orientation_;
   ViewpointIndex viewpoint_index_;