diff --git a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp b/ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp index b96a2c18..d5cd00aa 100644 --- a/ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp +++ b/ros_ws/src/robot/autonomy/planning/global/behavior_executive/src/behavior_executive.cpp @@ -207,8 +207,28 @@ void BehaviorExecutive::timer_callback(){ else land_action->set_failure(); } - else - land_action->set_failure(); + } + + if(pause_action->is_active()){ + pause_action->set_running(); + if(pause_action->active_has_changed()){ + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::PAUSE; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + mode_result.wait(); + } + } + + if(rewind_action->is_active()){ + rewind_action->set_running(); + if(rewind_action->active_has_changed()){ + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::REWIND; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + mode_result.wait(); + } } // follow fixed trajectory action