<commit_before>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "media/audio/audio_output_controller.h"

#include "base/bind.h"
#include "base/debug/trace_event.h"
#include "base/message_loop.h"
#include "base/synchronization/waitable_event.h"
#include "base/threading/platform_thread.h"
#include "base/threading/thread_restrictions.h"
#include "base/time.h"

using base::Time;
using base::WaitableEvent;

namespace media {

// Signal a pause in low-latency mode.
const int AudioOutputController::kPauseMark = -1;

// Polling-related constants.
const int AudioOutputController::kPollNumAttempts = 3;
const int AudioOutputController::kPollPauseInMilliseconds = 3;

AudioOutputController::AudioOutputController(EventHandler* handler,
                                             SyncReader* sync_reader)
    : handler_(handler),
      stream_(NULL),
      volume_(1.0),
      state_(kEmpty),
      sync_reader_(sync_reader),
      message_loop_(NULL),
      number_polling_attempts_left_(0),
      ALLOW_THIS_IN_INITIALIZER_LIST(weak_this_(this)) {
}

AudioOutputController::~AudioOutputController() {
  DCHECK_EQ(kClosed, state_);
  DCHECK(message_loop_);

  if (!message_loop_.get() || message_loop_->BelongsToCurrentThread()) {
    DoStopCloseAndClearStream(NULL);
  } else {
    // http://crbug.com/120973
    base::ThreadRestrictions::ScopedAllowWait allow_wait;
    WaitableEvent completion(true /* manual reset */,
                             false /* initial state */);
    message_loop_->PostTask(FROM_HERE,
        base::Bind(&AudioOutputController::DoStopCloseAndClearStream,
                   base::Unretained(this),
                   &completion));
    completion.Wait();
  }
}

// static
scoped_refptr<AudioOutputController> AudioOutputController::Create(
    AudioManager* audio_manager,
    EventHandler* event_handler,
    const AudioParameters& params,
    SyncReader* sync_reader) {
  DCHECK(audio_manager);
  DCHECK(sync_reader);

  if (!params.IsValid() || !audio_manager)
    return NULL;

  // Starts the audio controller thread.
  scoped_refptr<AudioOutputController> controller(new AudioOutputController(
      event_handler, sync_reader));

  controller->message_loop_ = audio_manager->GetMessageLoop();
  controller->message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoCreate, controller,
      base::Unretained(audio_manager), params));
  return controller;
}

void AudioOutputController::Play() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoPlay, base::Unretained(this)));
}

void AudioOutputController::Pause() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoPause, base::Unretained(this)));
}

void AudioOutputController::Flush() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoFlush, base::Unretained(this)));
}

void AudioOutputController::Close(const base::Closure& closed_task) {
  DCHECK(!closed_task.is_null());
  DCHECK(message_loop_);
  message_loop_->PostTaskAndReply(FROM_HERE, base::Bind(
      &AudioOutputController::DoClose, base::Unretained(this)), closed_task);
}

void AudioOutputController::SetVolume(double volume) {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoSetVolume, base::Unretained(this), volume));
}

void AudioOutputController::DoCreate(AudioManager* audio_manager,
                                     const AudioParameters& params) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Close() can be called before DoCreate() is executed.
  if (state_ == kClosed)
    return;
  DCHECK_EQ(kEmpty, state_);

  DoStopCloseAndClearStream(NULL);
  stream_ = audio_manager->MakeAudioOutputStreamProxy(params);
  if (!stream_) {
    // TODO(hclam): Define error types.
    handler_->OnError(this, 0);
    return;
  }

  if (!stream_->Open()) {
    DoStopCloseAndClearStream(NULL);

    // TODO(hclam): Define error types.
    handler_->OnError(this, 0);
    return;
  }

  // We have successfully opened the stream. Set the initial volume.
  stream_->SetVolume(volume_);

  // Finally set the state to kCreated.
  state_ = kCreated;

  // And then report we have been created.
  handler_->OnCreated(this);
}

void AudioOutputController::DoPlay() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // We can start from created or paused state.
  if (state_ != kCreated && state_ != kPaused)
    return;

  state_ = kStarting;

  // Ask for first packet.
  sync_reader_->UpdatePendingBytes(0);

  // Cannot start stream immediately, should give renderer some time
  // to deliver data.
  // TODO(vrk): The polling here and in WaitTillDataReady() is pretty clunky.
  // Refine the API such that polling is no longer needed. (crbug.com/112196)
  number_polling_attempts_left_ = kPollNumAttempts;
  message_loop_->PostDelayedTask(
      FROM_HERE,
      base::Bind(&AudioOutputController::PollAndStartIfDataReady,
      weak_this_.GetWeakPtr()),
      base::TimeDelta::FromMilliseconds(kPollPauseInMilliseconds));
}

void AudioOutputController::PollAndStartIfDataReady() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Being paranoid: do nothing if state unexpectedly changed.
  if ((state_ != kStarting) && (state_ != kPausedWhenStarting))
    return;

  bool pausing = (state_ == kPausedWhenStarting);
  // If we are ready to start the stream, start it.
  // Of course we may have to stop it immediately...
  if (--number_polling_attempts_left_ == 0 ||
      pausing ||
      sync_reader_->DataReady()) {
    StartStream();
    if (pausing) {
      DoPause();
    }
  } else {
    message_loop_->PostDelayedTask(
        FROM_HERE,
        base::Bind(&AudioOutputController::PollAndStartIfDataReady,
        weak_this_.GetWeakPtr()),
        base::TimeDelta::FromMilliseconds(kPollPauseInMilliseconds));
  }
}

void AudioOutputController::StartStream() {
  DCHECK(message_loop_->BelongsToCurrentThread());
  state_ = kPlaying;

  // We start the AudioOutputStream lazily.
  stream_->Start(this);

  // Tell the event handler that we are now playing.
  handler_->OnPlaying(this);
}

void AudioOutputController::DoPause() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  if (stream_)
    stream_->Stop();

  switch (state_) {
    case kStarting:
      // We were asked to pause while starting. There is delayed task that will
      // try starting playback, and there is no way to remove that task from the
      // queue. If we stop now that task will be executed anyway.
      // Delay pausing, let delayed task to do pause after it start playback.
      state_ = kPausedWhenStarting;
      break;
    case kPlaying:
      state_ = kPaused;

      // Then we stop the audio device. This is not the perfect solution
      // because it discards all the internal buffer in the audio device.
      // TODO(hclam): Actually pause the audio device.
      stream_->Stop();

      // Send a special pause mark to the low-latency audio thread.
      sync_reader_->UpdatePendingBytes(kPauseMark);

      handler_->OnPaused(this);
      break;
    default:
      return;
  }
}

void AudioOutputController::DoFlush() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // TODO(hclam): Actually flush the audio device.
}

void AudioOutputController::DoClose() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  if (state_ != kClosed) {
    DoStopCloseAndClearStream(NULL);
    sync_reader_->Close();
    state_ = kClosed;
  }
}

void AudioOutputController::DoSetVolume(double volume) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Saves the volume to a member first. We may not be able to set the volume
  // right away but when the stream is created we'll set the volume.
  volume_ = volume;

  switch (state_) {
    case kCreated:
    case kStarting:
    case kPausedWhenStarting:
    case kPlaying:
    case kPaused:
      stream_->SetVolume(volume_);
      break;
    default:
      return;
  }
}

void AudioOutputController::DoReportError(int code) {
  DCHECK(message_loop_->BelongsToCurrentThread());
  if (state_ != kClosed)
    handler_->OnError(this, code);
}

uint32 AudioOutputController::OnMoreData(uint8* dest,
                                         uint32 max_size,
                                         AudioBuffersState buffers_state) {
  TRACE_EVENT0("audio", "AudioOutputController::OnMoreData");

  {
    // Check state and do nothing if we are not playing.
    // We are on the hardware audio thread, so lock is needed.
    base::AutoLock auto_lock(lock_);
    if (state_ != kPlaying) {
      return 0;
    }
  }
  uint32 size = sync_reader_->Read(dest, max_size);
  sync_reader_->UpdatePendingBytes(buffers_state.total_bytes() + size);
  return size;
}

void AudioOutputController::WaitTillDataReady() {
  if (!sync_reader_->DataReady()) {
    // In the different place we use different mechanism to poll, get max
    // polling delay from constants used there.
    const base::TimeDelta kMaxPollingDelay = base::TimeDelta::FromMilliseconds(
        kPollNumAttempts * kPollPauseInMilliseconds);
    Time start_time = Time::Now();
    do {
      base::PlatformThread::Sleep(base::TimeDelta::FromMilliseconds(1));
    } while (!sync_reader_->DataReady() &&
             Time::Now() - start_time < kMaxPollingDelay);
  }
}

void AudioOutputController::OnError(AudioOutputStream* stream, int code) {
  // Handle error on the audio controller thread.
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoReportError, base::Unretained(this), code));
}

void AudioOutputController::DoStopCloseAndClearStream(WaitableEvent *done) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Allow calling unconditionally and bail if we don't have a stream_ to close.
  if (stream_ != NULL) {
    stream_->Stop();
    stream_->Close();
    stream_ = NULL;
    weak_this_.InvalidateWeakPtrs();
  }

  // Should be last in the method, do not touch "this" from here on.
  if (done != NULL)
    done->Signal();
}

}  // namespace media
<commit_msg>Fix race condition caused by r114084.<commit_after>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "media/audio/audio_output_controller.h"

#include "base/bind.h"
#include "base/debug/trace_event.h"
#include "base/message_loop.h"
#include "base/synchronization/waitable_event.h"
#include "base/threading/platform_thread.h"
#include "base/threading/thread_restrictions.h"
#include "base/time.h"

using base::Time;
using base::WaitableEvent;

namespace media {

// Signal a pause in low-latency mode.
const int AudioOutputController::kPauseMark = -1;

// Polling-related constants.
const int AudioOutputController::kPollNumAttempts = 3;
const int AudioOutputController::kPollPauseInMilliseconds = 3;

AudioOutputController::AudioOutputController(EventHandler* handler,
                                             SyncReader* sync_reader)
    : handler_(handler),
      stream_(NULL),
      volume_(1.0),
      state_(kEmpty),
      sync_reader_(sync_reader),
      message_loop_(NULL),
      number_polling_attempts_left_(0),
      ALLOW_THIS_IN_INITIALIZER_LIST(weak_this_(this)) {
}

AudioOutputController::~AudioOutputController() {
  DCHECK_EQ(kClosed, state_);
  DCHECK(message_loop_);

  if (!message_loop_.get() || message_loop_->BelongsToCurrentThread()) {
    DoStopCloseAndClearStream(NULL);
  } else {
    // http://crbug.com/120973
    base::ThreadRestrictions::ScopedAllowWait allow_wait;
    WaitableEvent completion(true /* manual reset */,
                             false /* initial state */);
    message_loop_->PostTask(FROM_HERE,
        base::Bind(&AudioOutputController::DoStopCloseAndClearStream,
                   base::Unretained(this),
                   &completion));
    completion.Wait();
  }
}

// static
scoped_refptr<AudioOutputController> AudioOutputController::Create(
    AudioManager* audio_manager,
    EventHandler* event_handler,
    const AudioParameters& params,
    SyncReader* sync_reader) {
  DCHECK(audio_manager);
  DCHECK(sync_reader);

  if (!params.IsValid() || !audio_manager)
    return NULL;

  // Starts the audio controller thread.
  scoped_refptr<AudioOutputController> controller(new AudioOutputController(
      event_handler, sync_reader));

  controller->message_loop_ = audio_manager->GetMessageLoop();
  controller->message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoCreate, controller,
      base::Unretained(audio_manager), params));
  return controller;
}

void AudioOutputController::Play() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoPlay, this));
}

void AudioOutputController::Pause() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoPause, this));
}

void AudioOutputController::Flush() {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoFlush, this));
}

void AudioOutputController::Close(const base::Closure& closed_task) {
  DCHECK(!closed_task.is_null());
  DCHECK(message_loop_);
  message_loop_->PostTaskAndReply(FROM_HERE, base::Bind(
      &AudioOutputController::DoClose, this), closed_task);
}

void AudioOutputController::SetVolume(double volume) {
  DCHECK(message_loop_);
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoSetVolume, this, volume));
}

void AudioOutputController::DoCreate(AudioManager* audio_manager,
                                     const AudioParameters& params) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Close() can be called before DoCreate() is executed.
  if (state_ == kClosed)
    return;
  DCHECK_EQ(kEmpty, state_);

  DoStopCloseAndClearStream(NULL);
  stream_ = audio_manager->MakeAudioOutputStreamProxy(params);
  if (!stream_) {
    // TODO(hclam): Define error types.
    handler_->OnError(this, 0);
    return;
  }

  if (!stream_->Open()) {
    DoStopCloseAndClearStream(NULL);

    // TODO(hclam): Define error types.
    handler_->OnError(this, 0);
    return;
  }

  // We have successfully opened the stream. Set the initial volume.
  stream_->SetVolume(volume_);

  // Finally set the state to kCreated.
  state_ = kCreated;

  // And then report we have been created.
  handler_->OnCreated(this);
}

void AudioOutputController::DoPlay() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // We can start from created or paused state.
  if (state_ != kCreated && state_ != kPaused)
    return;

  state_ = kStarting;

  // Ask for first packet.
  sync_reader_->UpdatePendingBytes(0);

  // Cannot start stream immediately, should give renderer some time
  // to deliver data.
  // TODO(vrk): The polling here and in WaitTillDataReady() is pretty clunky.
  // Refine the API such that polling is no longer needed. (crbug.com/112196)
  number_polling_attempts_left_ = kPollNumAttempts;
  message_loop_->PostDelayedTask(
      FROM_HERE,
      base::Bind(&AudioOutputController::PollAndStartIfDataReady,
      weak_this_.GetWeakPtr()),
      base::TimeDelta::FromMilliseconds(kPollPauseInMilliseconds));
}

void AudioOutputController::PollAndStartIfDataReady() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Being paranoid: do nothing if state unexpectedly changed.
  if ((state_ != kStarting) && (state_ != kPausedWhenStarting))
    return;

  bool pausing = (state_ == kPausedWhenStarting);
  // If we are ready to start the stream, start it.
  // Of course we may have to stop it immediately...
  if (--number_polling_attempts_left_ == 0 ||
      pausing ||
      sync_reader_->DataReady()) {
    StartStream();
    if (pausing) {
      DoPause();
    }
  } else {
    message_loop_->PostDelayedTask(
        FROM_HERE,
        base::Bind(&AudioOutputController::PollAndStartIfDataReady,
        weak_this_.GetWeakPtr()),
        base::TimeDelta::FromMilliseconds(kPollPauseInMilliseconds));
  }
}

void AudioOutputController::StartStream() {
  DCHECK(message_loop_->BelongsToCurrentThread());
  state_ = kPlaying;

  // We start the AudioOutputStream lazily.
  stream_->Start(this);

  // Tell the event handler that we are now playing.
  handler_->OnPlaying(this);
}

void AudioOutputController::DoPause() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  if (stream_)
    stream_->Stop();

  switch (state_) {
    case kStarting:
      // We were asked to pause while starting. There is delayed task that will
      // try starting playback, and there is no way to remove that task from the
      // queue. If we stop now that task will be executed anyway.
      // Delay pausing, let delayed task to do pause after it start playback.
      state_ = kPausedWhenStarting;
      break;
    case kPlaying:
      state_ = kPaused;

      // Then we stop the audio device. This is not the perfect solution
      // because it discards all the internal buffer in the audio device.
      // TODO(hclam): Actually pause the audio device.
      stream_->Stop();

      // Send a special pause mark to the low-latency audio thread.
      sync_reader_->UpdatePendingBytes(kPauseMark);

      handler_->OnPaused(this);
      break;
    default:
      return;
  }
}

void AudioOutputController::DoFlush() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // TODO(hclam): Actually flush the audio device.
}

void AudioOutputController::DoClose() {
  DCHECK(message_loop_->BelongsToCurrentThread());

  if (state_ != kClosed) {
    DoStopCloseAndClearStream(NULL);
    sync_reader_->Close();
    state_ = kClosed;
  }
}

void AudioOutputController::DoSetVolume(double volume) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Saves the volume to a member first. We may not be able to set the volume
  // right away but when the stream is created we'll set the volume.
  volume_ = volume;

  switch (state_) {
    case kCreated:
    case kStarting:
    case kPausedWhenStarting:
    case kPlaying:
    case kPaused:
      stream_->SetVolume(volume_);
      break;
    default:
      return;
  }
}

void AudioOutputController::DoReportError(int code) {
  DCHECK(message_loop_->BelongsToCurrentThread());
  if (state_ != kClosed)
    handler_->OnError(this, code);
}

uint32 AudioOutputController::OnMoreData(uint8* dest,
                                         uint32 max_size,
                                         AudioBuffersState buffers_state) {
  TRACE_EVENT0("audio", "AudioOutputController::OnMoreData");

  {
    // Check state and do nothing if we are not playing.
    // We are on the hardware audio thread, so lock is needed.
    base::AutoLock auto_lock(lock_);
    if (state_ != kPlaying) {
      return 0;
    }
  }
  uint32 size = sync_reader_->Read(dest, max_size);
  sync_reader_->UpdatePendingBytes(buffers_state.total_bytes() + size);
  return size;
}

void AudioOutputController::WaitTillDataReady() {
  if (!sync_reader_->DataReady()) {
    // In the different place we use different mechanism to poll, get max
    // polling delay from constants used there.
    const base::TimeDelta kMaxPollingDelay = base::TimeDelta::FromMilliseconds(
        kPollNumAttempts * kPollPauseInMilliseconds);
    Time start_time = Time::Now();
    do {
      base::PlatformThread::Sleep(base::TimeDelta::FromMilliseconds(1));
    } while (!sync_reader_->DataReady() &&
             Time::Now() - start_time < kMaxPollingDelay);
  }
}

void AudioOutputController::OnError(AudioOutputStream* stream, int code) {
  // Handle error on the audio controller thread.
  message_loop_->PostTask(FROM_HERE, base::Bind(
      &AudioOutputController::DoReportError, this, code));
}

void AudioOutputController::DoStopCloseAndClearStream(WaitableEvent *done) {
  DCHECK(message_loop_->BelongsToCurrentThread());

  // Allow calling unconditionally and bail if we don't have a stream_ to close.
  if (stream_ != NULL) {
    stream_->Stop();
    stream_->Close();
    stream_ = NULL;
    weak_this_.InvalidateWeakPtrs();
  }

  // Should be last in the method, do not touch "this" from here on.
  if (done != NULL)
    done->Signal();
}

}  // namespace media
<|endoftext|>


<commit_before>//
//  main.cpp
//  SparseMatrix
//
//  Created by Gabriele Carrettoni on 02/02/15.
//  Copyright (c) 2015 Gabriele Carrettoni. All rights reserved.
//

#include <iostream>
#include <cstdio>
#include <exception>
#include <stdexcept>

/**
 SparseMatrix
 Implementation of a SparseMatrix
 @param T the type to store in the matrix.
 */
template <typename T>
class SparseMatrix {

public:
    /**
     Struct to hold the elements with their respective coordinates.
     @param j the row coordinate
     @param k the coloumn cooridnate
     @param elem the element stored
     */
    struct Element {
        const int j, k;
        T elem;

        Element(int r, int c, const T e): j(r), k(c), elem(e) {}
    };

    class const_iterator;

    class iterator {
        //
    public:
        typedef std::forward_iterator_tag iterator_category;
        typedef Element                   value_type;
        typedef ptrdiff_t                 difference_type;
        typedef value_type*               pointer;
        typedef value_type*&              reference;

        /**
         Copy constructor
         @param other reference to the iterator to copy.
         */
        iterator(const iterator &other) {
            data = other.data;
        }

        /**
         Assignment
         @param other reference to the iterator to copy.
         */
        iterator& operator=(const iterator &other) {
            data = other.data;
            return *this;
        }

        /**
         Dereference pointer
         */
        reference operator*() const {
            return *data;
        }

        /**
         Return pointer
         @returns pointer to the current element.
         */
        pointer operator->() const {
            return *data;
        }

        /**
         Post-increment operator;
         @returns iterator not incremented;
         */
        iterator operator++(int) {
            return iterator(data++);
        }

        /**
         Pre-increment operator;
         @returns iterator incremented;
         */
        iterator& operator++() {
            ++data;
            return *this;
        }

        /**
         Equality operator, check if the two iterators point to the same element
         @param other reference to the other iterator
         */
        bool operator==(const iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check iif the two iterators
         don't point to the same element
         @param other reference to the other iterator
         */
        bool operator!=(const iterator &other) const {
            return data != other.data;
        }

        friend class const_iterator;

        /**
         Equality operator, check if the two iterators point to the same element.
         @param other reference to the other const_iterator
          */
        bool operator==(const const_iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check if the two iterators
         don't point to the same element.
         @param other reference to the other const_iterator
         */
        bool operator!=(const const_iterator &other) const {
            return data != other.data;
        }

    private:
        Element** data;

        friend class SparseMatrix;

        explicit iterator(Element** data_): data(data_) {}
    };

    class const_iterator {
        //
    public:
        typedef std::forward_iterator_tag iterator_category;
        typedef const Element             value_type;
        typedef ptrdiff_t                 difference_type;
        typedef value_type*               pointer;
        typedef value_type&               reference;

        /**
         Copy constructor
         @param other reference to the iterator to copy.
         */
        const_iterator(const iterator &other) {
            data = other.data;
        }

        /**
         Assignment
         @param other reference to the iterator to copy.
         */
        const_iterator& operator=(const iterator &other) {
            data = other.data;
            return *this;
        }

        /**
         Dereference pointer
         */
        reference operator*() const {
            return **data;
        }

        /**
         Return pointer
         @returns pointer to the current element.
         */
        pointer operator->() const {
            return *data;
        }

        /**
         Post-increment operator;
         @returns iterator not incremented;
         */
        const_iterator operator++(int) {
            return iterator(data++);
        }

        /**
         Pre-increment operator;
         @returns iterator incremented;
         */
        const_iterator& operator++() {
            ++data;
            return *this;
        }

        /**
         Equality operator, check if the two iterators point to the same element
         @param other reference to the other iterator
         */
        bool operator==(const const_iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check iif the two iterators
         don't point to the same element
         @param other reference to the other iterator
         */
        bool operator!=(const const_iterator &other) const {
            return data != other.data;
        }

        friend class const_iterator;

    private:
        Element** data;

        friend class SparseMatrix;

        const_iterator(Element** data_): data(data_) {}
    };

    /**
     Constructor
     @param r the number of rows
     @param c the number of coloumns
     @param d the default element to return when the user tries to retrieve an
     element not existent in the matrix.
     */
    explicit SparseMatrix(int r, int c, T d): rows(r), cols(c), def(d), size(0), m(NULL) {}

    /**
     Copy constructor
     @param sm the matrix to copy from
     */
    SparseMatrix(const SparseMatrix& sm) {
        rows = sm.rows;
        cols = sm.cols;

        def  = sm.def;
        size = sm.size;

        m = new Element*[size]();

        for (int i=0; i < size; i++) {
            m[i] = new Element(sm.m[i]->j, sm.m[i]->k, sm.m[i]->elem);
        }
    }

    template <typename Q>
    SparseMatrix(const SparseMatrix<Q>& sm) {
        rows = sm.get_rows();
        cols = sm.get_cols();

        def  = sm.get_def();
        size = sm.get_size();

        m = new Element*[size]();

        for (int i=0; i < size; i++) {
            m[i] = new Element(sm.get_m()[i]->j,
                               sm.get_m()[i]->k,
                               sm.get_m()[i]->elem);
        }
    }

    ~SparseMatrix() {
        for (int i=0; i < size; i++)
            delete m[i];

        delete[] m;
        m = NULL;
    }

    iterator begin() {
        return iterator(m);
    }

    iterator end() {
        return iterator(m+size);
    }

    const_iterator begin() const {
        return iterator(m);
    }

    const_iterator end() const {
        return iterator(m+size);
    }

    void add(int j, int k, const T elem) {
        //assert(j < rows && k < cols);

        Element** temp = new Element*[++size]();

        if (m) {
            for (int i=0; i < size; i++) {
                if (i == size-1) {
                    temp[i] = new Element(j, k, elem);
                    break;
                }

                if ((j < m[i]->j) || (j == m[i]->j && k < m[i]->k)) {
                    temp[i] = new Element(j, k, elem);

                    for (int x=i+1; x < size; x++) {
                        temp[x] = m[i++];
                    }

                    break;
                }

                if (j == m[i]->j && k == m[i]->k) {
                    m[i] = new Element(j, k, elem);
                    size--;

                    delete[] temp;
                    temp = NULL;

                    break;
                }

                temp[i] = m[i];
            }

        }
        else
            temp[0] = new Element(j, k, elem);

        if (temp) m = temp;
    }

    T operator()(int j, int k) {
        for (int i=0; i < size; i++) {
            if (m[i]->j == j && m[i]->k == k)
                return m[i]->elem;
        }

        return def;
    }

    const T get_def() const {
        return def;
    }

    const int get_rows() const {
        return rows;
    }

    const int get_cols() const {
        return cols;
    }

    const size_t get_size() const {
        return size;
    }

    Element** get_m() const  {
        return m;
    }

private:
    int rows;
    int cols;
    T def;

    Element** m;
    size_t size;
};

template <typename T, typename P>
int evaluate(SparseMatrix<T>& sm, P p) {
    int sum = 0;
    for (const typename SparseMatrix<T>::Element* e: sm) {
        if (p(e->elem)) sum++;
    }

    if (p(sm.get_def())) sum++;

    return sum;
}

bool t(int m) { return m > 1; }

int main(int argc, const char * argv[]) {
    SparseMatrix<float> m(10, 10, 0);
    m.add(0, 1, 1.2);
    m.add(9, 9, 0);
    m.add(3, 2, 9);
    m.add(3, 2, 5.3);

    SparseMatrix<int> m2(m);

    for (auto itr=m.begin(); itr != m.end(); itr++) {
        std::cout << itr->elem << std::endl;
    }

    std::cout << evaluate(m, t);

    return 0;
}
<commit_msg>Doc copy costructor<commit_after>//
//  main.cpp
//  SparseMatrix
//
//  Created by Gabriele Carrettoni on 02/02/15.
//  Copyright (c) 2015 Gabriele Carrettoni. All rights reserved.
//

#include <iostream>
#include <cstdio>
#include <exception>
#include <stdexcept>

/**
 SparseMatrix
 Implementation of a SparseMatrix
 @param T the type to store in the matrix.
 */
template <typename T>
class SparseMatrix {

public:
    /**
     Struct to hold the elements with their respective coordinates.
     @param j the row coordinate
     @param k the coloumn cooridnate
     @param elem the element stored
     */
    struct Element {
        const int j, k;
        T elem;

        Element(int r, int c, const T e): j(r), k(c), elem(e) {}
    };

    class const_iterator;

    class iterator {
        //
    public:
        typedef std::forward_iterator_tag iterator_category;
        typedef Element                   value_type;
        typedef ptrdiff_t                 difference_type;
        typedef value_type*               pointer;
        typedef value_type*&              reference;

        /**
         Copy constructor
         @param other reference to the iterator to copy.
         */
        iterator(const iterator &other) {
            data = other.data;
        }

        /**
         Assignment
         @param other reference to the iterator to copy.
         */
        iterator& operator=(const iterator &other) {
            data = other.data;
            return *this;
        }

        /**
         Dereference pointer
         */
        reference operator*() const {
            return *data;
        }

        /**
         Return pointer
         @returns pointer to the current element.
         */
        pointer operator->() const {
            return *data;
        }

        /**
         Post-increment operator;
         @returns iterator not incremented;
         */
        iterator operator++(int) {
            return iterator(data++);
        }

        /**
         Pre-increment operator;
         @returns iterator incremented;
         */
        iterator& operator++() {
            ++data;
            return *this;
        }

        /**
         Equality operator, check if the two iterators point to the same element
         @param other reference to the other iterator
         */
        bool operator==(const iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check iif the two iterators
         don't point to the same element
         @param other reference to the other iterator
         */
        bool operator!=(const iterator &other) const {
            return data != other.data;
        }

        friend class const_iterator;

        /**
         Equality operator, check if the two iterators point to the same element.
         @param other reference to the other const_iterator
          */
        bool operator==(const const_iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check if the two iterators
         don't point to the same element.
         @param other reference to the other const_iterator
         */
        bool operator!=(const const_iterator &other) const {
            return data != other.data;
        }

    private:
        Element** data;

        friend class SparseMatrix;

        explicit iterator(Element** data_): data(data_) {}
    };

    class const_iterator {
        //
    public:
        typedef std::forward_iterator_tag iterator_category;
        typedef const Element             value_type;
        typedef ptrdiff_t                 difference_type;
        typedef value_type*               pointer;
        typedef value_type&               reference;

        /**
         Copy constructor
         @param other reference to the iterator to copy.
         */
        const_iterator(const iterator &other) {
            data = other.data;
        }

        /**
         Assignment
         @param other reference to the iterator to copy.
         */
        const_iterator& operator=(const iterator &other) {
            data = other.data;
            return *this;
        }

        /**
         Dereference pointer
         */
        reference operator*() const {
            return **data;
        }

        /**
         Return pointer
         @returns pointer to the current element.
         */
        pointer operator->() const {
            return *data;
        }

        /**
         Post-increment operator;
         @returns iterator not incremented;
         */
        const_iterator operator++(int) {
            return iterator(data++);
        }

        /**
         Pre-increment operator;
         @returns iterator incremented;
         */
        const_iterator& operator++() {
            ++data;
            return *this;
        }

        /**
         Equality operator, check if the two iterators point to the same element
         @param other reference to the other iterator
         */
        bool operator==(const const_iterator &other) const {
            return data == other.data;
        }

        /**
         Inequality operator, check iif the two iterators
         don't point to the same element
         @param other reference to the other iterator
         */
        bool operator!=(const const_iterator &other) const {
            return data != other.data;
        }

        friend class const_iterator;

    private:
        Element** data;

        friend class SparseMatrix;

        const_iterator(Element** data_): data(data_) {}
    };

    /**
     Constructor
     @param r the number of rows
     @param c the number of coloumns
     @param d the default element to return when the user tries to retrieve an
     element not existent in the matrix.
     */
    explicit SparseMatrix(int r, int c, T d): rows(r), cols(c), def(d), size(0), m(NULL) {}

    /**
     Copy constructor
     @param sm the matrix to copy from
     */
    SparseMatrix(const SparseMatrix& sm) {
        rows = sm.rows;
        cols = sm.cols;

        def  = sm.def;
        size = sm.size;

        m = new Element*[size]();

        for (int i=0; i < size; i++) {
            m[i] = new Element(sm.m[i]->j, sm.m[i]->k, sm.m[i]->elem);
        }
    }

    /**
     Copy constructor from Matrix with different type trying to cast the type.
     @param sm the other matrix
     */
    template <typename Q>
    SparseMatrix(const SparseMatrix<Q>& sm) {
        rows = sm.get_rows();
        cols = sm.get_cols();

        def  = sm.get_def();
        size = sm.get_size();

        m = new Element*[size]();

        for (int i=0; i < size; i++) {
            m[i] = new Element(sm.get_m()[i]->j,
                               sm.get_m()[i]->k,
                               sm.get_m()[i]->elem);
        }
    }

    ~SparseMatrix() {
        for (int i=0; i < size; i++)
            delete m[i];

        delete[] m;
        m = NULL;
    }

    iterator begin() {
        return iterator(m);
    }

    iterator end() {
        return iterator(m+size);
    }

    const_iterator begin() const {
        return iterator(m);
    }

    const_iterator end() const {
        return iterator(m+size);
    }

    void add(int j, int k, const T elem) {
        //assert(j < rows && k < cols);

        Element** temp = new Element*[++size]();

        if (m) {
            for (int i=0; i < size; i++) {
                if (i == size-1) {
                    temp[i] = new Element(j, k, elem);
                    break;
                }

                if ((j < m[i]->j) || (j == m[i]->j && k < m[i]->k)) {
                    temp[i] = new Element(j, k, elem);

                    for (int x=i+1; x < size; x++) {
                        temp[x] = m[i++];
                    }

                    break;
                }

                if (j == m[i]->j && k == m[i]->k) {
                    m[i] = new Element(j, k, elem);
                    size--;

                    delete[] temp;
                    temp = NULL;

                    break;
                }

                temp[i] = m[i];
            }

        }
        else
            temp[0] = new Element(j, k, elem);

        if (temp) m = temp;
    }

    T operator()(int j, int k) {
        for (int i=0; i < size; i++) {
            if (m[i]->j == j && m[i]->k == k)
                return m[i]->elem;
        }

        return def;
    }

    const T get_def() const {
        return def;
    }

    const int get_rows() const {
        return rows;
    }

    const int get_cols() const {
        return cols;
    }

    const size_t get_size() const {
        return size;
    }

    Element** get_m() const  {
        return m;
    }

private:
    int rows;
    int cols;
    T def;

    Element** m;
    size_t size;
};

template <typename T, typename P>
int evaluate(SparseMatrix<T>& sm, P p) {
    int sum = 0;
    for (const typename SparseMatrix<T>::Element* e: sm) {
        if (p(e->elem)) sum++;
    }

    if (p(sm.get_def())) sum++;

    return sum;
}

bool t(int m) { return m > 1; }

int main(int argc, const char * argv[]) {
    SparseMatrix<float> m(10, 10, 0);
    m.add(0, 1, 1.2);
    m.add(9, 9, 0);
    m.add(3, 2, 9);
    m.add(3, 2, 5.3);

    SparseMatrix<int> m2(m);

    for (auto itr=m.begin(); itr != m.end(); itr++) {
        std::cout << itr->elem << std::endl;
    }

    std::cout << evaluate(m, t);

    return 0;
}
<|endoftext|>


<commit_before>#include "Gameplay.h"
#include "Sound.h"

// during idle, do a fanfare of light, music and fire
Metro kioskTimer(KIOSK_FANFARE_MAX);

// during play, the player can pause between button presses for this long before losing
Metro playerTimeout(PLAYER_TIMEOUT);

// track the game states
//
// the sequence length, [0, MAX_SEQUENCE_LENGTH-1]
int currentLength = 0;
// track the sequence.
char correctSequence[MAX_SEQUENCE_LENGTH];
// the count of corrent matches that the player has made.  <= currentLength
int correctLength = 0;

// implemented Simon as a finite state machine
// this is the definitions of the states that our program uses
State idle = State(idleState);
State game = State(gameState);
State player = State(playerState);
State exiting = State(exitingState);

// the state machine controls which of the states get attention and execution time
FSM simon = FSM(idle); //initialize state machine, start in state: idle

// startup
void gameplayStart(Sound &currSound) {
  Serial << F("Gameplay: startup.") << endl;
  sound = currSound;
}

// call this from loop()
boolean gameplayUpdate() {

  // update FSM
  simon.update();

  // return true if playing
  return ( !simon.isInState( idle ) );

}

// when there's nobody playing, we're in this state.
void idleState() {
//  Serial << F("Current State: Idle") << endl;
  if ( currentLength > 0 ) {
    kioskTimer.reset();
  }
  // reset everything.
  correctLength = 0;
  currentLength = 0;
  correctSequence[0] = '\0';

  // check buttons for game play start
  if ( touch.anyPressed() ) {
    Serial << F("Gameplay: Idle->Game") << endl;
    quiet();
    /* MGD removed this en lieu of anypresed.  The logic, below, doesn't exit with an abberant press.
     while (1) {
     if (buttonAnyChanged())
     break;
     if (touchAnyChanged())
     break;
     }*/
    while ( touch.anyPressed()) ;
    // let's play a game
    simon.transitionTo(game);
  }
  else if ( kioskTimer.check() ) {
    // let's do some light, music and fire
    idleFanfare();
  }

  // stubbing in microphone pickup activity.
  // doesn't do anything useful; just makes a tone that should be synchronized with the beat.
  mic.update();
  static Metro clearInterval(50);
  byte level=0;
  for(byte i = 0; i<NUM_FREQUENCY_BANDS; i++) {
    if( mic.getBeat(i) ) bitSet(level, 7-i);
  }
  if( level>0 ) { // got some kind of beat
    //    byte b1 = map(mic.getVol(0), byte(mic.getAvg(0)), byte(mic.getAvg(0)+mic.getSD(0)*mic.getTh(0)), LIGHT_OFF, LIGHT_ON);
    //    byte b2 = map(mic.getVol(1), byte(mic.getAvg(1)), byte(mic.getAvg(1)+mic.getSD(1)*mic.getTh(1)), LIGHT_OFF, LIGHT_ON);
    //    byte b3 = map(mic.getVol(2), byte(mic.getAvg(2)), byte(mic.getAvg(2)+mic.getSD(2)*mic.getTh(2)), LIGHT_OFF, LIGHT_ON);
    //    Serial << rl << endl; delay(25);
    //    light.setLight(I_RED, b3);
    //    light.setLight(I_GRN, b2);
    //    light.setLight(I_BLU, b1);

    light.setAllLight(level);
    light.setAllFire(bitRead(level,6) ? 10 : 0); // just the 160 Hz band, and only a little fire
    light.show();
    delay(10); // need to let the transmission happen

    clearInterval.reset();
  }

  if( clearInterval.check() ) {
    light.setAllOff(); // clear colors
    clearInterval.reset();
  }
}

// when the player has played, or at the start of the game, we're in this state
void gameState() {
  Serial << F("Current State: Game") << endl;
  // generate the next in sequence and add that to the sequence
  correctSequence[currentLength] = nextMove();
  correctSequence[currentLength + 1] = '\0'; // and null terminate it for printing purposes.
  // increment
  currentLength++;
  // play back sequence
  Serial << F("Gameplay: Game currentLength=") << currentLength << F(". sequence=") << correctSequence << endl;
  playSequence();
  // debug

  // reset timeout
  playerTimeout.reset();

  // give it back to the player
  Serial << F("Gameplay: Game->Player") << endl;
  simon.transitionTo(player);
}

// waiting for the player to press a button.
void playerState() {
  //Serial << F("Current State: Player.  correctLength: ") << correctLength << endl;
  // check if they waited too long
  if ( playerTimeout.check() ) {
    Serial << F("Gameplay: Player timeout.  currentLength = ") << currentLength << endl;
    // if so, show the correct next button
    play(correctSequence[correctLength], false);
    // wait
    sound.playBaff();
    Metro delayNow(2000);
    while (! delayNow.check() ) {
      light.update();
    }
    quiet();
    // and exit game play
    Serial << F("Gameplay: Player->Exiting") << endl;
    simon.transitionTo(exiting);
  }

  // otherwise, wait for button press.
  if ( touch.anyPressed() ) {
    boolean correct =
      (touch.pressed(I_GRN) && correctSequence[correctLength] == 'G') ||
      (touch.pressed(I_RED) && correctSequence[correctLength] == 'R') ||
      (touch.pressed(I_BLU) && correctSequence[correctLength] == 'B') ||
      (touch.pressed(I_YEL) && correctSequence[correctLength] == 'Y');

    // there's a total cheat.
    correct = correct || CHEATY_PANTS_MODE;

    // light and music
    play( correctSequence[correctLength], correct );
    // hold it while we're mashing
    while ( touch.anyPressed() ) {
      light.update();
    }

    // done
    quiet();

    if ( correct ) {
      correctLength++;
      Serial << "incCorrect: " << correctLength << "\n";
      // reset timeout
      playerTimeout.reset();
      // keep going
    }
    else {
      Serial << F("Gameplay: Player incorrect.  currentLength = ") << currentLength << " correct: " << correctLength << " expecting: " << correctSequence[correctLength] << endl;

      // if so, show the correct next button
      play(correctSequence[correctLength], false);

      // wait
      Metro delayNow(500);
      while (! delayNow.check() ) {
        light.update();
      }

      quiet();
      simon.transitionTo(exiting);
    }
  }

  /// check if they've got the sequence complete, or if there's just awesome
  if ( correctLength == currentLength || correctLength == (MAX_SEQUENCE_LENGTH - 1) ) {
    // nice.  pass it back to game
    Serial << F("Gameplay: Player correct.  currentLength = ") << currentLength << endl;
    Serial << F("Gameplay: Player->Game") << endl;

    correctLength = 0;
    simon.transitionTo(game);
  }
}

void exitingState() {
    Serial << "Inside exiting state.  correct: " << correctLength << endl;
    // if so, show the correct next button
    //play(correctSequence[correctLength], false);
    byte fanfareLevel = getLevel(currentLength-1);
    if (fanfareLevel == FANFARE_NONE) {
      Serial << "Animating failure";
      animateFailure();
      // wait
      Metro delayNow(3000);
      while (! delayNow.check() ) {
        light.update();
      }
    } else {
        playerFanfare(fanfareLevel);
    }
    quiet();

    // reset correct length
    correctLength = 0;

    // exit game play
    Serial << F("Gameplay: Player->Idle") << endl;
    simon.transitionTo(idle);
}

// generate a character to append to the current correct sequence
char nextMove() {
  int move = random(1, 4 + 1);
  // 1=GRN, 2=RED, 3=BLU, 4=YEL
  switch ( move ) {
  case 1:
    return ('G');
  case 2:
    return ('R');
  case 3:
    return ('B');
  case 4:
    return ('Y');
  }
}

// helper function to tie together Tower, Light, Sound
void setSoundLights(byte colorIndex, boolean correctTone) {
  // order is important: sound is slower than lights

  // Sound on Console and Tower
  sound.playTone(colorIndex, correctTone);

  // Lights on Tower
  light.setLight(colorIndex, LIGHT_ON, true);

}

// decode color character and set lights and music
void play(char color, boolean correctTone) {
  switch ( color ) {
  case 'G':
    setSoundLights(I_GRN, correctTone);
    break;
  case 'R':
    setSoundLights(I_RED, correctTone);
    break;
  case 'B':
    setSoundLights(I_BLU, correctTone);
    break;
  case 'Y':
    setSoundLights(I_YEL, correctTone);
    break;
  }
}

void animateFailure()
{
  Serial << "AnimateFailure!" << endl;
  sound.setVolume(0);
  sound.playLose();
  for (int i = 0; i < 6; i++) {
    light.setAllLight(LIGHT_ON, true);
    delay(300);
    light.setAllOff();
    delay(100);
  }

  quiet();
}

// turn off lights and music
void quiet() {
  // Lights and Fire on Tower
  // Lights on Console
  light.setAllOff();

  // Sound on Console and Tower
  sound.stop();

}

// playback a character array of the current correct sequence
void playSequence() {
  // how long between each
  unsigned long pauseDuration = 50; // ms
  Metro pauseAfter(pauseDuration);

  // how long to light and tone
  unsigned long playDuration = 420;
  if ( currentLength >= 6 ) playDuration = 320;
  if ( currentLength >= 14) playDuration = 220;
  Metro pauseDuring(playDuration);

  // delay after a player's last move
  delay(playDuration);

  for (int i = 0; i < currentLength; i++) {
    // lights and music
    play(correctSequence[i], true);
    // wait
    pauseDuring.reset();
    while ( !pauseDuring.check() ) {
      light.update(); // for resend, really.
    }

    // done
    quiet();

    // pause between next
    // wait
    pauseAfter.reset();
    while ( !pauseAfter.check() ) {
      light.update(); // for resend, really.
    }
  }
}

// generate a random number on the interval [a, b] with mode c.
unsigned long trandom(int a, int c, int b) {
  // using a triangular pdf
  // http://en.wikipedia.org/wiki/Triangular_distribution#Generating_Triangular-distributed_random_variates

  float cut = (c - a) / (b - a);

  float u = random(0, 101) / 100;

  if ( u < cut ) return ( a + sqrt( u * (b - a) * (c - a) ) );
  else return ( b - sqrt( (1 - u) * (b - a) * (b - c) ) );

}

//
void flameOn() {
}
void flameOff() {
}

void towerPoof(int poofCount) {
  Serial << F("! Poofs. N:") << poofCount << endl;
  for (int i = 0; i < poofCount; i++) {
    flameOn();
    delay(trandom(POOF_MIN, POOF_MODE, POOF_MAX));
    flameOff();
    delay(random(WAIT_MIN, WAIT_MAX));
  }
}

void idleFanfare() {
  Serial << F("Gameplay: Idle Fanfare !!") << endl;

  // pick a song
  int minSong = 22;
  int maxSong = 121;
  int song = random(minSong, maxSong + 1);
  Serial << F(" song: ") << song << endl;
  //  musicPlayer.play_P(song_table[song], 0);  // tunez and fire.  yeah.

  Serial << F("Gameplay: Idle fanfare ended") << endl;

  // reset the timer
  delay(300); // be still, my electrical heart
  unsigned long kioskTimerInterval = random(KIOSK_FANFARE_MIN, KIOSK_FANFARE_MAX);
  kioskTimer.interval(kioskTimerInterval);
  Serial << F("Gameplay: idle Fanfare interval reset to ") << kioskTimerInterval << endl;
  kioskTimer.reset();
}

void playerFanfare(byte level) {
  Serial << "***Inside playerFanFare: level: " << level << "\n";

  if (!FANFARE_ENABLED) {
    Serial.println("Fanfare disabled");
    return;
  }

  Serial << F("Gameplay: Player fanfare, duration: ") << FANFARE_DURATION_PER_CORRECT * currentLength << endl;

  Metro fanfareDuration(FANFARE_DURATION_PER_CORRECT * currentLength);

  // turn up the music
  byte volume = MUSIC_DEFAULT_VOL;
  sound.setVolume(volume);
/*
  // make sweet fire/light/music.
  sound.playWin();

  while ( ! fanfareDuration.check() ) {
    // should calculate Light and Fire on Towers here.

    // resend tower commands
    light.update();
  }
*/
    if (level >= FANFARE_1) {
      Serial << "Playing win level 0";
      for(byte n=0; n < 8; n++) {
        for ( byte i = 0; i < N_COLORS; i++ ) {
          if (n % 2 == 0) {
            light.setLight(I_BLU, LIGHT_ON);
            light.setFire(i, LIGHT_ON);
          } else {
            light.setLight(I_BLU, LIGHT_OFF);
            light.setFire(i, LIGHT_OFF);
          }
        }
        if (n % 2 == 0) {
          light.show();
          Metro delayNow(50);
          while (! delayNow.check() ) {
            light.update();
          }
        } else {
          light.show();
          Metro delayNow(400);
          while (! delayNow.check() ) {
            light.update();
          }
        }
      }
    }

    light.setAllLight(LIGHT_OFF);
    light.setAllFire(LIGHT_OFF);

  // ramp down the volume to exit the music playing cleanly.
  Metro volumeRampTime(MUSIC_RAMP_DOWN_TIME / volume);
  while ( volume > 0 ) {
    if ( volumeRampTime.check() ) {
      volumeRampTime.reset();
      volume--;
      sound.decVolume();
    }

    // resend tower commands
    light.update();
  }

  // end display
  quiet();

  Serial << F("Gameplay: Player fanfare ended") << endl;
}

// Get the fanfare level to play based on # correct.  Returns 0-3
byte getLevel(int correct) {
  Serial << "Get Level: correct: " << correct << "\n";
  if (correct < FANFARE_LEVEL1) {
    return FANFARE_NONE;
  } else if (correct <= FANFARE_LEVEL2) {
    return FANFARE_1;
  } else if (correct <= FANFARE_LEVEL3) {
    return FANFARE_2;
  } else {
    return FANFARE_3;
  }
}

<commit_msg>wav-trigger: fix gameplay useage of playtone<commit_after>#include "Gameplay.h"
#include "Sound.h"

// during idle, do a fanfare of light, music and fire
Metro kioskTimer(KIOSK_FANFARE_MAX);

// during play, the player can pause between button presses for this long before losing
Metro playerTimeout(PLAYER_TIMEOUT);

// track the game states
//
// the sequence length, [0, MAX_SEQUENCE_LENGTH-1]
int currentLength = 0;
// track the sequence.
char correctSequence[MAX_SEQUENCE_LENGTH];
// the count of corrent matches that the player has made.  <= currentLength
int correctLength = 0;

// implemented Simon as a finite state machine
// this is the definitions of the states that our program uses
State idle = State(idleState);
State game = State(gameState);
State player = State(playerState);
State exiting = State(exitingState);

// the state machine controls which of the states get attention and execution time
FSM simon = FSM(idle); //initialize state machine, start in state: idle

// startup
void gameplayStart(Sound &currSound) {
  Serial << F("Gameplay: startup.") << endl;
  sound = currSound;
}

// call this from loop()
boolean gameplayUpdate() {

  // update FSM
  simon.update();

  // return true if playing
  return ( !simon.isInState( idle ) );

}

// when there's nobody playing, we're in this state.
void idleState() {
//  Serial << F("Current State: Idle") << endl;
  if ( currentLength > 0 ) {
    kioskTimer.reset();
  }
  // reset everything.
  correctLength = 0;
  currentLength = 0;
  correctSequence[0] = '\0';

  // check buttons for game play start
  if ( touch.anyPressed() ) {
    Serial << F("Gameplay: Idle->Game") << endl;
    quiet();
    /* MGD removed this en lieu of anypresed.  The logic, below, doesn't exit with an abberant press.
     while (1) {
     if (buttonAnyChanged())
     break;
     if (touchAnyChanged())
     break;
     }*/
    while ( touch.anyPressed()) ;
    // let's play a game
    simon.transitionTo(game);
  }
  else if ( kioskTimer.check() ) {
    // let's do some light, music and fire
    idleFanfare();
  }

  // stubbing in microphone pickup activity.
  // doesn't do anything useful; just makes a tone that should be synchronized with the beat.
  mic.update();
  static Metro clearInterval(50);
  byte level=0;
  for(byte i = 0; i<NUM_FREQUENCY_BANDS; i++) {
    if( mic.getBeat(i) ) bitSet(level, 7-i);
  }
  if( level>0 ) { // got some kind of beat
    //    byte b1 = map(mic.getVol(0), byte(mic.getAvg(0)), byte(mic.getAvg(0)+mic.getSD(0)*mic.getTh(0)), LIGHT_OFF, LIGHT_ON);
    //    byte b2 = map(mic.getVol(1), byte(mic.getAvg(1)), byte(mic.getAvg(1)+mic.getSD(1)*mic.getTh(1)), LIGHT_OFF, LIGHT_ON);
    //    byte b3 = map(mic.getVol(2), byte(mic.getAvg(2)), byte(mic.getAvg(2)+mic.getSD(2)*mic.getTh(2)), LIGHT_OFF, LIGHT_ON);
    //    Serial << rl << endl; delay(25);
    //    light.setLight(I_RED, b3);
    //    light.setLight(I_GRN, b2);
    //    light.setLight(I_BLU, b1);

    light.setAllLight(level);
    light.setAllFire(bitRead(level,6) ? 10 : 0); // just the 160 Hz band, and only a little fire
    light.show();
    delay(10); // need to let the transmission happen

    clearInterval.reset();
  }

  if( clearInterval.check() ) {
    light.setAllOff(); // clear colors
    clearInterval.reset();
  }
}

// when the player has played, or at the start of the game, we're in this state
void gameState() {
  Serial << F("Current State: Game") << endl;
  // generate the next in sequence and add that to the sequence
  correctSequence[currentLength] = nextMove();
  correctSequence[currentLength + 1] = '\0'; // and null terminate it for printing purposes.
  // increment
  currentLength++;
  // play back sequence
  Serial << F("Gameplay: Game currentLength=") << currentLength << F(". sequence=") << correctSequence << endl;
  playSequence();
  // debug

  // reset timeout
  playerTimeout.reset();

  // give it back to the player
  Serial << F("Gameplay: Game->Player") << endl;
  simon.transitionTo(player);
}

// waiting for the player to press a button.
void playerState() {
  //Serial << F("Current State: Player.  correctLength: ") << correctLength << endl;
  // check if they waited too long
  if ( playerTimeout.check() ) {
    Serial << F("Gameplay: Player timeout.  currentLength = ") << currentLength << endl;
    // if so, show the correct next button
    play(correctSequence[correctLength], false);
    // wait
    sound.playBaff();
    Metro delayNow(2000);
    while (! delayNow.check() ) {
      light.update();
    }
    quiet();
    // and exit game play
    Serial << F("Gameplay: Player->Exiting") << endl;
    simon.transitionTo(exiting);
  }

  // otherwise, wait for button press.
  if ( touch.anyPressed() ) {
    boolean correct =
      (touch.pressed(I_GRN) && correctSequence[correctLength] == 'G') ||
      (touch.pressed(I_RED) && correctSequence[correctLength] == 'R') ||
      (touch.pressed(I_BLU) && correctSequence[correctLength] == 'B') ||
      (touch.pressed(I_YEL) && correctSequence[correctLength] == 'Y');

    // there's a total cheat.
    correct = correct || CHEATY_PANTS_MODE;

    // light and music
    play( correctSequence[correctLength], correct );
    // hold it while we're mashing
    while ( touch.anyPressed() ) {
      light.update();
    }

    // done
    quiet();

    if ( correct ) {
      correctLength++;
      Serial << "incCorrect: " << correctLength << "\n";
      // reset timeout
      playerTimeout.reset();
      // keep going
    }
    else {
      Serial << F("Gameplay: Player incorrect.  currentLength = ") << currentLength << " correct: " << correctLength << " expecting: " << correctSequence[correctLength] << endl;

      // if so, show the correct next button
      play(correctSequence[correctLength], false);

      // wait
      Metro delayNow(500);
      while (! delayNow.check() ) {
        light.update();
      }

      quiet();
      simon.transitionTo(exiting);
    }
  }

  /// check if they've got the sequence complete, or if there's just awesome
  if ( correctLength == currentLength || correctLength == (MAX_SEQUENCE_LENGTH - 1) ) {
    // nice.  pass it back to game
    Serial << F("Gameplay: Player correct.  currentLength = ") << currentLength << endl;
    Serial << F("Gameplay: Player->Game") << endl;

    correctLength = 0;
    simon.transitionTo(game);
  }
}

void exitingState() {
    Serial << "Inside exiting state.  correct: " << correctLength << endl;
    // if so, show the correct next button
    //play(correctSequence[correctLength], false);
    byte fanfareLevel = getLevel(currentLength-1);
    if (fanfareLevel == FANFARE_NONE) {
      Serial << "Animating failure";
      animateFailure();
      // wait
      Metro delayNow(3000);
      while (! delayNow.check() ) {
        light.update();
      }
    } else {
        playerFanfare(fanfareLevel);
    }
    quiet();

    // reset correct length
    correctLength = 0;

    // exit game play
    Serial << F("Gameplay: Player->Idle") << endl;
    simon.transitionTo(idle);
}

// generate a character to append to the current correct sequence
char nextMove() {
  int move = random(1, 4 + 1);
  // 1=GRN, 2=RED, 3=BLU, 4=YEL
  switch ( move ) {
  case 1:
    return ('G');
  case 2:
    return ('R');
  case 3:
    return ('B');
  case 4:
    return ('Y');
  }
}

// helper function to tie together Tower, Light, Sound
void setSoundLights(byte colorIndex, boolean correctTone) {
  // order is important: sound is slower than lights

  // Sound on Console and Tower
  if (correctTone) {
      sound.playTone(colorIndex);
  } else {
      sound.playFailureTone();
  }

  // Lights on Tower
  light.setLight(colorIndex, LIGHT_ON, true);

}

// decode color character and set lights and music
void play(char color, boolean correctTone) {
  switch ( color ) {
  case 'G':
    setSoundLights(I_GRN, correctTone);
    break;
  case 'R':
    setSoundLights(I_RED, correctTone);
    break;
  case 'B':
    setSoundLights(I_BLU, correctTone);
    break;
  case 'Y':
    setSoundLights(I_YEL, correctTone);
    break;
  }
}

void animateFailure()
{
  Serial << "AnimateFailure!" << endl;
  sound.setVolume(0);
  sound.playLose();
  for (int i = 0; i < 6; i++) {
    light.setAllLight(LIGHT_ON, true);
    delay(300);
    light.setAllOff();
    delay(100);
  }

  quiet();
}

// turn off lights and music
void quiet() {
  // Lights and Fire on Tower
  // Lights on Console
  light.setAllOff();

  // Sound on Console and Tower
  sound.stop();

}

// playback a character array of the current correct sequence
void playSequence() {
  // how long between each
  unsigned long pauseDuration = 50; // ms
  Metro pauseAfter(pauseDuration);

  // how long to light and tone
  unsigned long playDuration = 420;
  if ( currentLength >= 6 ) playDuration = 320;
  if ( currentLength >= 14) playDuration = 220;
  Metro pauseDuring(playDuration);

  // delay after a player's last move
  delay(playDuration);

  for (int i = 0; i < currentLength; i++) {
    // lights and music
    play(correctSequence[i], true);
    // wait
    pauseDuring.reset();
    while ( !pauseDuring.check() ) {
      light.update(); // for resend, really.
    }

    // done
    quiet();

    // pause between next
    // wait
    pauseAfter.reset();
    while ( !pauseAfter.check() ) {
      light.update(); // for resend, really.
    }
  }
}

// generate a random number on the interval [a, b] with mode c.
unsigned long trandom(int a, int c, int b) {
  // using a triangular pdf
  // http://en.wikipedia.org/wiki/Triangular_distribution#Generating_Triangular-distributed_random_variates

  float cut = (c - a) / (b - a);

  float u = random(0, 101) / 100;

  if ( u < cut ) return ( a + sqrt( u * (b - a) * (c - a) ) );
  else return ( b - sqrt( (1 - u) * (b - a) * (b - c) ) );

}

//
void flameOn() {
}
void flameOff() {
}

void towerPoof(int poofCount) {
  Serial << F("! Poofs. N:") << poofCount << endl;
  for (int i = 0; i < poofCount; i++) {
    flameOn();
    delay(trandom(POOF_MIN, POOF_MODE, POOF_MAX));
    flameOff();
    delay(random(WAIT_MIN, WAIT_MAX));
  }
}

void idleFanfare() {
  Serial << F("Gameplay: Idle Fanfare !!") << endl;

  // pick a song
  int minSong = 22;
  int maxSong = 121;
  int song = random(minSong, maxSong + 1);
  Serial << F(" song: ") << song << endl;
  //  musicPlayer.play_P(song_table[song], 0);  // tunez and fire.  yeah.

  Serial << F("Gameplay: Idle fanfare ended") << endl;

  // reset the timer
  delay(300); // be still, my electrical heart
  unsigned long kioskTimerInterval = random(KIOSK_FANFARE_MIN, KIOSK_FANFARE_MAX);
  kioskTimer.interval(kioskTimerInterval);
  Serial << F("Gameplay: idle Fanfare interval reset to ") << kioskTimerInterval << endl;
  kioskTimer.reset();
}

void playerFanfare(byte level) {
  Serial << "***Inside playerFanFare: level: " << level << "\n";

  if (!FANFARE_ENABLED) {
    Serial.println("Fanfare disabled");
    return;
  }

  Serial << F("Gameplay: Player fanfare, duration: ") << FANFARE_DURATION_PER_CORRECT * currentLength << endl;

  Metro fanfareDuration(FANFARE_DURATION_PER_CORRECT * currentLength);

  // turn up the music
  byte volume = MUSIC_DEFAULT_VOL;
  sound.setVolume(volume);
/*
  // make sweet fire/light/music.
  sound.playWin();

  while ( ! fanfareDuration.check() ) {
    // should calculate Light and Fire on Towers here.

    // resend tower commands
    light.update();
  }
*/
    if (level >= FANFARE_1) {
      Serial << "Playing win level 0";
      for(byte n=0; n < 8; n++) {
        for ( byte i = 0; i < N_COLORS; i++ ) {
          if (n % 2 == 0) {
            light.setLight(I_BLU, LIGHT_ON);
            light.setFire(i, LIGHT_ON);
          } else {
            light.setLight(I_BLU, LIGHT_OFF);
            light.setFire(i, LIGHT_OFF);
          }
        }
        if (n % 2 == 0) {
          light.show();
          Metro delayNow(50);
          while (! delayNow.check() ) {
            light.update();
          }
        } else {
          light.show();
          Metro delayNow(400);
          while (! delayNow.check() ) {
            light.update();
          }
        }
      }
    }

    light.setAllLight(LIGHT_OFF);
    light.setAllFire(LIGHT_OFF);

  // ramp down the volume to exit the music playing cleanly.
  Metro volumeRampTime(MUSIC_RAMP_DOWN_TIME / volume);
  while ( volume > 0 ) {
    if ( volumeRampTime.check() ) {
      volumeRampTime.reset();
      volume--;
      sound.decVolume();
    }

    // resend tower commands
    light.update();
  }

  // end display
  quiet();

  Serial << F("Gameplay: Player fanfare ended") << endl;
}

// Get the fanfare level to play based on # correct.  Returns 0-3
byte getLevel(int correct) {
  Serial << "Get Level: correct: " << correct << "\n";
  if (correct < FANFARE_LEVEL1) {
    return FANFARE_NONE;
  } else if (correct <= FANFARE_LEVEL2) {
    return FANFARE_1;
  } else if (correct <= FANFARE_LEVEL3) {
    return FANFARE_2;
  } else {
    return FANFARE_3;
  }
}

<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Jon Binney, Ioan Sucan */

#include <cmath>
#include <moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h>
#include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <XmlRpcException.h>

#include <memory>

namespace occupancy_map_monitor
{
static const std::string LOGNAME = "occupancy_map_monitor";
PointCloudOctomapUpdater::PointCloudOctomapUpdater()
  : OccupancyMapUpdater("PointCloudUpdater")
  , private_nh_("~")
  , scale_(1.0)
  , padding_(0.0)
  , max_range_(std::numeric_limits<double>::infinity())
  , point_subsample_(1)
  , max_update_rate_(0)
  , point_cloud_subscriber_(nullptr)
  , point_cloud_filter_(nullptr)
{
}

PointCloudOctomapUpdater::~PointCloudOctomapUpdater()
{
  stopHelper();
}

bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
{
  try
  {
    if (!params.hasMember("point_cloud_topic"))
      return false;
    point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);

    readXmlParam(params, "max_range", &max_range_);
    readXmlParam(params, "padding_offset", &padding_);
    readXmlParam(params, "padding_scale", &scale_);
    readXmlParam(params, "point_subsample", &point_subsample_);
    if (params.hasMember("max_update_rate"))
      readXmlParam(params, "max_update_rate", &max_update_rate_);
    if (params.hasMember("filtered_cloud_topic"))
      filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
  }
  catch (XmlRpc::XmlRpcException& ex)
  {
    ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage());
    return false;
  }

  return true;
}

bool PointCloudOctomapUpdater::initialize()
{
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, root_nh_);
  shape_mask_ = std::make_unique<point_containment_filter::ShapeMask>();
  shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
  if (!filtered_cloud_topic_.empty())
    filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
  return true;
}

void PointCloudOctomapUpdater::start()
{
  if (point_cloud_subscriber_)
    return;
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 5);
  if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty())
  {
    point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_buffer_,
                                                                               monitor_->getMapFrame(), 5, root_nh_);
    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
                   point_cloud_filter_->getTargetFramesString().c_str());
  }
  else
  {
    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str());
  }
}

void PointCloudOctomapUpdater::stopHelper()
{
  delete point_cloud_filter_;
  delete point_cloud_subscriber_;
}

void PointCloudOctomapUpdater::stop()
{
  stopHelper();
  point_cloud_filter_ = nullptr;
  point_cloud_subscriber_ = nullptr;
}

ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& shape)
{
  ShapeHandle h = 0;
  if (shape_mask_)
    h = shape_mask_->addShape(shape, scale_, padding_);
  else
    ROS_ERROR_NAMED(LOGNAME, "Shape filter not yet initialized!");
  return h;
}

void PointCloudOctomapUpdater::forgetShape(ShapeHandle handle)
{
  if (shape_mask_)
    shape_mask_->removeShape(handle);
}

bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const
{
  ShapeTransformCache::const_iterator it = transform_cache_.find(h);
  if (it != transform_cache_.end())
  {
    transform = it->second;
  }
  return it != transform_cache_.end();
}

void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& /*cloud*/,
                                          const Eigen::Vector3d& /*sensor_origin*/, std::vector<int>& /*mask*/)
{
}

void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
  ROS_DEBUG_NAMED(LOGNAME, "Received a new point cloud message");
  ros::WallTime start = ros::WallTime::now();

  if (max_update_rate_ > 0)
  {
    // ensure we are not updating the octomap representation too often
    if (ros::Time::now() - last_update_time_ <= ros::Duration(1.0 / max_update_rate_))
      return;
    last_update_time_ = ros::Time::now();
  }

  if (monitor_->getMapFrame().empty())
    monitor_->setMapFrame(cloud_msg->header.frame_id);

  /* get transform for cloud into map frame */
  tf2::Stamped<tf2::Transform> map_h_sensor;
  if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
    map_h_sensor.setIdentity();
  else
  {
    if (tf_buffer_)
    {
      try
      {
        tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id,
                                                 cloud_msg->header.stamp),
                     map_h_sensor);
      }
      catch (tf2::TransformException& ex)
      {
        ROS_ERROR_STREAM_NAMED(LOGNAME, "Transform error of sensor data: " << ex.what() << "; quitting callback");
        return;
      }
    }
    else
      return;
  }

  /* compute sensor origin in map frame */
  const tf2::Vector3& sensor_origin_tf = map_h_sensor.getOrigin();
  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
  Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());

  if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
    return;

  /* mask out points on the robot */
  shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
  updateMask(*cloud_msg, sensor_origin_eigen, mask_);

  octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
  std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;

  // We only use these iterators if we are creating a filtered_cloud for
  // publishing. We cannot default construct these, so we use unique_ptr's
  // to defer construction
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_x;
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_y;
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_z;

  if (!filtered_cloud_topic_.empty())
  {
    filtered_cloud = std::make_unique<sensor_msgs::PointCloud2>();
    filtered_cloud->header = cloud_msg->header;
    sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
    pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
    pcd_modifier.resize(cloud_msg->width * cloud_msg->height);

    // we have created a filtered_out, so we can create the iterators now
    iter_filtered_x = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "x");
    iter_filtered_y = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "y");
    iter_filtered_z = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "z");
  }
  size_t filtered_cloud_size = 0;

  tree_->lockRead();

  try
  {
    /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
     * should be occupied */
    for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
    {
      unsigned int row_c = row * cloud_msg->width;
      sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
      // set iterator to point at start of the current row
      pt_iter += row_c;

      for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
      {
        // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
        //  continue;

        /* check for NaN */
        if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
        {
          /* transform to map frame */
          tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);

          /* occupied cell at ray endpoint if ray is shorter than max range and this point
             isn't on a part of the robot*/
          if (mask_[row_c + col] == point_containment_filter::ShapeMask::INSIDE)
            model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
          else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
            clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
          else
          {
            occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
            // build list of valid points if we want to publish them
            if (filtered_cloud)
            {
              **iter_filtered_x = pt_iter[0];
              **iter_filtered_y = pt_iter[1];
              **iter_filtered_z = pt_iter[2];
              ++filtered_cloud_size;
              ++*iter_filtered_x;
              ++*iter_filtered_y;
              ++*iter_filtered_z;
            }
          }
        }
      }
    }

    /* compute the free cells along each ray that ends at an occupied cell */
    for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(occupied_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

    /* compute the free cells along each ray that ends at a model cell */
    for (const octomap::OcTreeKey& model_cell : model_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(model_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

    /* compute the free cells along each ray that ends at a clipped cell */
    for (const octomap::OcTreeKey& clip_cell : clip_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(clip_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());
  }
  catch (...)
  {
    tree_->unlockRead();
    return;
  }

  tree_->unlockRead();

  /* cells that overlap with the model are not occupied */
  for (const octomap::OcTreeKey& model_cell : model_cells)
    occupied_cells.erase(model_cell);

  /* occupied cells are not free */
  for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
    free_cells.erase(occupied_cell);

  tree_->lockWrite();

  try
  {
    /* mark free cells only if not seen occupied in this cloud */
    for (const octomap::OcTreeKey& free_cell : free_cells)
      tree_->updateNode(free_cell, false);

    /* now mark all occupied cells */
    for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
      tree_->updateNode(occupied_cell, true);

    // set the logodds to the minimum for the cells that are part of the model
    const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
    for (const octomap::OcTreeKey& model_cell : model_cells)
      tree_->updateNode(model_cell, lg);
  }
  catch (...)
  {
    ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree");
  }
  tree_->unlockWrite();
  ROS_DEBUG_NAMED(LOGNAME, "Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
  tree_->triggerUpdateCallback();

  if (filtered_cloud)
  {
    sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
    pcd_modifier.resize(filtered_cloud_size);
    filtered_cloud_publisher_.publish(*filtered_cloud);
  }
}
}  // namespace occupancy_map_monitor
<commit_msg>clipped points should only be considered up to the max_range (#2848)<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Jon Binney, Ioan Sucan */

#include <cmath>
#include <moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h>
#include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <XmlRpcException.h>

#include <memory>

namespace occupancy_map_monitor
{
static const std::string LOGNAME = "occupancy_map_monitor";
PointCloudOctomapUpdater::PointCloudOctomapUpdater()
  : OccupancyMapUpdater("PointCloudUpdater")
  , private_nh_("~")
  , scale_(1.0)
  , padding_(0.0)
  , max_range_(std::numeric_limits<double>::infinity())
  , point_subsample_(1)
  , max_update_rate_(0)
  , point_cloud_subscriber_(nullptr)
  , point_cloud_filter_(nullptr)
{
}

PointCloudOctomapUpdater::~PointCloudOctomapUpdater()
{
  stopHelper();
}

bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
{
  try
  {
    if (!params.hasMember("point_cloud_topic"))
      return false;
    point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);

    readXmlParam(params, "max_range", &max_range_);
    readXmlParam(params, "padding_offset", &padding_);
    readXmlParam(params, "padding_scale", &scale_);
    readXmlParam(params, "point_subsample", &point_subsample_);
    if (params.hasMember("max_update_rate"))
      readXmlParam(params, "max_update_rate", &max_update_rate_);
    if (params.hasMember("filtered_cloud_topic"))
      filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
  }
  catch (XmlRpc::XmlRpcException& ex)
  {
    ROS_ERROR_STREAM_NAMED(LOGNAME, "XmlRpc Exception: " << ex.getMessage());
    return false;
  }

  return true;
}

bool PointCloudOctomapUpdater::initialize()
{
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, root_nh_);
  shape_mask_ = std::make_unique<point_containment_filter::ShapeMask>();
  shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
  if (!filtered_cloud_topic_.empty())
    filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
  return true;
}

void PointCloudOctomapUpdater::start()
{
  if (point_cloud_subscriber_)
    return;
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 5);
  if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty())
  {
    point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_buffer_,
                                                                               monitor_->getMapFrame(), 5, root_nh_);
    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
                   point_cloud_filter_->getTargetFramesString().c_str());
  }
  else
  {
    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str());
  }
}

void PointCloudOctomapUpdater::stopHelper()
{
  delete point_cloud_filter_;
  delete point_cloud_subscriber_;
}

void PointCloudOctomapUpdater::stop()
{
  stopHelper();
  point_cloud_filter_ = nullptr;
  point_cloud_subscriber_ = nullptr;
}

ShapeHandle PointCloudOctomapUpdater::excludeShape(const shapes::ShapeConstPtr& shape)
{
  ShapeHandle h = 0;
  if (shape_mask_)
    h = shape_mask_->addShape(shape, scale_, padding_);
  else
    ROS_ERROR_NAMED(LOGNAME, "Shape filter not yet initialized!");
  return h;
}

void PointCloudOctomapUpdater::forgetShape(ShapeHandle handle)
{
  if (shape_mask_)
    shape_mask_->removeShape(handle);
}

bool PointCloudOctomapUpdater::getShapeTransform(ShapeHandle h, Eigen::Isometry3d& transform) const
{
  ShapeTransformCache::const_iterator it = transform_cache_.find(h);
  if (it != transform_cache_.end())
  {
    transform = it->second;
  }
  return it != transform_cache_.end();
}

void PointCloudOctomapUpdater::updateMask(const sensor_msgs::PointCloud2& /*cloud*/,
                                          const Eigen::Vector3d& /*sensor_origin*/, std::vector<int>& /*mask*/)
{
}

void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
  ROS_DEBUG_NAMED(LOGNAME, "Received a new point cloud message");
  ros::WallTime start = ros::WallTime::now();

  if (max_update_rate_ > 0)
  {
    // ensure we are not updating the octomap representation too often
    if (ros::Time::now() - last_update_time_ <= ros::Duration(1.0 / max_update_rate_))
      return;
    last_update_time_ = ros::Time::now();
  }

  if (monitor_->getMapFrame().empty())
    monitor_->setMapFrame(cloud_msg->header.frame_id);

  /* get transform for cloud into map frame */
  tf2::Stamped<tf2::Transform> map_h_sensor;
  if (monitor_->getMapFrame() == cloud_msg->header.frame_id)
    map_h_sensor.setIdentity();
  else
  {
    if (tf_buffer_)
    {
      try
      {
        tf2::fromMsg(tf_buffer_->lookupTransform(monitor_->getMapFrame(), cloud_msg->header.frame_id,
                                                 cloud_msg->header.stamp),
                     map_h_sensor);
      }
      catch (tf2::TransformException& ex)
      {
        ROS_ERROR_STREAM_NAMED(LOGNAME, "Transform error of sensor data: " << ex.what() << "; quitting callback");
        return;
      }
    }
    else
      return;
  }

  /* compute sensor origin in map frame */
  const tf2::Vector3& sensor_origin_tf = map_h_sensor.getOrigin();
  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());
  Eigen::Vector3d sensor_origin_eigen(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());

  if (!updateTransformCache(cloud_msg->header.frame_id, cloud_msg->header.stamp))
    return;

  /* mask out points on the robot */
  shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
  updateMask(*cloud_msg, sensor_origin_eigen, mask_);

  octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
  std::unique_ptr<sensor_msgs::PointCloud2> filtered_cloud;

  // We only use these iterators if we are creating a filtered_cloud for
  // publishing. We cannot default construct these, so we use unique_ptr's
  // to defer construction
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_x;
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_y;
  std::unique_ptr<sensor_msgs::PointCloud2Iterator<float>> iter_filtered_z;

  if (!filtered_cloud_topic_.empty())
  {
    filtered_cloud = std::make_unique<sensor_msgs::PointCloud2>();
    filtered_cloud->header = cloud_msg->header;
    sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
    pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
    pcd_modifier.resize(cloud_msg->width * cloud_msg->height);

    // we have created a filtered_out, so we can create the iterators now
    iter_filtered_x = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "x");
    iter_filtered_y = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "y");
    iter_filtered_z = std::make_unique<sensor_msgs::PointCloud2Iterator<float>>(*filtered_cloud, "z");
  }
  size_t filtered_cloud_size = 0;

  tree_->lockRead();

  try
  {
    /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
     * should be occupied */
    for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
    {
      unsigned int row_c = row * cloud_msg->width;
      sensor_msgs::PointCloud2ConstIterator<float> pt_iter(*cloud_msg, "x");
      // set iterator to point at start of the current row
      pt_iter += row_c;

      for (unsigned int col = 0; col < cloud_msg->width; col += point_subsample_, pt_iter += point_subsample_)
      {
        // if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
        //  continue;

        /* check for NaN */
        if (!std::isnan(pt_iter[0]) && !std::isnan(pt_iter[1]) && !std::isnan(pt_iter[2]))
        {
          /* occupied cell at ray endpoint if ray is shorter than max range and this point
             isn't on a part of the robot*/
          if (mask_[row_c + col] == point_containment_filter::ShapeMask::INSIDE)
          {
            // transform to map frame
            tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
            model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
          }
          else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
          {
            tf2::Vector3 clipped_point_tf =
                map_h_sensor * (tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]).normalize() * max_range_);
            clip_cells.insert(
                tree_->coordToKey(clipped_point_tf.getX(), clipped_point_tf.getY(), clipped_point_tf.getZ()));
          }
          else
          {
            tf2::Vector3 point_tf = map_h_sensor * tf2::Vector3(pt_iter[0], pt_iter[1], pt_iter[2]);
            occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
            // build list of valid points if we want to publish them
            if (filtered_cloud)
            {
              **iter_filtered_x = pt_iter[0];
              **iter_filtered_y = pt_iter[1];
              **iter_filtered_z = pt_iter[2];
              ++filtered_cloud_size;
              ++*iter_filtered_x;
              ++*iter_filtered_y;
              ++*iter_filtered_z;
            }
          }
        }
      }
    }

    /* compute the free cells along each ray that ends at an occupied cell */
    for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(occupied_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

    /* compute the free cells along each ray that ends at a model cell */
    for (const octomap::OcTreeKey& model_cell : model_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(model_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

    /* compute the free cells along each ray that ends at a clipped cell */
    for (const octomap::OcTreeKey& clip_cell : clip_cells)
      if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(clip_cell), key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());
  }
  catch (...)
  {
    tree_->unlockRead();
    return;
  }

  tree_->unlockRead();

  /* cells that overlap with the model are not occupied */
  for (const octomap::OcTreeKey& model_cell : model_cells)
    occupied_cells.erase(model_cell);

  /* occupied cells are not free */
  for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
    free_cells.erase(occupied_cell);

  tree_->lockWrite();

  try
  {
    /* mark free cells only if not seen occupied in this cloud */
    for (const octomap::OcTreeKey& free_cell : free_cells)
      tree_->updateNode(free_cell, false);

    /* now mark all occupied cells */
    for (const octomap::OcTreeKey& occupied_cell : occupied_cells)
      tree_->updateNode(occupied_cell, true);

    // set the logodds to the minimum for the cells that are part of the model
    const float lg = tree_->getClampingThresMinLog() - tree_->getClampingThresMaxLog();
    for (const octomap::OcTreeKey& model_cell : model_cells)
      tree_->updateNode(model_cell, lg);
  }
  catch (...)
  {
    ROS_ERROR_NAMED(LOGNAME, "Internal error while updating octree");
  }
  tree_->unlockWrite();
  ROS_DEBUG_NAMED(LOGNAME, "Processed point cloud in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
  tree_->triggerUpdateCallback();

  if (filtered_cloud)
  {
    sensor_msgs::PointCloud2Modifier pcd_modifier(*filtered_cloud);
    pcd_modifier.resize(filtered_cloud_size);
    filtered_cloud_publisher_.publish(*filtered_cloud);
  }
}
}  // namespace occupancy_map_monitor
<|endoftext|>