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_;