Internal changes

project_guideline/audio/BUILD

@@ -151,6 +151,8 @@ cc_library(
         "//project_guideline/audio/assets:legacy_v4_2_warning_embed",
         "//project_guideline/audio/assets:legacy_warning_embed",
         "//project_guideline/audio/assets:obstacle_embed",
+        "//project_guideline/audio/assets:turn_embed",
+        "//project_guideline/audio/assets:turn_fast_embed",
         "//project_guideline/audio/assets:warning_pitch_shift_2x_embed",
         "//project_guideline/environment:control_signal",
         "//project_guideline/logging:guideline_logger",

project_guideline/audio/assets/BUILD

@@ -45,6 +45,8 @@ ALL_OGG_ASSETS = [
     "steering_on_course_impulse_tick",
     "stopstopstop",
     "straight",
+    "turn",
+    "turn_fast",
     "warning_pitch_shift_2x",
 ]
 

project_guideline/audio/legacy_sound_pack.cc

@@ -14,6 +14,7 @@
 
 #include "project_guideline/audio/legacy_sound_pack.h"
 
+#include <cmath>
 #include <complex>
 #include <cstdlib>
 #include <memory>
@@ -33,6 +34,8 @@
 #include "project_guideline/audio/assets/legacy_v4_2_warning_embed.h"
 #include "project_guideline/audio/assets/legacy_warning_embed.h"
 #include "project_guideline/audio/assets/obstacle_embed.h"
+#include "project_guideline/audio/assets/turn_embed.h"
+#include "project_guideline/audio/assets/turn_fast_embed.h"
 #include "project_guideline/audio/assets/warning_pitch_shift_2x_embed.h"
 #include "project_guideline/audio/sound_player.h"
 #include "project_guideline/logging/guideline_logger.h"
@@ -63,6 +66,15 @@ static PanningStrategy LinearThresholdPan(float threshold) {
   };
 }
 
+static PanningStrategy LegacyThresholdPan(float threshold, bool reversed) {
+  return [threshold, reversed](float input, float& left, float& right) {
+    float l = util::ClampedLerp<float>(input, threshold, 1, 0, 1);
+    float r = util::ClampedLerp<float>(input, -1, -threshold, 1, 0);
+    left = reversed ? r : l;
+    right = reversed ? l : r;
+  };
+}
+
 float ApplyShaping(float x, float sensitivity_curve) {
   if (sensitivity_curve == 0) {
     return x;
@@ -151,12 +163,26 @@ absl::Status LegacySoundPack::Initialize() {
   CHECK(steering_sound_toc);
   CHECK(warning_sound_toc);
 
+  const util::EmbeddedFileToc* curve_sound_toc = nullptr;
+  if (options_.use_fast_curve_sound()) {
+    curve_sound_toc = turn_fast_embed_create();
+  } else {
+    curve_sound_toc = turn_embed_create();
+  }
+  curve_panner_ = LegacyThresholdPan(options_.turn_sensitivity(), false);
+  curve_rate_strategy_ = [](float input) { return 1; };
+
+  CHECK(curve_sound_toc);
+
   GL_ASSIGN_OR_RETURN(steering_sound_,
                       sound_player_->LoadStereoSound(steering_sound_toc));
   sound_player_->SetLoop(steering_sound_, true);
   GL_ASSIGN_OR_RETURN(warning_sound_,
                       sound_player_->LoadStereoSound(warning_sound_toc));
   sound_player_->SetLoop(warning_sound_, true);
+  GL_ASSIGN_OR_RETURN(curve_sound_,
+                      sound_player_->LoadStereoSound(curve_sound_toc));
+  sound_player_->SetLoop(curve_sound_, true);
   GL_ASSIGN_OR_RETURN(obstacle_sound_,
                       sound_player_->LoadStereoSound(obstacle_embed_create()));
   sound_player_->SetLoop(obstacle_sound_, true);
@@ -209,6 +235,21 @@ void LegacySoundPack::OnControlSignal(
   sound_player_->SetPlaybackRate(warning_sound_,
                                  warning_rate_strategy_(warning_position));
   sound_player_->Play(warning_sound_);
+
+  float curve_panning_position = 0;
+  curve_panning_position = util::ClampedLerp<float>(
+      signal.rotation_movement_ahead_degrees, -options_.max_rotation_degrees(),
+      options_.max_rotation_degrees(), -1, 1);
+  float curve_left_volume = 0;
+  float curve_right_volume = 0;
+  curve_panning_position =
+      ApplyShaping(curve_panning_position, options_.sensitivity_curvature());
+  curve_panner_(curve_panning_position, curve_left_volume, curve_right_volume);
+  sound_player_->SetStereoVolume(curve_sound_, curve_left_volume,
+                                 curve_right_volume);
+  sound_player_->SetPlaybackRate(curve_sound_,
+                                 curve_rate_strategy_(curve_panning_position));
+  sound_player_->Play(curve_sound_);
 }
 
 std::optional<const util::EmbeddedFileToc*>

project_guideline/audio/legacy_sound_pack.h

@@ -63,11 +63,14 @@ class LegacySoundPack : public SoundPack {
 
   PanningStrategy steering_panner_ = nullptr;
   PanningStrategy warning_panner_ = nullptr;
+  PanningStrategy curve_panner_ = nullptr;
   RateStrategy steering_rate_strategy_ = nullptr;
   RateStrategy warning_rate_strategy_ = nullptr;
+  RateStrategy curve_rate_strategy_ = nullptr;
 
   SoundId steering_sound_;
   SoundId warning_sound_;
+  SoundId curve_sound_;
   SoundId obstacle_sound_;
 };
 

project_guideline/environment/BUILD

@@ -56,15 +56,14 @@ cc_library(
         "//visibility:public",
     ],
     deps = [
+        ":control_signal",
+        ":environment",
+        ":obstacle",
+        ":path_planning",
         "//project_guideline/camera:camera_model",
         "//project_guideline/depth:depth_align_ransac",
         "//project_guideline/depth:point_cloud_util",
-        "//project_guideline/environment",
-        "//project_guideline/environment:control_signal",
-        "//project_guideline/environment:obstacle",
-        "//project_guideline/environment:path_planning",
         "//project_guideline/logging:guideline_logger",
-        "//project_guideline/motion:motion_tracker",
         "//project_guideline/motion:tracking_feature",
         "//project_guideline/util:geometry",
         "//project_guideline/util:hit_test_util",
@@ -73,8 +72,6 @@ cc_library(
         "//third_party/protobuf:time_util",
         "@com_google_absl//absl/log",
         "@com_google_absl//absl/log:check",
-        "@com_google_absl//absl/memory",
-        "@com_google_absl//absl/status",
         "@com_google_absl//absl/status:statusor",
         "@com_google_absl//absl/time",
         "@eigen_archive//:eigen3",
@@ -277,8 +274,8 @@ cc_library(
         "//visibility:public",
     ],
     deps = [
+        ":control_signal",
         ":obstacle",
-        "//project_guideline/environment:control_signal",
         "//project_guideline/proto:guideline_engine_config_cc_proto",
         "//project_guideline/util:geometry",
         "//project_guideline/util:windowed_value_latch",

project_guideline/environment/control_signal.h

@@ -30,6 +30,7 @@ namespace guideline::environment {
 // movement negative is clockwise and positive is counterclockwise.
 struct ControlSignal {
   bool stop = false;
+  int stop_reason = 0;
 
   // Lateral distance tangential from the line.
   float lateral_movement_meters = 0.;
@@ -38,6 +39,10 @@ struct ControlSignal {
   // upcoming line.
   float rotation_movement_degrees = 0.;
 
+  // Desired user rotational movement based on user position, direction of
+  // upcoming line and ahead distance omitted.
+  float rotation_movement_ahead_degrees = 0.;
+
   // World coordinates of an upcoming turn point, if any.
   std::optional<Eigen::Vector3d> turn_point = std::nullopt;
 

project_guideline/environment/guidance_system.cc

@@ -139,7 +139,7 @@ GuidanceSystem::GuidanceSystem(const GuidanceSystemOptions& options,
       control_system_(std::move(control_system)) {}
 
 absl::Status GuidanceSystem::Stop() {
-  ResetAndSendStopSignal();
+  ResetAndSendStopSignal(1);
 
   return absl::OkStatus();
 }
@@ -260,7 +260,7 @@ void GuidanceSystem::OnTrackingStateChanged(const bool is_tracking) {
   // OnCameraPose will start being invoked again with from a new reference
   // point.
   if (!is_tracking) {
-    ResetAndSendStopSignal();
+    ResetAndSendStopSignal(2);
   }
 }
 
@@ -372,7 +372,7 @@ void GuidanceSystem::OnDetection(
                                   first_empty_keypoints_timestamp_us_) >=
                eager_stop_threshold) {
       first_empty_keypoints_timestamp_us_ = 0;
-      ResetAndSendStopSignal();
+      ResetAndSendStopSignal(3);
     }
   }
 }
@@ -445,7 +445,7 @@ void GuidanceSystem::ProcessDepthMap(int64_t timestamp_us,
   LogPointCloud(timestamp_us, *environment_, *logger_);
 }
 
-void GuidanceSystem::ResetAndSendStopSignal() {
+void GuidanceSystem::ResetAndSendStopSignal(int reason) {
   {
     absl::MutexLock lock(&pending_camera_poses_mutex_);
     pending_detection_poses_.clear();
@@ -457,6 +457,7 @@ void GuidanceSystem::ResetAndSendStopSignal() {
   // Generate a stop signal and notify callbacks.
   ControlSignal stop_signal;
   stop_signal.stop = true;
+  stop_signal.stop_reason = reason;
 
   std::vector<ControlSignalCallback> callbacks;
   {

project_guideline/environment/guidance_system.h

@@ -103,7 +103,7 @@ class GuidanceSystem {
                        const util::DepthImage& depth_map,
                        const PendingFeatures& features);
 
-  void ResetAndSendStopSignal();
+  void ResetAndSendStopSignal(int reason);
 
   std::shared_ptr<camera::CameraModel> GetCameraModel()
       ABSL_LOCKS_EXCLUDED(camera_model_mutex_);

project_guideline/environment/path_planning.cc

@@ -280,6 +280,21 @@ RotationMovementOutput SimpleControlSystem::ComputeRotationalMovement(
   return RotationMovementOutput(rotation_movement_degrees, all_rotations,
                                 num_guideline_points_per_segment);
 }
+RotationMovementOutput
+SimpleControlSystem::ComputeRotationalMovementWithLookAhead(
+    const Vector3d& human_direction,
+    absl::Span<const Vector3d> guideline_points,
+    int closest_guideline_point_indx, float rotational_movement_ahead_meter,
+    const Axis3 vertical_axis) {
+  auto indices_skip =
+      static_cast<size_t>(rotational_movement_ahead_meter *
+                          options_.num_guideline_points_per_meter());
+  auto closest_guideline_point_indx_offset =
+      closest_guideline_point_indx + indices_skip;
+  return ComputeRotationalMovement(human_direction, guideline_points,
+                                   closest_guideline_point_indx_offset,
+                                   vertical_axis);
+}
 
 TurnPointOutput SimpleControlSystem::FindTurnPoint(
     const Vector3d& human_position, absl::Span<const Vector3d> guideline_points,
@@ -314,14 +329,14 @@ TurnPointOutput SimpleControlSystem::FindTurnPoint(
   return TurnPointOutput(turn_point, turn_angle, turn_point_distance_meters);
 }
 
-bool SimpleControlSystem::IsStopCondition() {
+int SimpleControlSystem::IsStopCondition() {
   {
     absl::MutexLock lock(&control_signal_history_lock_);
 
     auto current_control_signal = control_signal_history_.back();
     // Out of guideline scenario.
     if (std::isnan(current_control_signal.lateral_movement_meters)) {
-      return true;
+      return 4;
     }
 
     // Deviation from the guideline scenario.
@@ -351,19 +366,21 @@ bool SimpleControlSystem::IsStopCondition() {
         }
       }
       if (!has_in_range_movement) {
-        return true;
+        return 5;
       }
     }
-    return false;
+    return 0;
   }
 }
 
 // Post processes and returns final control signal to be communicated to the
 // human.
 const ControlSignal SimpleControlSystem::PostProcessControlSignals() {
-  if (IsStopCondition()) {
+  int stop_condition = IsStopCondition();
+  if (stop_condition != 0) {
     ControlSignal stop_signal;
     stop_signal.stop = true;
+    stop_signal.stop_reason = stop_condition;
     obstacle_latch_.Reset();
     return stop_signal;
   }
@@ -447,6 +464,10 @@ ControlSignal SimpleControlSystem::GenerateControlSignal(
     auto rotational_movement = ComputeRotationalMovement(
         human_direction, guideline_points,
         lateral_movement.closest_guideline_point_indx, vertical_axis);
+    auto rotational_movement_ahead = ComputeRotationalMovementWithLookAhead(
+        human_direction, guideline_points,
+        lateral_movement.closest_guideline_point_indx,
+        options_.rotational_movement_ahead_meters(), vertical_axis);
     auto turn_point = FindTurnPoint(
         human_position, guideline_points,
         lateral_movement.closest_guideline_point_indx,
@@ -458,6 +479,8 @@ ControlSignal SimpleControlSystem::GenerateControlSignal(
         lateral_movement.lateral_movement_meters;
     current_control_signal.rotation_movement_degrees =
         rotational_movement.rotation_movement_degrees;
+    current_control_signal.rotation_movement_ahead_degrees =
+        rotational_movement_ahead.rotation_movement_degrees;
     current_control_signal.turn_point = turn_point.turn_point;
     current_control_signal.turn_angle_degrees = turn_point.turn_angle_degrees;
     current_control_signal.turn_point_distance_meters =

project_guideline/environment/path_planning.h

@@ -137,6 +137,11 @@ class SimpleControlSystem : public ControlSystem {
       const ::Eigen::Vector3d& human_direction,
       absl::Span<const ::Eigen::Vector3d> guideline_points,
       int closest_guideline_point_indx, util::Axis3 vertical_axis);
+  RotationMovementOutput ComputeRotationalMovementWithLookAhead(
+      const ::Eigen::Vector3d& human_direction,
+      absl::Span<const ::Eigen::Vector3d> guideline_points,
+      int closest_guideline_point_indx, float rotational_movement_ahead_meter,
+      util::Axis3 vertical_axis);
   TurnPointOutput FindTurnPoint(
       const ::Eigen::Vector3d& human_position,
       absl::Span<const ::Eigen::Vector3d> guideline_points,
@@ -150,7 +155,7 @@ class SimpleControlSystem : public ControlSystem {
         obstacle_latch_(/*value_threshold=*/0.5f,
                         options.obstacle_smoothing_window_frames(),
                         options.obstacle_smoothing_min_interval_frames()) {}
-  bool IsStopCondition();
+  int IsStopCondition();
   const ControlSignal PostProcessControlSignals();
   SimpleControlSystemOptions options_;
   absl::Mutex control_signal_history_lock_;

project_guideline/logging/BUILD

@@ -80,3 +80,36 @@ cc_library(
         "@com_google_absl//absl/status",
     ],
 )
+
+cc_library(
+    name = "debug_signal",
+    hdrs = ["debug_signal.h"],
+    visibility = ["//visibility:public"],
+)
+
+cc_library(
+    name = "debug_signal_provider",
+    srcs = ["debug_signal_provider.cc"],
+    hdrs = ["debug_signal_provider.h"],
+    visibility = [
+        "//visibility:public",
+    ],
+    deps = [
+        ":debug_signal",
+        "//project_guideline/camera:camera_model",
+        "//project_guideline/environment:control_signal",
+        "//project_guideline/motion:tracking_feature",
+        "//project_guideline/proto:guideline_engine_config_cc_proto",
+        "//project_guideline/util:image",
+        "//project_guideline/util:transformation",
+        "@com_google_absl//absl/functional:bind_front",
+        "@com_google_absl//absl/log",
+        "@com_google_absl//absl/log:check",
+        "@com_google_absl//absl/memory",
+        "@com_google_absl//absl/status",
+        "@com_google_absl//absl/status:statusor",
+        "@com_google_absl//absl/synchronization",
+        "@com_google_absl//absl/time",
+        "@eigen_archive//:eigen3",
+    ],
+)

project_guideline/logging/debug_signal.h

@@ -0,0 +1,31 @@
+// Copyright 2023 Google LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//    https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_
+#define PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_
+
+#include <cstdint>
+namespace guideline::logging {
+struct DebugSignal {
+  bool tracking;
+  int64_t camera_pose_fps;
+  int64_t camera_pose_lagging_frame;
+  int64_t detection_fps;
+  int64_t detection_lagging_frame;
+  int64_t tracking_features_fps;
+  int64_t tracking_features_lagging_frame;
+};
+}  // namespace guideline::logging
+
+#endif  // PROJECT_GUIDELINE_LOGGING_DEBUG_SIGNAL_H_