Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update tutorial activity user interface #39

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions project_guideline/audio/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
2 changes: 2 additions & 0 deletions project_guideline/audio/assets/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ ALL_OGG_ASSETS = [
"steering_on_course_impulse_tick",
"stopstopstop",
"straight",
"turn",
"turn_fast",
"warning_pitch_shift_2x",
]

Expand Down
Binary file added project_guideline/audio/assets/turn.ogg
Binary file not shown.
Binary file added project_guideline/audio/assets/turn_fast.ogg
Binary file not shown.
41 changes: 41 additions & 0 deletions project_guideline/audio/legacy_sound_pack.cc
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include "project_guideline/audio/legacy_sound_pack.h"

#include <cmath>
#include <complex>
#include <cstdlib>
#include <memory>
Expand All @@ -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"
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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*>
Expand Down
3 changes: 3 additions & 0 deletions project_guideline/audio/legacy_sound_pack.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
};

Expand Down
13 changes: 5 additions & 8 deletions project_guideline/environment/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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",
Expand Down Expand Up @@ -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",
Expand Down
5 changes: 5 additions & 0 deletions project_guideline/environment/control_signal.h
Original file line number Diff line number Diff line change
Expand Up @@ -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.;
Expand All @@ -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;

Expand Down
9 changes: 5 additions & 4 deletions project_guideline/environment/guidance_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -372,7 +372,7 @@ void GuidanceSystem::OnDetection(
first_empty_keypoints_timestamp_us_) >=
eager_stop_threshold) {
first_empty_keypoints_timestamp_us_ = 0;
ResetAndSendStopSignal();
ResetAndSendStopSignal(3);
}
}
}
Expand Down Expand Up @@ -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();
Expand All @@ -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;
{
Expand Down
2 changes: 1 addition & 1 deletion project_guideline/environment/guidance_system.h
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down
33 changes: 28 additions & 5 deletions project_guideline/environment/path_planning.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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,
Expand All @@ -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 =
Expand Down
7 changes: 6 additions & 1 deletion project_guideline/environment/path_planning.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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_;
Expand Down
33 changes: 33 additions & 0 deletions project_guideline/logging/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -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",
],
)
31 changes: 31 additions & 0 deletions project_guideline/logging/debug_signal.h
Original file line number Diff line number Diff line change
@@ -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_
Loading