<commit_before>// Copyright (c) 2006-2008 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.
//
// This code glues the RLZ library DLL with Chrome. It allows Chrome to work
// with or without the DLL being present. If the DLL is not present the
// functions do nothing and just return false.

#include "chrome/browser/rlz/rlz.h"

#include <windows.h>
#include <process.h>

#include "base/file_util.h"
#include "base/message_loop.h"
#include "base/path_service.h"
#include "base/task.h"
#include "base/thread.h"
#include "chrome/browser/browser_process.h"
#include "chrome/browser/profile.h"
#include "chrome/browser/profile_manager.h"
#include "chrome/browser/search_engines/template_url_model.h"
#include "chrome/common/chrome_paths.h"
#include "chrome/common/env_vars.h"
#include "chrome/common/notification_service.h"
#include "chrome/installer/util/google_update_settings.h"

namespace {

// The maximum length of an access points RLZ in wide chars.
const DWORD kMaxRlzLength = 64;

// The RLZ is a DLL that might not be present in the system. We load it
// as needed but never unload it.
volatile HMODULE rlz_dll = NULL;

enum {
  ACCESS_VALUES_STALE,      // Possibly new values available.
  ACCESS_VALUES_FRESH       // The cached values are current.
};

// Tracks if we have tried and succeeded sending the ping. This helps us
// decide if we need to refresh the some cached strings.
volatile int access_values_state = ACCESS_VALUES_STALE;

extern "C" {
typedef bool (*RecordProductEventFn)(RLZTracker::Product product,
                                     RLZTracker::AccessPoint point,
                                     RLZTracker::Event event_id,
                                     void* reserved);

typedef bool (*GetAccessPointRlzFn)(RLZTracker::AccessPoint point,
                                    wchar_t* rlz,
                                    DWORD rlz_size,
                                    void* reserved);

typedef bool (*ClearAllProductEventsFn)(RLZTracker::Product product,
                                        void* reserved);

typedef bool (*SendFinancialPingFn)(RLZTracker::Product product,
                                    RLZTracker::AccessPoint* access_points,
                                    const WCHAR* product_signature,
                                    const WCHAR* product_brand,
                                    const WCHAR* product_id,
                                    const WCHAR* product_lang,
                                    bool exclude_id,
                                    void* reserved);
}  // extern "C".

RecordProductEventFn record_event = NULL;
GetAccessPointRlzFn get_access_point = NULL;
ClearAllProductEventsFn clear_all_events = NULL;
SendFinancialPingFn send_ping = NULL;

template <typename FuncT>
FuncT WireExport(HMODULE module, const char* export_name) {
  void* entry_point = ::GetProcAddress(module, export_name);
  return (module)? reinterpret_cast<FuncT>(entry_point) : NULL;
}

HMODULE LoadRLZLibraryInternal(int directory_key) {
  std::wstring rlz_path;
  if (!PathService::Get(directory_key, &rlz_path))
    return NULL;
  file_util::AppendToPath(&rlz_path, L"rlz.dll");
  return ::LoadLibraryW(rlz_path.c_str());
}

bool LoadRLZLibrary(int directory_key) {
  rlz_dll = LoadRLZLibraryInternal(directory_key);
  if (!rlz_dll) {
    // As a last resort we can try the EXE directory.
    if (directory_key != base::DIR_EXE)
      rlz_dll = LoadRLZLibraryInternal(base::DIR_EXE);
  }
  if (rlz_dll) {
    record_event =
        WireExport<RecordProductEventFn>(rlz_dll, "RecordProductEvent");
    get_access_point =
        WireExport<GetAccessPointRlzFn>(rlz_dll, "GetAccessPointRlz");
    clear_all_events =
        WireExport<ClearAllProductEventsFn>(rlz_dll, "ClearAllProductEvents");
    send_ping =
        WireExport<SendFinancialPingFn>(rlz_dll, "SendFinancialPing");
    return (record_event && get_access_point && clear_all_events && send_ping);
  }
  return false;
}

bool SendFinancialPing(const wchar_t* brand, const wchar_t* lang,
                       const wchar_t* referral, bool exclude_id) {
  RLZTracker::AccessPoint points[] = {RLZTracker::CHROME_OMNIBOX,
                                      RLZTracker::CHROME_HOME_PAGE,
                                      RLZTracker::NO_ACCESS_POINT};
  if (!send_ping)
    return false;
  return send_ping(RLZTracker::CHROME, points, L"chrome", brand, referral, lang,
                   exclude_id, NULL);
}

// This class leverages the AutocompleteEditModel notification to know when
// the user first interacted with the omnibox and set a global accordingly.
class OmniBoxUsageObserver : public NotificationObserver {
 public:
  OmniBoxUsageObserver() {
    NotificationService::current()->AddObserver(this,
        NOTIFY_OMNIBOX_OPENED_URL,
        NotificationService::AllSources());
    omnibox_used_ = false;
    DCHECK(!instance_);
    instance_ = this;
  }

  virtual void Observe(NotificationType type,
                       const NotificationSource& source,
                       const NotificationDetails& details) {
    // Try to record event now, else set the flag to try later when we
    // attempt the ping.
    if (!RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                        RLZTracker::CHROME_OMNIBOX,
                                        RLZTracker::FIRST_SEARCH))
      omnibox_used_ = true;
    delete this;
  }

  static bool used() {
    return omnibox_used_;
  }

  // Deletes the single instance of OmniBoxUsageObserver.
  static void DeleteInstance() {
    delete instance_;
  }

 private:
  // Dtor is private so the object cannot be created on the stack.
  ~OmniBoxUsageObserver() {
    NotificationService::current()->RemoveObserver(this,
        NOTIFY_OMNIBOX_OPENED_URL,
        NotificationService::AllSources());
    instance_ = NULL;
  }

  static bool omnibox_used_;

  // There should only be one instance created at a time, and instance_ points
  // to that instance.
  // NOTE: this is only non-null for the amount of time it is needed. Once the
  // instance_ is no longer needed (or Chrome is exitting), this is null.
  static OmniBoxUsageObserver* instance_;
};

bool OmniBoxUsageObserver::omnibox_used_ = false;
OmniBoxUsageObserver* OmniBoxUsageObserver::instance_ = NULL;

// This task is run in the file thread, so to not block it for a long time
// we use a throwaway thread to do the blocking url request.
class DailyPingTask : public Task {
 public:
  virtual ~DailyPingTask() {
  }
  virtual void Run() {
    // We use a transient thread because we have no guarantees about
    // how long the RLZ lib can block us.
    _beginthread(PingNow, 0, NULL);
  }

 private:
  // Causes a ping to the server using WinInet. There is logic inside RLZ dll
  // that throttles it to a maximum of one ping per day.
  static void _cdecl PingNow(void*) {
    std::wstring lang;
    GoogleUpdateSettings::GetLanguage(&lang);
    if (lang.empty())
      lang = L"en";
    std::wstring brand;
    GoogleUpdateSettings::GetBrand(&brand);
    std::wstring referral;
    GoogleUpdateSettings::GetReferral(&referral);
    if (SendFinancialPing(brand.c_str(), lang.c_str(), referral.c_str(),
                          is_organic(brand))) {
      access_values_state = ACCESS_VALUES_STALE;
      GoogleUpdateSettings::ClearReferral();
    }
  }

  // Organic brands all start with GG, such as GGCM.
  static bool is_organic(const std::wstring brand) {
    return (brand.size() < 2) ? false : (brand.substr(0,2) == L"GG");
  }
};

// Performs late RLZ initialization and RLZ event recording for chrome.
// This task needs to run on the UI thread.
class DelayedInitTask : public Task {
 public:
  explicit DelayedInitTask(int directory_key, bool first_run)
      : directory_key_(directory_key), first_run_(first_run) {
  }
  virtual ~DelayedInitTask() {
  }
  virtual void Run() {
    // For non-interactive tests we don't do the rest of the initialization
    // because sometimes the very act of loading the dll causes QEMU to crash.
    if (::GetEnvironmentVariableW(env_vars::kHeadless, NULL, 0))
      return;
    if (!LoadRLZLibrary(directory_key_))
      return;
    // Do the initial event recording if is the first run or if we have an
    // empty rlz which means we haven't got a chance to do it.
    std::wstring omnibox_rlz;
    RLZTracker::GetAccessPointRlz(RLZTracker::CHROME_OMNIBOX, &omnibox_rlz);

    if (first_run_ || omnibox_rlz.empty()) {
      // Record the installation of chrome.
      RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                     RLZTracker::CHROME_OMNIBOX,
                                     RLZTracker::INSTALL);
      RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                     RLZTracker::CHROME_HOME_PAGE,
                                     RLZTracker::INSTALL);
      // Record if google is the initial search provider.
      if (IsGoogleDefaultSearch()) {
        RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                       RLZTracker::CHROME_OMNIBOX,
                                       RLZTracker::SET_TO_GOOGLE);
      }
      // Record first user interaction with the omnibox.
      if (OmniBoxUsageObserver::used()) {
        RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                       RLZTracker::CHROME_OMNIBOX,
                                       RLZTracker::FIRST_SEARCH);
      }
    }
    // Schedule the daily RLZ ping.
    base::Thread* thread = g_browser_process->file_thread();
    if (thread)
      thread->message_loop()->PostTask(FROM_HERE, new DailyPingTask());
  }

 private:
  bool IsGoogleDefaultSearch() {
    if (!g_browser_process)
      return false;
    std::wstring user_data_dir;
    if (!PathService::Get(chrome::DIR_USER_DATA, &user_data_dir))
      return false;
    ProfileManager* profile_manager = g_browser_process->profile_manager();
    Profile* profile = profile_manager->
        GetDefaultProfile(FilePath::FromWStringHack(user_data_dir));
    if (!profile)
      return false;
    const TemplateURL* url_template =
        profile->GetTemplateURLModel()->GetDefaultSearchProvider();
    if (!url_template)
      return false;
    return url_template->url()->HasGoogleBaseURLs();
  }

  int directory_key_;
  bool first_run_;
  DISALLOW_IMPLICIT_CONSTRUCTORS(DelayedInitTask);
};

}  // namespace

bool RLZTracker::InitRlz(int directory_key) {
  return LoadRLZLibrary(directory_key);
}

bool RLZTracker::InitRlzDelayed(int directory_key, bool first_run) {
  if (!OmniBoxUsageObserver::used())
    new OmniBoxUsageObserver();
  // Schedule the delayed init items.
  const int kTwentySeconds = 20 * 1000;
  MessageLoop::current()->PostDelayedTask(FROM_HERE,
      new DelayedInitTask(directory_key, first_run), kTwentySeconds);
  return true;
}

bool RLZTracker::RecordProductEvent(Product product, AccessPoint point,
                                    Event event) {
  return (record_event) ? record_event(product, point, event, NULL) : false;
}

bool RLZTracker::ClearAllProductEvents(Product product) {
  return (clear_all_events) ? clear_all_events(product, NULL) : false;
}

// We implement caching of the answer of get_access_point() if the request
// is for CHROME_OMNIBOX. If we had a successful ping, then we update the
// cached value.

bool RLZTracker::GetAccessPointRlz(AccessPoint point, std::wstring* rlz) {
  static std::wstring cached_ommibox_rlz;
  if (!get_access_point)
    return false;
  if ((CHROME_OMNIBOX == point) &&
      (access_values_state == ACCESS_VALUES_FRESH)) {
    *rlz = cached_ommibox_rlz;
    return true;
  }
  wchar_t str_rlz[kMaxRlzLength];
  if (!get_access_point(point, str_rlz, kMaxRlzLength, NULL))
    return false;
  if (CHROME_OMNIBOX == point) {
    access_values_state = ACCESS_VALUES_FRESH;
    cached_ommibox_rlz.assign(str_rlz);
  }
  *rlz = str_rlz;
  return true;
}

// static
void RLZTracker::CleanupRlz() {
  OmniBoxUsageObserver::DeleteInstance();
}
<commit_msg>Move RLZ back to 90 seconds delayed init<commit_after>// Copyright (c) 2006-2008 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.
//
// This code glues the RLZ library DLL with Chrome. It allows Chrome to work
// with or without the DLL being present. If the DLL is not present the
// functions do nothing and just return false.

#include "chrome/browser/rlz/rlz.h"

#include <windows.h>
#include <process.h>

#include "base/file_util.h"
#include "base/message_loop.h"
#include "base/path_service.h"
#include "base/task.h"
#include "base/thread.h"
#include "chrome/browser/browser_process.h"
#include "chrome/browser/profile.h"
#include "chrome/browser/profile_manager.h"
#include "chrome/browser/search_engines/template_url_model.h"
#include "chrome/common/chrome_paths.h"
#include "chrome/common/env_vars.h"
#include "chrome/common/notification_service.h"
#include "chrome/installer/util/google_update_settings.h"

namespace {

// The maximum length of an access points RLZ in wide chars.
const DWORD kMaxRlzLength = 64;

// The RLZ is a DLL that might not be present in the system. We load it
// as needed but never unload it.
volatile HMODULE rlz_dll = NULL;

enum {
  ACCESS_VALUES_STALE,      // Possibly new values available.
  ACCESS_VALUES_FRESH       // The cached values are current.
};

// Tracks if we have tried and succeeded sending the ping. This helps us
// decide if we need to refresh the some cached strings.
volatile int access_values_state = ACCESS_VALUES_STALE;

extern "C" {
typedef bool (*RecordProductEventFn)(RLZTracker::Product product,
                                     RLZTracker::AccessPoint point,
                                     RLZTracker::Event event_id,
                                     void* reserved);

typedef bool (*GetAccessPointRlzFn)(RLZTracker::AccessPoint point,
                                    wchar_t* rlz,
                                    DWORD rlz_size,
                                    void* reserved);

typedef bool (*ClearAllProductEventsFn)(RLZTracker::Product product,
                                        void* reserved);

typedef bool (*SendFinancialPingFn)(RLZTracker::Product product,
                                    RLZTracker::AccessPoint* access_points,
                                    const WCHAR* product_signature,
                                    const WCHAR* product_brand,
                                    const WCHAR* product_id,
                                    const WCHAR* product_lang,
                                    bool exclude_id,
                                    void* reserved);
}  // extern "C".

RecordProductEventFn record_event = NULL;
GetAccessPointRlzFn get_access_point = NULL;
ClearAllProductEventsFn clear_all_events = NULL;
SendFinancialPingFn send_ping = NULL;

template <typename FuncT>
FuncT WireExport(HMODULE module, const char* export_name) {
  void* entry_point = ::GetProcAddress(module, export_name);
  return (module)? reinterpret_cast<FuncT>(entry_point) : NULL;
}

HMODULE LoadRLZLibraryInternal(int directory_key) {
  std::wstring rlz_path;
  if (!PathService::Get(directory_key, &rlz_path))
    return NULL;
  file_util::AppendToPath(&rlz_path, L"rlz.dll");
  return ::LoadLibraryW(rlz_path.c_str());
}

bool LoadRLZLibrary(int directory_key) {
  rlz_dll = LoadRLZLibraryInternal(directory_key);
  if (!rlz_dll) {
    // As a last resort we can try the EXE directory.
    if (directory_key != base::DIR_EXE)
      rlz_dll = LoadRLZLibraryInternal(base::DIR_EXE);
  }
  if (rlz_dll) {
    record_event =
        WireExport<RecordProductEventFn>(rlz_dll, "RecordProductEvent");
    get_access_point =
        WireExport<GetAccessPointRlzFn>(rlz_dll, "GetAccessPointRlz");
    clear_all_events =
        WireExport<ClearAllProductEventsFn>(rlz_dll, "ClearAllProductEvents");
    send_ping =
        WireExport<SendFinancialPingFn>(rlz_dll, "SendFinancialPing");
    return (record_event && get_access_point && clear_all_events && send_ping);
  }
  return false;
}

bool SendFinancialPing(const wchar_t* brand, const wchar_t* lang,
                       const wchar_t* referral, bool exclude_id) {
  RLZTracker::AccessPoint points[] = {RLZTracker::CHROME_OMNIBOX,
                                      RLZTracker::CHROME_HOME_PAGE,
                                      RLZTracker::NO_ACCESS_POINT};
  if (!send_ping)
    return false;
  return send_ping(RLZTracker::CHROME, points, L"chrome", brand, referral, lang,
                   exclude_id, NULL);
}

// This class leverages the AutocompleteEditModel notification to know when
// the user first interacted with the omnibox and set a global accordingly.
class OmniBoxUsageObserver : public NotificationObserver {
 public:
  OmniBoxUsageObserver() {
    NotificationService::current()->AddObserver(this,
        NOTIFY_OMNIBOX_OPENED_URL,
        NotificationService::AllSources());
    omnibox_used_ = false;
    DCHECK(!instance_);
    instance_ = this;
  }

  virtual void Observe(NotificationType type,
                       const NotificationSource& source,
                       const NotificationDetails& details) {
    // Try to record event now, else set the flag to try later when we
    // attempt the ping.
    if (!RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                        RLZTracker::CHROME_OMNIBOX,
                                        RLZTracker::FIRST_SEARCH))
      omnibox_used_ = true;
    delete this;
  }

  static bool used() {
    return omnibox_used_;
  }

  // Deletes the single instance of OmniBoxUsageObserver.
  static void DeleteInstance() {
    delete instance_;
  }

 private:
  // Dtor is private so the object cannot be created on the stack.
  ~OmniBoxUsageObserver() {
    NotificationService::current()->RemoveObserver(this,
        NOTIFY_OMNIBOX_OPENED_URL,
        NotificationService::AllSources());
    instance_ = NULL;
  }

  static bool omnibox_used_;

  // There should only be one instance created at a time, and instance_ points
  // to that instance.
  // NOTE: this is only non-null for the amount of time it is needed. Once the
  // instance_ is no longer needed (or Chrome is exitting), this is null.
  static OmniBoxUsageObserver* instance_;
};

bool OmniBoxUsageObserver::omnibox_used_ = false;
OmniBoxUsageObserver* OmniBoxUsageObserver::instance_ = NULL;

// This task is run in the file thread, so to not block it for a long time
// we use a throwaway thread to do the blocking url request.
class DailyPingTask : public Task {
 public:
  virtual ~DailyPingTask() {
  }
  virtual void Run() {
    // We use a transient thread because we have no guarantees about
    // how long the RLZ lib can block us.
    _beginthread(PingNow, 0, NULL);
  }

 private:
  // Causes a ping to the server using WinInet. There is logic inside RLZ dll
  // that throttles it to a maximum of one ping per day.
  static void _cdecl PingNow(void*) {
    std::wstring lang;
    GoogleUpdateSettings::GetLanguage(&lang);
    if (lang.empty())
      lang = L"en";
    std::wstring brand;
    GoogleUpdateSettings::GetBrand(&brand);
    std::wstring referral;
    GoogleUpdateSettings::GetReferral(&referral);
    if (SendFinancialPing(brand.c_str(), lang.c_str(), referral.c_str(),
                          is_organic(brand))) {
      access_values_state = ACCESS_VALUES_STALE;
      GoogleUpdateSettings::ClearReferral();
    }
  }

  // Organic brands all start with GG, such as GGCM.
  static bool is_organic(const std::wstring brand) {
    return (brand.size() < 2) ? false : (brand.substr(0,2) == L"GG");
  }
};

// Performs late RLZ initialization and RLZ event recording for chrome.
// This task needs to run on the UI thread.
class DelayedInitTask : public Task {
 public:
  explicit DelayedInitTask(int directory_key, bool first_run)
      : directory_key_(directory_key), first_run_(first_run) {
  }
  virtual ~DelayedInitTask() {
  }
  virtual void Run() {
    // For non-interactive tests we don't do the rest of the initialization
    // because sometimes the very act of loading the dll causes QEMU to crash.
    if (::GetEnvironmentVariableW(env_vars::kHeadless, NULL, 0))
      return;
    if (!LoadRLZLibrary(directory_key_))
      return;
    // Do the initial event recording if is the first run or if we have an
    // empty rlz which means we haven't got a chance to do it.
    std::wstring omnibox_rlz;
    RLZTracker::GetAccessPointRlz(RLZTracker::CHROME_OMNIBOX, &omnibox_rlz);

    if (first_run_ || omnibox_rlz.empty()) {
      // Record the installation of chrome.
      RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                     RLZTracker::CHROME_OMNIBOX,
                                     RLZTracker::INSTALL);
      RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                     RLZTracker::CHROME_HOME_PAGE,
                                     RLZTracker::INSTALL);
      // Record if google is the initial search provider.
      if (IsGoogleDefaultSearch()) {
        RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                       RLZTracker::CHROME_OMNIBOX,
                                       RLZTracker::SET_TO_GOOGLE);
      }
      // Record first user interaction with the omnibox.
      if (OmniBoxUsageObserver::used()) {
        RLZTracker::RecordProductEvent(RLZTracker::CHROME,
                                       RLZTracker::CHROME_OMNIBOX,
                                       RLZTracker::FIRST_SEARCH);
      }
    }
    // Schedule the daily RLZ ping.
    base::Thread* thread = g_browser_process->file_thread();
    if (thread)
      thread->message_loop()->PostTask(FROM_HERE, new DailyPingTask());
  }

 private:
  bool IsGoogleDefaultSearch() {
    if (!g_browser_process)
      return false;
    std::wstring user_data_dir;
    if (!PathService::Get(chrome::DIR_USER_DATA, &user_data_dir))
      return false;
    ProfileManager* profile_manager = g_browser_process->profile_manager();
    Profile* profile = profile_manager->
        GetDefaultProfile(FilePath::FromWStringHack(user_data_dir));
    if (!profile)
      return false;
    const TemplateURL* url_template =
        profile->GetTemplateURLModel()->GetDefaultSearchProvider();
    if (!url_template)
      return false;
    return url_template->url()->HasGoogleBaseURLs();
  }

  int directory_key_;
  bool first_run_;
  DISALLOW_IMPLICIT_CONSTRUCTORS(DelayedInitTask);
};

}  // namespace

bool RLZTracker::InitRlz(int directory_key) {
  return LoadRLZLibrary(directory_key);
}

bool RLZTracker::InitRlzDelayed(int directory_key, bool first_run) {
  if (!OmniBoxUsageObserver::used())
    new OmniBoxUsageObserver();
  // Schedule the delayed init items.
  const int kNinetySeconds = 90 * 1000;
  MessageLoop::current()->PostDelayedTask(FROM_HERE,
      new DelayedInitTask(directory_key, first_run), kNinetySeconds);
  return true;
}

bool RLZTracker::RecordProductEvent(Product product, AccessPoint point,
                                    Event event) {
  return (record_event) ? record_event(product, point, event, NULL) : false;
}

bool RLZTracker::ClearAllProductEvents(Product product) {
  return (clear_all_events) ? clear_all_events(product, NULL) : false;
}

// We implement caching of the answer of get_access_point() if the request
// is for CHROME_OMNIBOX. If we had a successful ping, then we update the
// cached value.

bool RLZTracker::GetAccessPointRlz(AccessPoint point, std::wstring* rlz) {
  static std::wstring cached_ommibox_rlz;
  if (!get_access_point)
    return false;
  if ((CHROME_OMNIBOX == point) &&
      (access_values_state == ACCESS_VALUES_FRESH)) {
    *rlz = cached_ommibox_rlz;
    return true;
  }
  wchar_t str_rlz[kMaxRlzLength];
  if (!get_access_point(point, str_rlz, kMaxRlzLength, NULL))
    return false;
  if (CHROME_OMNIBOX == point) {
    access_values_state = ACCESS_VALUES_FRESH;
    cached_ommibox_rlz.assign(str_rlz);
  }
  *rlz = str_rlz;
  return true;
}

// static
void RLZTracker::CleanupRlz() {
  OmniBoxUsageObserver::DeleteInstance();
}
<|endoftext|>


<commit_before>//
//  MCIMAPCheckAccountOperation.cc
//  mailcore2
//
//  Created by DINH Viêt Hoà on 1/12/13.
//  Copyright (c) 2013 MailCore. All rights reserved.
//

#include "MCIMAPCheckAccountOperation.h"

#include "MCIMAPAsyncConnection.h"
#include "MCIMAPSession.h"

using namespace mailcore;

void IMAPCheckAccountOperation::main()
{
    ErrorCode error;
    session()->session()->login(&error);
    setError(error);
}
<commit_msg>Was trying to login before creating a connection in IMAPCheckAccountOperation, which was causing a crash<commit_after>//
//  MCIMAPCheckAccountOperation.cc
//  mailcore2
//
//  Created by DINH Viêt Hoà on 1/12/13.
//  Copyright (c) 2013 MailCore. All rights reserved.
//

#include "MCIMAPCheckAccountOperation.h"

#include "MCIMAPAsyncConnection.h"
#include "MCIMAPSession.h"

using namespace mailcore;

void IMAPCheckAccountOperation::main()
{
    ErrorCode error;
    session()->session()->connect(&error);
    if (error == ErrorNone)
        session()->session()->login(&error);
    setError(error);
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Dan Leinir Turthra Jensen <admin@leinir.dk>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#include <QQmlEngine>
#include <QQmlContext>
#include <QQmlComponent>

#include <QStandardPaths>
#include <QDir>
#include <QDebug>
#include <QThread>

#include <KLocalizedString>

#include <QApplication>
#include <QCommandLineParser>
#include <QCommandLineOption>

#include <iostream>

int main(int argc, char** argv)
{
    QApplication app(argc, argv);
    app.setApplicationDisplayName("Peruse");

    QCommandLineParser parser;
    // TODO file option for opening comics by passing them through on the command line
    parser.addHelpOption();
    parser.process(app);

    if (parser.positionalArguments().size() > 1) {
        parser.showHelp(1);
    }

    QQmlEngine engine;

#ifdef Q_OS_WIN
    // Because windows is a bit funny with paths and whatnot, just so the thing with the lib paths...
    QDir appdir(qApp->applicationDirPath());
    appdir.cdUp();
    engine.addImportPath(appdir.canonicalPath() + "/lib/qml");
#endif

    QQmlContext* objectContext = engine.rootContext();
    QString platformEnv(qgetenv("PLASMA_PLATFORM"));
    engine.rootContext()->setContextProperty("PLASMA_PLATFORM", platformEnv);
    // Yes, i realise this is a touch on the ugly side. I have found no better way to allow for
    // things like the archive book model to create imageproviders for the archives
    engine.rootContext()->setContextProperty("globalQmlEngine", &engine);

    QString path;
    if (platformEnv.startsWith("phone")) {
        path = QStandardPaths::locate(QStandardPaths::DataLocation, "qml/MobileMain.qml");
    } else {
        path = QStandardPaths::locate(QStandardPaths::DataLocation, "qml/Main.qml");
    }
    int rt = 0;
    QQmlComponent component(&engine, path);
    if (component.isError())
    {
        qCritical() << "Failed to load the component from disk. Reported error was:" << component.errorString();
        rt = -1;
    }
    else
    {
        if(component.status() == QQmlComponent::Ready)
        {
            QObject* obj = component.create(objectContext);
            if(obj)
            {
                rt = app.exec();
            }
            else
            {
                qCritical() << "Failed to create an object from our component";
                rt = -2;
            }
        }
        else
        {
            qCritical() << "Failed to make the Qt Quick component ready. Status is:" << component.status();
            rt = -3;
        }
    }
    return rt;
}
<commit_msg>Tell kio not to fork on windows<commit_after>/*
 * Copyright (C) 2015 Dan Leinir Turthra Jensen <admin@leinir.dk>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#include <QQmlEngine>
#include <QQmlContext>
#include <QQmlComponent>

#include <QStandardPaths>
#include <QDir>
#include <QDebug>
#include <QThread>

#include <KLocalizedString>

#include <QApplication>
#include <QCommandLineParser>
#include <QCommandLineOption>

#include <iostream>

int main(int argc, char** argv)
{
    QApplication app(argc, argv);
    app.setApplicationDisplayName("Peruse");

    QCommandLineParser parser;
    // TODO file option for opening comics by passing them through on the command line
    parser.addHelpOption();
    parser.process(app);

    if (parser.positionalArguments().size() > 1) {
        parser.showHelp(1);
    }

    QQmlEngine engine;

#ifdef Q_OS_WIN
    // Because windows is a bit funny with paths and whatnot, just so the thing with the lib paths...
    QDir appdir(qApp->applicationDirPath());
    appdir.cdUp();
    engine.addImportPath(appdir.canonicalPath() + "/lib/qml");
    // Hey, let's try and avoid all those extra stale processes, right?
    qputenv("KDE_FORK_SLAVES", "true");
#endif

    QQmlContext* objectContext = engine.rootContext();
    QString platformEnv(qgetenv("PLASMA_PLATFORM"));
    engine.rootContext()->setContextProperty("PLASMA_PLATFORM", platformEnv);
    // Yes, i realise this is a touch on the ugly side. I have found no better way to allow for
    // things like the archive book model to create imageproviders for the archives
    engine.rootContext()->setContextProperty("globalQmlEngine", &engine);

    QString path;
    if (platformEnv.startsWith("phone")) {
        path = QStandardPaths::locate(QStandardPaths::DataLocation, "qml/MobileMain.qml");
    } else {
        path = QStandardPaths::locate(QStandardPaths::DataLocation, "qml/Main.qml");
    }
    int rt = 0;
    QQmlComponent component(&engine, path);
    if (component.isError())
    {
        qCritical() << "Failed to load the component from disk. Reported error was:" << component.errorString();
        rt = -1;
    }
    else
    {
        if(component.status() == QQmlComponent::Ready)
        {
            QObject* obj = component.create(objectContext);
            if(obj)
            {
                rt = app.exec();
            }
            else
            {
                qCritical() << "Failed to create an object from our component";
                rt = -2;
            }
        }
        else
        {
            qCritical() << "Failed to make the Qt Quick component ready. Status is:" << component.status();
            rt = -3;
        }
    }

    return rt;
}
<|endoftext|>


<commit_before>/*
  Copyright (c) 2016 Jakob Nixdorf <flocke@shadowice.org>

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
*/

#include <cstdlib>
#include <gtk/gtk.h>

#include "sterm/config.hpp"
#include "sterm/function_handler.hpp"
#include "sterm/misc.hpp"
#include "sterm/terminal.hpp"

std::string config_file = sterm::misc::xdg_config_file_path("sterm", "sterm.ini");

GtkWidget *main_window = NULL;
sterm::config *configuration = NULL;
sterm::function_handler *functions = NULL;
sterm::terminal *terminal = NULL;

gboolean running = false;

static gchar *opt_config_file = NULL;
static gchar *opt_command = NULL;

static GOptionEntry options[] {
  { "config", 'c', 0, G_OPTION_ARG_FILENAME, &opt_config_file, "Path to an alternative configuration file", NULL },
  { "execute", 'e', 0, G_OPTION_ARG_STRING, &opt_command, "Execute a command", NULL },
  { NULL }
};

static void main_cleanup() {
  gtk_main_quit();

  if ( terminal != NULL ) {
    delete(terminal);
    terminal = NULL;
  }

  if ( functions != NULL ) {
    delete(functions);
    functions = NULL;
  }

  if ( configuration != NULL ) {
    delete(configuration);
    configuration = NULL;
  }

  running = false;
}

static void main_exit_cb(GtkWidget *i_widget) {
  main_cleanup();

  exit(EXIT_SUCCESS);
}

static void main_exit_with_status_cb(GtkWidget* i_widget, int status) {
  main_cleanup();

  exit(WIFEXITED(status) ? WEXITSTATUS(status) : EXIT_FAILURE);
}

static gboolean parse_commandline(int argc, char* argv[]) {
  gboolean success = true;
  GError *error = NULL;

  GOptionContext *context = g_option_context_new(NULL);
  g_option_context_set_summary(context, "STerm - a simple terminal emulator based on the VTE library");
  g_option_context_add_main_entries(context, options, NULL);
  g_option_context_add_group(context, gtk_get_option_group(true));
  if ( ! g_option_context_parse(context, &argc, &argv, &error) ) {
    g_warning("failed to parse the commandline options: %s", error->message);
    g_error_free(error);
    success = false;
  }

  return(success);
}

int main(int argc, char *argv[]) {
  gtk_init(&argc, &argv);

  if ( ! parse_commandline(argc, argv) )
    return(EXIT_FAILURE);

  if ( opt_config_file != NULL )
    config_file = opt_config_file;

  configuration = new sterm::config(config_file);

  gtk_window_set_default_icon_name("utilities-terminal");

  main_window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
  gtk_window_set_title(GTK_WINDOW(main_window), "STerm");
  gtk_container_set_border_width(GTK_CONTAINER(main_window), 0);

  terminal = new sterm::terminal(configuration);

  std::string child_command;
  if ( opt_command != NULL )
    child_command = opt_command;

  terminal->attach_to_container(GTK_CONTAINER(main_window));

  terminal->spawn_child(child_command);
  terminal->connect_callback("child-exited", G_CALLBACK(main_exit_with_status_cb), NULL);

  terminal->connect_callback("destroy", G_CALLBACK(main_exit_cb), NULL);
  terminal->link_property_to_terminal("window-title", G_OBJECT(main_window), "title");
  g_signal_connect(G_OBJECT(main_window), "destroy", G_CALLBACK(main_exit_cb), NULL);

  functions = new sterm::function_handler(configuration, terminal);

  gtk_widget_show_all(main_window);
  running = true;
  gtk_main();

  main_cleanup();
  return(EXIT_FAILURE);
}

<commit_msg>Fix a small memory leak in the main app<commit_after>/*
  Copyright (c) 2016 Jakob Nixdorf <flocke@shadowice.org>

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
*/

#include <cstdlib>
#include <gtk/gtk.h>

#include "sterm/config.hpp"
#include "sterm/function_handler.hpp"
#include "sterm/misc.hpp"
#include "sterm/terminal.hpp"

std::string config_file = sterm::misc::xdg_config_file_path("sterm", "sterm.ini");

GtkWidget *main_window = NULL;
sterm::config *configuration = NULL;
sterm::function_handler *functions = NULL;
sterm::terminal *terminal = NULL;

gboolean running = false;

static gchar *opt_config_file = NULL;
static gchar *opt_command = NULL;

static GOptionEntry options[] {
  { "config", 'c', 0, G_OPTION_ARG_FILENAME, &opt_config_file, "Path to an alternative configuration file", NULL },
  { "execute", 'e', 0, G_OPTION_ARG_STRING, &opt_command, "Execute a command", NULL },
  { NULL }
};

static void main_cleanup() {
  gtk_main_quit();

  if ( terminal != NULL ) {
    delete(terminal);
    terminal = NULL;
  }

  if ( functions != NULL ) {
    delete(functions);
    functions = NULL;
  }

  if ( configuration != NULL ) {
    delete(configuration);
    configuration = NULL;
  }

  running = false;
}

static void main_exit_cb(GtkWidget *i_widget) {
  main_cleanup();

  exit(EXIT_SUCCESS);
}

static void main_exit_with_status_cb(GtkWidget* i_widget, int status) {
  main_cleanup();

  exit(WIFEXITED(status) ? WEXITSTATUS(status) : EXIT_FAILURE);
}

static gboolean parse_commandline(int argc, char* argv[]) {
  gboolean success = true;
  GError *error = NULL;

  GOptionContext *context = g_option_context_new(NULL);

  g_option_context_set_summary(context, "STerm - a simple terminal emulator based on the VTE library");
  g_option_context_add_main_entries(context, options, NULL);
  g_option_context_add_group(context, gtk_get_option_group(true));

  if ( ! g_option_context_parse(context, &argc, &argv, &error) ) {
    g_warning("failed to parse the commandline options: %s", error->message);
    g_error_free(error);
    success = false;
  }

  g_option_context_free(context);

  return(success);
}

int main(int argc, char *argv[]) {
  gtk_init(&argc, &argv);

  if ( ! parse_commandline(argc, argv) )
    return(EXIT_FAILURE);

  if ( opt_config_file != NULL )
    config_file = opt_config_file;

  configuration = new sterm::config(config_file);

  gtk_window_set_default_icon_name("utilities-terminal");

  main_window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
  gtk_window_set_title(GTK_WINDOW(main_window), "STerm");
  gtk_container_set_border_width(GTK_CONTAINER(main_window), 0);

  terminal = new sterm::terminal(configuration);

  std::string child_command;
  if ( opt_command != NULL )
    child_command = opt_command;

  terminal->attach_to_container(GTK_CONTAINER(main_window));

  terminal->spawn_child(child_command);
  terminal->connect_callback("child-exited", G_CALLBACK(main_exit_with_status_cb), NULL);

  terminal->connect_callback("destroy", G_CALLBACK(main_exit_cb), NULL);
  terminal->link_property_to_terminal("window-title", G_OBJECT(main_window), "title");
  g_signal_connect(G_OBJECT(main_window), "destroy", G_CALLBACK(main_exit_cb), NULL);

  functions = new sterm::function_handler(configuration, terminal);

  gtk_widget_show_all(main_window);
  running = true;
  gtk_main();

  main_cleanup();
  return(EXIT_FAILURE);
}

<|endoftext|>


<commit_before><commit_msg>fix typo in ssl patch<commit_after><|endoftext|>


<commit_before><commit_msg>use more reads, even if strand != xs<commit_after><|endoftext|>


<commit_before>#include <stdio.h>
#include <cmath>

#include <geometry_msgs/PoseStamped.h>
#include <jsk_rviz_plugins/OverlayText.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>

#include <autoware_msgs/lane.h>
#include <autoware_msgs/traffic_light.h>

#include <cross_road_area.hpp>
#include <decision_maker_node.hpp>
#include <state.hpp>
#include <state_context.hpp>

namespace decision_maker
{
void DecisionMakerNode::callbackFromCurrentPose(const geometry_msgs::PoseStamped &msg)
{
  geometry_msgs::PoseStamped _pose = current_pose_ = msg;
  bool initLocalizationFlag = ctx->isCurrentState(state_machine::INITIAL_LOCATEVEHICLE_STATE);
  if (initLocalizationFlag &&
      isLocalizationConvergence(_pose.pose.position.x, _pose.pose.position.y, _pose.pose.position.z,
                                _pose.pose.orientation.x, _pose.pose.orientation.y, _pose.pose.orientation.z))
  {
    ROS_INFO("Localization was convergence");
  }
}

bool DecisionMakerNode::handleStateCmd(const uint64_t _state_num)
{
  bool _ret;
  ctx->setEnableForceSetState(true);
  if (!ctx->isCurrentState(_state_num))
  {
    _ret = ctx->setCurrentState((state_machine::StateFlags)_state_num);
    if(_state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE
		    || _state_num == state_machine::DRIVE_BEHAVIOR_TRAFFIC_LIGHT_GREEN_STATE){
	    isManualLight = true;
    }
  }
  else
  {
    _ret = ctx->disableCurrentState((state_machine::StateFlags)_state_num);
    if(_state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE
		    || _state_num == state_machine::DRIVE_BEHAVIOR_TRAFFIC_LIGHT_GREEN_STATE){
	    isManualLight = false;
  }
  ctx->setEnableForceSetState(false);

  return _ret;
}

void DecisionMakerNode::callbackFromSimPose(const geometry_msgs::PoseStamped &msg)
{
  ROS_INFO("Received system is going to simulation mode");
  handleStateCmd(state_machine::DRIVE_STATE);
  Subs["sim_pose"].shutdown();
}

void DecisionMakerNode::callbackFromStateCmd(const std_msgs::Int32 &msg)
{
  ROS_INFO("Received forcing state changing request: %llx", 1ULL << (uint64_t)msg.data);
  handleStateCmd((uint64_t)1ULL << (uint64_t)msg.data);
}

void DecisionMakerNode::callbackFromLaneChangeFlag(const std_msgs::Int32 &msg)
{
  if (msg.data == enumToInteger<E_ChangeFlags>(E_ChangeFlags::LEFT))
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
    ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
  }
  else if (msg.data == enumToInteger<E_ChangeFlags>(E_ChangeFlags::RIGHT))
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
    ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
  }
  else
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
  }
}

void DecisionMakerNode::callbackFromConfig(const autoware_msgs::ConfigDecisionMaker &msg)
{
  ROS_INFO("Param setted by Runtime Manager");
  enableDisplayMarker = msg.enable_display_marker;
  ctx->setEnableForceSetState(msg.enable_force_state_change);

  param_target_waypoint_ = msg.target_waypoint;
  param_stopline_target_waypoint_ = msg.stopline_target_waypoint;
  param_shift_width_ = msg.shift_width;
}

void DecisionMakerNode::callbackFromLightColor(const ros::MessageEvent<autoware_msgs::traffic_light const> &event)
{
  const autoware_msgs::traffic_light *light = event.getMessage().get();
//  const ros::M_string &header = event.getConnectionHeader();
//  std::string topic = header.at("topic");

  if(!isManualLight){// && topic.find("manage") == std::string::npos){
	  current_traffic_light_ = light->traffic_light;
	  if (current_traffic_light_ == state_machine::E_RED || current_traffic_light_ == state_machine::E_YELLOW)
	  {
		  ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE);
		  ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE);
	  }
	  else
	  {
		  ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE);
		  ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE);
	  }
  }
}

void DecisionMakerNode::callbackFromPointsRaw(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
  if (ctx->setCurrentState(state_machine::INITIAL_LOCATEVEHICLE_STATE))
    Subs["points_raw"].shutdown();
}

void DecisionMakerNode::insertPointWithinCrossRoad(const std::vector<CrossRoadArea> &_intersects,
                                                   autoware_msgs::LaneArray &lane_array)
{
  for (auto &lane : lane_array.lanes)
  {
    for (auto &wp : lane.waypoints)
    {
      geometry_msgs::Point pp;
      pp.x = wp.pose.pose.position.x;
      pp.y = wp.pose.pose.position.y;
      pp.z = wp.pose.pose.position.z;

      for (auto &area : intersects)
      {
        if (CrossRoadArea::isInsideArea(&area, pp))
        {
          // area's
          if (area.insideLanes.empty() || wp.gid != area.insideLanes.back().waypoints.back().gid + 1)
          {
            autoware_msgs::lane nlane;
            area.insideLanes.push_back(nlane);
          }
          area.insideLanes.back().waypoints.push_back(wp);
          area.insideWaypoint_points.push_back(pp);  // geometry_msgs::point
          // area.insideLanes.Waypoints.push_back(wp);//autoware_msgs::waypoint
          // lane's wp
          wp.wpstate.aid = area.area_id;
        }
      }
    }
  }
}

geometry_msgs::Point to_geoPoint(const vector_map_msgs::Point &vp)
{
  geometry_msgs::Point gp;
  gp.x = vp.ly;
  gp.y = vp.bx;
  gp.z = vp.h;
  return gp;
}

void DecisionMakerNode::setWaypointState(autoware_msgs::LaneArray &lane_array)
{
  insertPointWithinCrossRoad(intersects, lane_array);
  // STR
  for (auto &area : intersects)
  {
    for (auto &laneinArea : area.insideLanes)
    {
      // To straight/left/right recognition by using angle
      // between first-waypoint and end-waypoint in intersection area.
      int angle_deg = ((int)std::floor(calcIntersectWayAngle(laneinArea)));  // normalized
      int steering_state;

      if (angle_deg <= ANGLE_LEFT)
        steering_state = autoware_msgs::WaypointState::STR_LEFT;
      else if (angle_deg >= ANGLE_RIGHT)
        steering_state = autoware_msgs::WaypointState::STR_RIGHT;
      else
        steering_state = autoware_msgs::WaypointState::STR_STRAIGHT;

      for (auto &wp_lane : laneinArea.waypoints)
        for (auto &lane : lane_array.lanes)
          for (auto &wp : lane.waypoints)
            if (wp.gid == wp_lane.gid && wp.wpstate.aid == area.area_id)
            {
              wp.wpstate.steering_state = steering_state;
            }
    }
  }
  // STOP
  std::vector<StopLine> stoplines = g_vmap.findByFilter([&](const StopLine &stopline) {
    return (g_vmap.findByKey(Key<RoadSign>(stopline.signid)).type == (int)vector_map_msgs::RoadSign::TYPE_STOP);
  });

  for (auto &lane : lane_array.lanes)
  {
    for (size_t wp_idx = 0; wp_idx < lane.waypoints.size() - 1; wp_idx++)
    {
      for (auto &stopline : stoplines)
      {
        geometry_msgs::Point bp =
            to_geoPoint(g_vmap.findByKey(Key<Point>(g_vmap.findByKey(Key<Line>(stopline.lid)).bpid)));
        geometry_msgs::Point fp =
            to_geoPoint(g_vmap.findByKey(Key<Point>(g_vmap.findByKey(Key<Line>(stopline.lid)).fpid)));

        if (amathutils::isIntersectLine(lane.waypoints.at(wp_idx).pose.pose.position.x,
                                        lane.waypoints.at(wp_idx).pose.pose.position.y,
                                        lane.waypoints.at(wp_idx + 1).pose.pose.position.x,
                                        lane.waypoints.at(wp_idx + 1).pose.pose.position.y, bp.x, bp.y, fp.x, fp.y))
        {
          geometry_msgs::Point center_point;
          center_point.x = (bp.x * 2 + fp.x) / 3;
          center_point.y = (bp.y * 2 + fp.y) / 3;
          if (amathutils::isPointLeftFromLine(
                  center_point.x, center_point.y, lane.waypoints.at(wp_idx).pose.pose.position.x,
                  lane.waypoints.at(wp_idx).pose.pose.position.y, lane.waypoints.at(wp_idx + 1).pose.pose.position.x,
                  lane.waypoints.at(wp_idx + 1).pose.pose.position.y))
          {
            lane.waypoints.at(wp_idx).wpstate.stopline_state = 1;
            // lane.waypoints.at(wp_idx + 1).wpstate.stopline_state = 1;
          }
        }
      }
    }
  }
}

// for based waypoint
void DecisionMakerNode::callbackFromLaneWaypoint(const autoware_msgs::LaneArray &msg)
{
  ROS_INFO("[%s]:LoadedWaypointLaneArray\n", __func__);
  current_based_lane_array_ = msg;  // cached based path
  // indexing
  int gid = 0;
  for (auto &lane : current_based_lane_array_.lanes)
  {
    int lid = 0;
    for (auto &wp : lane.waypoints)
    {
      wp.gid = gid++;
      wp.lid = lid++;
      wp.wpstate.aid = 0;
      wp.wpstate.steering_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.accel_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.stopline_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.lanechange_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.event_state = 0;
    }
  }
  setWaypointState(current_based_lane_array_);
  current_controlled_lane_array_ = current_shifted_lane_array_ = current_based_lane_array_;  // controlled path

  publishControlledLaneArray();
  updateLaneWaypointsArray();
}

state_machine::StateFlags getStateFlags(uint8_t msg_state)
{
  if (msg_state == (uint8_t)autoware_msgs::WaypointState::STR_LEFT)
    return state_machine::DRIVE_STR_LEFT_STATE;
  else if (msg_state == (uint8_t)autoware_msgs::WaypointState::STR_RIGHT)
    return state_machine::DRIVE_STR_RIGHT_STATE;
  else
    return state_machine::DRIVE_STR_STRAIGHT_STATE;
}

void DecisionMakerNode::callbackFromFinalWaypoint(const autoware_msgs::lane &msg)
{
  if (!hasvMap())
  {
    std::cerr << "Not found vmap subscribe" << std::endl;
    return;
  }

  if (!ctx->isCurrentState(state_machine::DRIVE_STATE))
  {
    std::cerr << "State is not DRIVE_STATE[" << ctx->getCurrentStateName() << "]" << std::endl;
    return;
  }
  // cached
  current_finalwaypoints_ = msg;

  size_t idx = current_finalwaypoints_.waypoints.size() - 1 > param_stopline_target_waypoint_ ?
                   param_stopline_target_waypoint_ :
                   current_finalwaypoints_.waypoints.size() - 1;

  if (current_finalwaypoints_.waypoints.at(idx).wpstate.stopline_state)
	  ctx->setCurrentState(state_machine::DRIVE_ACC_STOPLINE_STATE);
  // steering
  idx = current_finalwaypoints_.waypoints.size() - 1 > param_target_waypoint_ ?
            param_target_waypoint_ :
            current_finalwaypoints_.waypoints.size() - 1;
  if (ctx->isCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE))
  {
	  ctx->setCurrentState(state_machine::DRIVE_STR_LEFT_STATE);
  }
  if (ctx->isCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE))
  {
	  ctx->setCurrentState(state_machine::DRIVE_STR_RIGHT_STATE);
  }
  else
  {
	  ctx->setCurrentState(getStateFlags(current_finalwaypoints_.waypoints.at(idx).wpstate.steering_state));
  }

  // for publish plan of velocity
  publishToVelocityArray();
}
void DecisionMakerNode::callbackFromTwistCmd(const geometry_msgs::TwistStamped &msg)
{
  static bool Twistflag = false;

  if (Twistflag)
    ctx->handleTwistCmd(false);
  else
    Twistflag = true;
}

void DecisionMakerNode::callbackFromClosestWaypoint(const std_msgs::Int32 &msg)
{
  closest_waypoint_ = msg.data;
}

void DecisionMakerNode::callbackFromVectorMapArea(const vector_map_msgs::AreaArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapPoint(const vector_map_msgs::PointArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapLine(const vector_map_msgs::LineArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapCrossRoad(const vector_map_msgs::CrossRoadArray &msg)
{
  initVectorMap();
}

void DecisionMakerNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStamped &msg)
{
  current_velocity_ = amathutils::mps2kmph(msg.twist.linear.x);
}
}
<commit_msg>fix a state num name<commit_after>#include <stdio.h>
#include <cmath>

#include <geometry_msgs/PoseStamped.h>
#include <jsk_rviz_plugins/OverlayText.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>

#include <autoware_msgs/lane.h>
#include <autoware_msgs/traffic_light.h>

#include <cross_road_area.hpp>
#include <decision_maker_node.hpp>
#include <state.hpp>
#include <state_context.hpp>

namespace decision_maker
{
void DecisionMakerNode::callbackFromCurrentPose(const geometry_msgs::PoseStamped &msg)
{
  geometry_msgs::PoseStamped _pose = current_pose_ = msg;
  bool initLocalizationFlag = ctx->isCurrentState(state_machine::INITIAL_LOCATEVEHICLE_STATE);
  if (initLocalizationFlag &&
      isLocalizationConvergence(_pose.pose.position.x, _pose.pose.position.y, _pose.pose.position.z,
                                _pose.pose.orientation.x, _pose.pose.orientation.y, _pose.pose.orientation.z))
  {
    ROS_INFO("Localization was convergence");
  }
}

bool DecisionMakerNode::handleStateCmd(const uint64_t _state_num)
{
  bool _ret;
  ctx->setEnableForceSetState(true);
  if (!ctx->isCurrentState(_state_num))
  {
    _ret = ctx->setCurrentState((state_machine::StateFlags)_state_num);
    if(_state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHTRED_STATE
		    || _state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE){
	    isManualLight = true;
    }
  }
  else
  {
    _ret = ctx->disableCurrentState((state_machine::StateFlags)_state_num);
    if(_state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE
		    || _state_num == state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE){
	    isManualLight = false;
  }
  ctx->setEnableForceSetState(false);

  return _ret;
}

void DecisionMakerNode::callbackFromSimPose(const geometry_msgs::PoseStamped &msg)
{
  ROS_INFO("Received system is going to simulation mode");
  handleStateCmd(state_machine::DRIVE_STATE);
  Subs["sim_pose"].shutdown();
}

void DecisionMakerNode::callbackFromStateCmd(const std_msgs::Int32 &msg)
{
  ROS_INFO("Received forcing state changing request: %llx", 1ULL << (uint64_t)msg.data);
  handleStateCmd((uint64_t)1ULL << (uint64_t)msg.data);
}

void DecisionMakerNode::callbackFromLaneChangeFlag(const std_msgs::Int32 &msg)
{
  if (msg.data == enumToInteger<E_ChangeFlags>(E_ChangeFlags::LEFT))
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
    ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
  }
  else if (msg.data == enumToInteger<E_ChangeFlags>(E_ChangeFlags::RIGHT))
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
    ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
  }
  else
  {
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE);
    ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE);
  }
}

void DecisionMakerNode::callbackFromConfig(const autoware_msgs::ConfigDecisionMaker &msg)
{
  ROS_INFO("Param setted by Runtime Manager");
  enableDisplayMarker = msg.enable_display_marker;
  ctx->setEnableForceSetState(msg.enable_force_state_change);

  param_target_waypoint_ = msg.target_waypoint;
  param_stopline_target_waypoint_ = msg.stopline_target_waypoint;
  param_shift_width_ = msg.shift_width;
}

void DecisionMakerNode::callbackFromLightColor(const ros::MessageEvent<autoware_msgs::traffic_light const> &event)
{
  const autoware_msgs::traffic_light *light = event.getMessage().get();
//  const ros::M_string &header = event.getConnectionHeader();
//  std::string topic = header.at("topic");

  if(!isManualLight){// && topic.find("manage") == std::string::npos){
	  current_traffic_light_ = light->traffic_light;
	  if (current_traffic_light_ == state_machine::E_RED || current_traffic_light_ == state_machine::E_YELLOW)
	  {
		  ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE);
		  ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE);
	  }
	  else
	  {
		  ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE);
		  ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE);
	  }
  }
}

void DecisionMakerNode::callbackFromPointsRaw(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
  if (ctx->setCurrentState(state_machine::INITIAL_LOCATEVEHICLE_STATE))
    Subs["points_raw"].shutdown();
}

void DecisionMakerNode::insertPointWithinCrossRoad(const std::vector<CrossRoadArea> &_intersects,
                                                   autoware_msgs::LaneArray &lane_array)
{
  for (auto &lane : lane_array.lanes)
  {
    for (auto &wp : lane.waypoints)
    {
      geometry_msgs::Point pp;
      pp.x = wp.pose.pose.position.x;
      pp.y = wp.pose.pose.position.y;
      pp.z = wp.pose.pose.position.z;

      for (auto &area : intersects)
      {
        if (CrossRoadArea::isInsideArea(&area, pp))
        {
          // area's
          if (area.insideLanes.empty() || wp.gid != area.insideLanes.back().waypoints.back().gid + 1)
          {
            autoware_msgs::lane nlane;
            area.insideLanes.push_back(nlane);
          }
          area.insideLanes.back().waypoints.push_back(wp);
          area.insideWaypoint_points.push_back(pp);  // geometry_msgs::point
          // area.insideLanes.Waypoints.push_back(wp);//autoware_msgs::waypoint
          // lane's wp
          wp.wpstate.aid = area.area_id;
        }
      }
    }
  }
}

geometry_msgs::Point to_geoPoint(const vector_map_msgs::Point &vp)
{
  geometry_msgs::Point gp;
  gp.x = vp.ly;
  gp.y = vp.bx;
  gp.z = vp.h;
  return gp;
}

void DecisionMakerNode::setWaypointState(autoware_msgs::LaneArray &lane_array)
{
  insertPointWithinCrossRoad(intersects, lane_array);
  // STR
  for (auto &area : intersects)
  {
    for (auto &laneinArea : area.insideLanes)
    {
      // To straight/left/right recognition by using angle
      // between first-waypoint and end-waypoint in intersection area.
      int angle_deg = ((int)std::floor(calcIntersectWayAngle(laneinArea)));  // normalized
      int steering_state;

      if (angle_deg <= ANGLE_LEFT)
        steering_state = autoware_msgs::WaypointState::STR_LEFT;
      else if (angle_deg >= ANGLE_RIGHT)
        steering_state = autoware_msgs::WaypointState::STR_RIGHT;
      else
        steering_state = autoware_msgs::WaypointState::STR_STRAIGHT;

      for (auto &wp_lane : laneinArea.waypoints)
        for (auto &lane : lane_array.lanes)
          for (auto &wp : lane.waypoints)
            if (wp.gid == wp_lane.gid && wp.wpstate.aid == area.area_id)
            {
              wp.wpstate.steering_state = steering_state;
            }
    }
  }
  // STOP
  std::vector<StopLine> stoplines = g_vmap.findByFilter([&](const StopLine &stopline) {
    return (g_vmap.findByKey(Key<RoadSign>(stopline.signid)).type == (int)vector_map_msgs::RoadSign::TYPE_STOP);
  });

  for (auto &lane : lane_array.lanes)
  {
    for (size_t wp_idx = 0; wp_idx < lane.waypoints.size() - 1; wp_idx++)
    {
      for (auto &stopline : stoplines)
      {
        geometry_msgs::Point bp =
            to_geoPoint(g_vmap.findByKey(Key<Point>(g_vmap.findByKey(Key<Line>(stopline.lid)).bpid)));
        geometry_msgs::Point fp =
            to_geoPoint(g_vmap.findByKey(Key<Point>(g_vmap.findByKey(Key<Line>(stopline.lid)).fpid)));

        if (amathutils::isIntersectLine(lane.waypoints.at(wp_idx).pose.pose.position.x,
                                        lane.waypoints.at(wp_idx).pose.pose.position.y,
                                        lane.waypoints.at(wp_idx + 1).pose.pose.position.x,
                                        lane.waypoints.at(wp_idx + 1).pose.pose.position.y, bp.x, bp.y, fp.x, fp.y))
        {
          geometry_msgs::Point center_point;
          center_point.x = (bp.x * 2 + fp.x) / 3;
          center_point.y = (bp.y * 2 + fp.y) / 3;
          if (amathutils::isPointLeftFromLine(
                  center_point.x, center_point.y, lane.waypoints.at(wp_idx).pose.pose.position.x,
                  lane.waypoints.at(wp_idx).pose.pose.position.y, lane.waypoints.at(wp_idx + 1).pose.pose.position.x,
                  lane.waypoints.at(wp_idx + 1).pose.pose.position.y))
          {
            lane.waypoints.at(wp_idx).wpstate.stopline_state = 1;
            // lane.waypoints.at(wp_idx + 1).wpstate.stopline_state = 1;
          }
        }
      }
    }
  }
}

// for based waypoint
void DecisionMakerNode::callbackFromLaneWaypoint(const autoware_msgs::LaneArray &msg)
{
  ROS_INFO("[%s]:LoadedWaypointLaneArray\n", __func__);
  current_based_lane_array_ = msg;  // cached based path
  // indexing
  int gid = 0;
  for (auto &lane : current_based_lane_array_.lanes)
  {
    int lid = 0;
    for (auto &wp : lane.waypoints)
    {
      wp.gid = gid++;
      wp.lid = lid++;
      wp.wpstate.aid = 0;
      wp.wpstate.steering_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.accel_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.stopline_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.lanechange_state = autoware_msgs::WaypointState::NULLSTATE;
      wp.wpstate.event_state = 0;
    }
  }
  setWaypointState(current_based_lane_array_);
  current_controlled_lane_array_ = current_shifted_lane_array_ = current_based_lane_array_;  // controlled path

  publishControlledLaneArray();
  updateLaneWaypointsArray();
}

state_machine::StateFlags getStateFlags(uint8_t msg_state)
{
  if (msg_state == (uint8_t)autoware_msgs::WaypointState::STR_LEFT)
    return state_machine::DRIVE_STR_LEFT_STATE;
  else if (msg_state == (uint8_t)autoware_msgs::WaypointState::STR_RIGHT)
    return state_machine::DRIVE_STR_RIGHT_STATE;
  else
    return state_machine::DRIVE_STR_STRAIGHT_STATE;
}

void DecisionMakerNode::callbackFromFinalWaypoint(const autoware_msgs::lane &msg)
{
  if (!hasvMap())
  {
    std::cerr << "Not found vmap subscribe" << std::endl;
    return;
  }

  if (!ctx->isCurrentState(state_machine::DRIVE_STATE))
  {
    std::cerr << "State is not DRIVE_STATE[" << ctx->getCurrentStateName() << "]" << std::endl;
    return;
  }
  // cached
  current_finalwaypoints_ = msg;

  size_t idx = current_finalwaypoints_.waypoints.size() - 1 > param_stopline_target_waypoint_ ?
                   param_stopline_target_waypoint_ :
                   current_finalwaypoints_.waypoints.size() - 1;

  if (current_finalwaypoints_.waypoints.at(idx).wpstate.stopline_state)
	  ctx->setCurrentState(state_machine::DRIVE_ACC_STOPLINE_STATE);
  // steering
  idx = current_finalwaypoints_.waypoints.size() - 1 > param_target_waypoint_ ?
            param_target_waypoint_ :
            current_finalwaypoints_.waypoints.size() - 1;
  if (ctx->isCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_LEFT_STATE))
  {
	  ctx->setCurrentState(state_machine::DRIVE_STR_LEFT_STATE);
  }
  if (ctx->isCurrentState(state_machine::DRIVE_BEHAVIOR_LANECHANGE_RIGHT_STATE))
  {
	  ctx->setCurrentState(state_machine::DRIVE_STR_RIGHT_STATE);
  }
  else
  {
	  ctx->setCurrentState(getStateFlags(current_finalwaypoints_.waypoints.at(idx).wpstate.steering_state));
  }

  // for publish plan of velocity
  publishToVelocityArray();
}
void DecisionMakerNode::callbackFromTwistCmd(const geometry_msgs::TwistStamped &msg)
{
  static bool Twistflag = false;

  if (Twistflag)
    ctx->handleTwistCmd(false);
  else
    Twistflag = true;
}

void DecisionMakerNode::callbackFromClosestWaypoint(const std_msgs::Int32 &msg)
{
  closest_waypoint_ = msg.data;
}

void DecisionMakerNode::callbackFromVectorMapArea(const vector_map_msgs::AreaArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapPoint(const vector_map_msgs::PointArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapLine(const vector_map_msgs::LineArray &msg)
{
  initVectorMap();
}
void DecisionMakerNode::callbackFromVectorMapCrossRoad(const vector_map_msgs::CrossRoadArray &msg)
{
  initVectorMap();
}

void DecisionMakerNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStamped &msg)
{
  current_velocity_ = amathutils::mps2kmph(msg.twist.linear.x);
}
}
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <humanoid_catching/CalculateTorques.h>
#include <Moby/qpOASES.h>
#include <Ravelin/VectorNd.h>
#include <Ravelin/MatrixNd.h>
#include <Ravelin/Quatd.h>
#include <Ravelin/Opsd.h>
#include <Moby/qpOASES.h>
#include <Ravelin/LinAlgd.h>

namespace {
using namespace std;
using namespace humanoid_catching;
using namespace Ravelin;

static const double GRAVITY = 9.81;

class Balancer {
private:
    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! Balancer Service
    ros::ServiceServer balancerService;

public:
   Balancer() :
     pnh("~") {

       balancerService = nh.advertiseService("/balancer/torques",
            &Balancer::calculateTorques, this);
	}

private:

    /**
     * Convert a geometry_msgs::Vector3 to a Ravelin::Vector3d
     * @param v3 geometry_msgs::Vector3
     * @return Ravelin Vector3d
     */
    static Vector3d toVector(const geometry_msgs::Vector3& v3) {
        Vector3d v;
        v[0] = v3.x;
        v[1] = v3.y;
        v[2] = v3.z;
        return v;
    }

    /**
     * Convert a geometry_msgs::Point to a Ravelin::Vector3d
     * @param p geometry_msgs::Point
     * @return Ravelin Vector3d
     */
    static Vector3d toVector(const geometry_msgs::Point& p) {
        Vector3d v;
        v[0] = p.x;
        v[1] = p.y;
        v[2] = p.z;
        return v;
    }

    /**
     * Calculate balancing torques
     * @param req Request
     * @param res Response
     * @return Success
     */
    bool calculateTorques(humanoid_catching::CalculateTorques::Request& req,
                          humanoid_catching::CalculateTorques::Response& res) {

      ROS_INFO("Calculating torques frame %s", req.header.frame_id.c_str());

      // x
      // linear velocity of body
      ROS_INFO("Calculating x vector");
      const Vector3d x = toVector(req.body_velocity.linear);
      ROS_INFO_STREAM("x: " << x);
      // w
      // angular velocity of body
      ROS_INFO("Calculating w vector");
      const Vector3d w = toVector(req.body_velocity.angular);
      ROS_INFO_STREAM("w: " << w);

      // v(t)
      // | x |
      // | w |
      ROS_INFO("Calculating v vector");
      VectorNd v(6);
      v.set_sub_vec(0, x);
      v.set_sub_vec(3, w);
      ROS_INFO_STREAM("v: " << v);

      // R
      // Pole rotation matrix
      ROS_INFO("Calculating R matrix");
      const Matrix3d R = MatrixNd(Quatd(req.body_com.orientation.x, req.body_com.orientation.y, req.body_com.orientation.z, req.body_com.orientation.w));
      ROS_INFO_STREAM("R: " << R);

      // J
      // Pole inertia matrix
      ROS_INFO("Calculating J matrix");
      ROS_INFO_STREAM("BIM: " << req.body_inertia_matrix.size());
      const Matrix3d J = Matrix3d(&req.body_inertia_matrix[0]);
      ROS_INFO_STREAM("J: " << J);

      // JRobot
      // Robot end effector jacobian matrix
      ROS_INFO("Calculating JRobot");
      const MatrixNd JRobot = MatrixNd(req.torque_limits.size(), 6, &req.jacobian_matrix[0]);
      ROS_INFO_STREAM("JRobot: " << JRobot);

      // RJR_t
      ROS_INFO("Calculating RJR");
      Matrix3d RJR;
      Matrix3d RTranspose = R.transpose(RTranspose);
      RJR = R.mult(J, RJR).mult(RTranspose, RJR);
      ROS_INFO_STREAM("RJR: " << RJR);

      // M
      // | Im   0 |
      // | 0 RJR_t|
      ROS_INFO("Calculating M");
      MatrixNd M(6, 6);
      M.set_zero(M.rows(), M.columns());
      M.set_sub_mat(0, 0, Matrix3d::identity() * req.body_mass);
      M.set_sub_mat(3, 3, RJR);
      ROS_INFO_STREAM("M: " << M);

      // delta t
      double deltaT = req.time_delta.toSec();

      // Working vector
      Vector3d tempVector;

      // fext
      // | g           |
      // | -w x RJR_tw |
      ROS_INFO("Calculating fExt");
      VectorNd fExt(6);
      fExt[0] = 0;
      fExt[1] = 0;
      fExt[2] = GRAVITY;
      fExt.set_sub_vec(3, Vector3d::cross(-w, RJR.mult(w, tempVector)));
      ROS_INFO_STREAM("fExt: " << fExt);

      // n_hat
      // x component of ground contact position
      ROS_INFO("Calculating nHat");
      Vector3d nHat;
      nHat[0] = req.ground_contact.x;
      nHat[1] = 0;
      nHat[2] = 0;
      ROS_INFO_STREAM("nHat: " << nHat);

      // s_hat
      // s component of contact position
      ROS_INFO("Calculating sHat");
      Vector3d sHat;
      nHat[0] = 0;
      nHat[1] = req.ground_contact.y;
      nHat[2] = 0;
      ROS_INFO_STREAM("sHat: " << sHat);

      // t_hat
      // z component of contact position
      ROS_INFO("Calculating tHat");
      Vector3d tHat;
      tHat[0] = 0;
      tHat[1] = 0;
      tHat[2] = req.ground_contact.z;
      ROS_INFO_STREAM("tHat: " << tHat);

      // q_hat
      // contact normal
      ROS_INFO("Calculating qHat");
      const Vector3d qHat = toVector(req.contact_normal);
      ROS_INFO_STREAM("qHat: " << qHat);

      // p
      // contact point
      ROS_INFO("Calculating P");
      const Vector3d p = toVector(req.ground_contact);
      ROS_INFO_STREAM("p: " << p);

      // x_bar
      // pole COM
      ROS_INFO("Calculating xBar");
      const Vector3d xBar = toVector(req.body_com.position);
      ROS_INFO_STREAM("xBar: " << xBar);

      // r
      ROS_INFO("Calculating r");
      const Vector3d r = p - xBar;
      ROS_INFO_STREAM("r: " << r);

      // N
      // | n_hat     |
      // | r x n_hat |
      ROS_INFO("Calculating N");
      VectorNd N(6);
      N.set_sub_vec(0, nHat);
      N.set_sub_vec(3, r.cross(nHat, tempVector));
      ROS_INFO_STREAM("N: " << N);

      // S
      // | s_hat     |
      // | r x s_hat |
      ROS_INFO("Calculating S");
      VectorNd S(6);
      S.set_sub_vec(0, sHat);
      S.set_sub_vec(3, r.cross(sHat, tempVector));
      ROS_INFO_STREAM("S: " << S);

      // T
      // | t_hat     |
      // | r x t_hat |
      ROS_INFO("Calculating T");
      VectorNd T(6);
      T.set_sub_vec(0, tHat);
      T.set_sub_vec(3, r.cross(tHat, tempVector));
      ROS_INFO_STREAM("T: " << T);

      // Q
      ROS_INFO("Calculating Q");
      VectorNd Q(6);
      Q.set_sub_vec(0, qHat);
      Q.set_sub_vec(3, -r.cross(qHat, tempVector));
      ROS_INFO_STREAM("Q: " << Q);

      // Result vector
      // Torques, f_n, f_s, f_t, f_robot, v_(t + tdelta)
      const int torqueIdx = 0;
      const int fNIdx = req.torque_limits.size();
      const int fSIdx = fNIdx + 1;
      const int fTIdx = fSIdx + 1;
      const int fRobotIdx = fTIdx + 1;
      const int vTDeltaIdx = fRobotIdx + 1;
      VectorNd z(req.torque_limits.size() + 1 + 1 + 1 + 1 + 6);

      // Set up minimization function
      ROS_INFO("Calculating H");
      MatrixNd H(6, z.size());
      H.set_sub_mat(0, vTDeltaIdx, M);
      ROS_INFO_STREAM("H: " << H);

      ROS_INFO("Calculating c");
      VectorNd c(6);
      c.set_zero(c.rows());
      ROS_INFO_STREAM("c: " << c);

      // Linear equality constraints
      ROS_INFO("Calculating A + b");
      MatrixNd A(1 * 2 + req.torque_limits.size() + 6, z.size());
      VectorNd b(1 * 2 + req.torque_limits.size() + 6);

      ROS_INFO("Setting Sv constraint");
      // Sv(t + t_delta) = 0 (no tangent velocity)
      unsigned idx = 0;
      A.set_sub_mat(idx, vTDeltaIdx, S);
      b.set_sub_vec(idx, VectorNd::zero(1));
      idx += 1;

      ROS_INFO("Setting Tv constraint");
      // Tv(t + t_delta) = 0 (no tangent velocity)
      A.set_sub_mat(idx, vTDeltaIdx, T);
      b.set_sub_vec(idx, VectorNd::zero(1));
      idx += 1;

      ROS_INFO("Setting torque constraint");
      // J_robot(transpose) * Q(transpose) * f_robot = torques
      // Transformed to:
      // J_Robot(transpose) * Q(transpose) * f_robot - I * torques = 0
      MatrixNd JQ(req.torque_limits.size(), 1);
      MatrixNd Jt = JRobot;
      Jt.transpose();
      MatrixNd Qt(Q, eTranspose);
      Jt.mult(Qt, JQ);
      A.set_sub_mat(idx, fRobotIdx, JQ);
      A.set_sub_mat(idx, torqueIdx, MatrixNd::identity(req.torque_limits.size()).negate());
      b.set_sub_vec(idx, VectorNd::zero(req.torque_limits.size()));
      idx += req.torque_limits.size();

      ROS_INFO("Setting velocity constraint");
      // v_(t + t_delta) = v_t + M_inv (N_t * f_n + S_t * f_s + T_t * f_t + delta_t * f_ext + Q_t * delta_t * f_robot)
      // Manipulated to fit constraint form
      // -v_t - M_inv * delta_t * f_ext = M_inv * N_t * f_n + M_inv * S_t * f_s + M_inv * T_t * f_t + M_inv * Q_t * delta_t * f_robot + -I * v_(t + t_delta)
      LinAlgd linAlgd;
      MatrixNd MInverse = M;
      linAlgd.pseudo_invert(MInverse);

      MatrixNd MInverseN(MInverse.rows(), N.columns());
      MInverse.mult(N, MInverseN);
      A.set_sub_mat(idx, fNIdx, MInverseN);

      MatrixNd MInverseS(MInverse.rows(), S.columns());
      MInverse.mult(S, MInverseS);
      A.set_sub_mat(idx, fSIdx, MInverseS);

      MatrixNd MInverseT(MInverse.rows(), T.columns());
      MInverse.mult(T, MInverseT);
      A.set_sub_mat(idx, fTIdx, MInverseT);

      MatrixNd MInverseQ(MInverse.rows(), Q.columns());
      MInverse.mult(Q, MInverseQ);
      MInverseQ *= deltaT;
      A.set_sub_mat(idx, fRobotIdx, MInverseQ);

      A.set_sub_mat(idx, vTDeltaIdx, MatrixNd::identity(6).negate());

      VectorNd MInverseFExt(6);
      MInverseFExt.set_zero();
      MInverse.mult(fExt, MInverseFExt, deltaT);
      MInverseFExt.negate() -= v;
      b.set_sub_vec(idx, MInverseFExt);
      ROS_INFO_STREAM("A: " << A);
      ROS_INFO_STREAM("b: " << b);

      // Linear inequality constraints
      ROS_INFO("Calculating Mc and q");
      MatrixNd Mc(6, z.size());
      VectorNd q(6);

      // Nv(t) >= 0 (non-negative normal velocity)
      Mc.set_sub_mat(0, vTDeltaIdx, N);
      q.set_sub_vec(0, VectorNd::zero(6));
      ROS_INFO_STREAM("Mc: " << Mc);
      ROS_INFO_STREAM("q: " << q);

      // Solution variable constraint
      ROS_INFO("Calculating lb and ub");
      VectorNd lb(z.size());
      VectorNd ub(z.size());

      // Torque constraints
      unsigned int bound = 0;
      for (bound; bound < req.torque_limits.size(); ++bound) {
        lb[bound] = req.torque_limits[bound].minimum;
        ub[bound] = req.torque_limits[bound].maximum;
      }

      // f_n >= 0
      lb[bound] = 0;
      ub[bound] = INFINITY;
      ++bound;

      // f_s (no constraints)
      lb[bound] = INFINITY;
      ub[bound] = INFINITY;
      ++bound;

      // f_t (no constraints)
      lb[bound] = INFINITY;
      ub[bound] = INFINITY;
      ++bound;

      // f_robot >= 0
      lb[bound] = 0;
      ub[bound] = INFINITY;
      ++bound;

      // v_t (no constraints)
      for (bound; bound < z.size(); ++bound) {
        lb[bound] = INFINITY;
        ub[bound] = INFINITY;
      }
      ROS_INFO_STREAM("lb: " << lb);
      ROS_INFO_STREAM("ub: " << ub);

      // Call solver
      ROS_INFO("Calling solver");
      Moby::QPOASES qp;
      if (!qp.qp_activeset(H, p, lb, ub, Mc, q, A, b, z)){
            ROS_ERROR("QP failed to find feasible point");
            return false;
      }

      ROS_INFO_STREAM("QP solved successfully: " << z);

      // Copy over result
      res.torques.resize(req.torque_limits.size());
      for (unsigned int i = 0; i < z.size(); ++i) {
        res.torques[i] = z[i];
      }
      return true;
    }
  };
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "balancer");
  Balancer bal;
  ros::spin();
}
<commit_msg>Small calculation change<commit_after>#include <ros/ros.h>
#include <humanoid_catching/CalculateTorques.h>
#include <Moby/qpOASES.h>
#include <Ravelin/VectorNd.h>
#include <Ravelin/MatrixNd.h>
#include <Ravelin/Quatd.h>
#include <Ravelin/Opsd.h>
#include <Moby/qpOASES.h>
#include <Ravelin/LinAlgd.h>

namespace {
using namespace std;
using namespace humanoid_catching;
using namespace Ravelin;

static const double GRAVITY = 9.81;

class Balancer {
private:
    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! Balancer Service
    ros::ServiceServer balancerService;

public:
   Balancer() :
     pnh("~") {

       balancerService = nh.advertiseService("/balancer/torques",
            &Balancer::calculateTorques, this);
	}

private:

    /**
     * Convert a geometry_msgs::Vector3 to a Ravelin::Vector3d
     * @param v3 geometry_msgs::Vector3
     * @return Ravelin Vector3d
     */
    static Vector3d toVector(const geometry_msgs::Vector3& v3) {
        Vector3d v;
        v[0] = v3.x;
        v[1] = v3.y;
        v[2] = v3.z;
        return v;
    }

    /**
     * Convert a geometry_msgs::Point to a Ravelin::Vector3d
     * @param p geometry_msgs::Point
     * @return Ravelin Vector3d
     */
    static Vector3d toVector(const geometry_msgs::Point& p) {
        Vector3d v;
        v[0] = p.x;
        v[1] = p.y;
        v[2] = p.z;
        return v;
    }

    /**
     * Calculate balancing torques
     * @param req Request
     * @param res Response
     * @return Success
     */
    bool calculateTorques(humanoid_catching::CalculateTorques::Request& req,
                          humanoid_catching::CalculateTorques::Response& res) {

      ROS_INFO("Calculating torques frame %s", req.header.frame_id.c_str());

      // x
      // linear velocity of body
      ROS_INFO("Calculating x vector");
      const Vector3d x = toVector(req.body_velocity.linear);
      ROS_INFO_STREAM("x: " << x);
      // w
      // angular velocity of body
      ROS_INFO("Calculating w vector");
      const Vector3d w = toVector(req.body_velocity.angular);
      ROS_INFO_STREAM("w: " << w);

      // v(t)
      // | x |
      // | w |
      ROS_INFO("Calculating v vector");
      VectorNd v(6);
      v.set_sub_vec(0, x);
      v.set_sub_vec(3, w);
      ROS_INFO_STREAM("v: " << v);

      // R
      // Pole rotation matrix
      ROS_INFO("Calculating R matrix");
      const Matrix3d R = Quatd(req.body_com.orientation.x, req.body_com.orientation.y, req.body_com.orientation.z, req.body_com.orientation.w);
      ROS_INFO_STREAM("R: " << R);

      // J
      // Pole inertia matrix
      ROS_INFO("Calculating J matrix");
      ROS_INFO_STREAM("BIM: " << req.body_inertia_matrix.size());
      const Matrix3d J = Matrix3d(&req.body_inertia_matrix[0]);
      ROS_INFO_STREAM("J: " << J);

      // JRobot
      // Robot end effector jacobian matrix
      ROS_INFO("Calculating JRobot");
      const MatrixNd JRobot = MatrixNd(req.torque_limits.size(), 6, &req.jacobian_matrix[0]);
      ROS_INFO_STREAM("JRobot: " << JRobot);

      // RJR_t
      ROS_INFO("Calculating RJR");
      Matrix3d RJR;
      Matrix3d RTranspose = R.transpose(RTranspose);
      RJR = R.mult(J, RJR).mult(RTranspose, RJR);
      ROS_INFO_STREAM("RJR: " << RJR);

      // M
      // | Im   0 |
      // | 0 RJR_t|
      ROS_INFO("Calculating M");
      MatrixNd M(6, 6);
      M.set_zero(M.rows(), M.columns());
      M.set_sub_mat(0, 0, Matrix3d::identity() * req.body_mass);
      M.set_sub_mat(3, 3, RJR);
      ROS_INFO_STREAM("M: " << M);

      // delta t
      double deltaT = req.time_delta.toSec();

      // Working vector
      Vector3d tempVector;

      // fext
      // | g           |
      // | -w x RJR_tw |
      ROS_INFO("Calculating fExt");
      VectorNd fExt(6);
      fExt[0] = 0;
      fExt[1] = 0;
      fExt[2] = GRAVITY;
      fExt.set_sub_vec(3, Vector3d::cross(-w, RJR.mult(w, tempVector)));
      ROS_INFO_STREAM("fExt: " << fExt);

      // n_hat
      // x component of ground contact position
      ROS_INFO("Calculating nHat");
      Vector3d nHat;
      nHat[0] = req.ground_contact.x;
      nHat[1] = 0;
      nHat[2] = 0;
      ROS_INFO_STREAM("nHat: " << nHat);

      // s_hat
      // s component of contact position
      ROS_INFO("Calculating sHat");
      Vector3d sHat;
      nHat[0] = 0;
      nHat[1] = req.ground_contact.y;
      nHat[2] = 0;
      ROS_INFO_STREAM("sHat: " << sHat);

      // t_hat
      // z component of contact position
      ROS_INFO("Calculating tHat");
      Vector3d tHat;
      tHat[0] = 0;
      tHat[1] = 0;
      tHat[2] = req.ground_contact.z;
      ROS_INFO_STREAM("tHat: " << tHat);

      // q_hat
      // contact normal
      ROS_INFO("Calculating qHat");
      const Vector3d qHat = toVector(req.contact_normal);
      ROS_INFO_STREAM("qHat: " << qHat);

      // p
      // contact point
      ROS_INFO("Calculating P");
      const Vector3d p = toVector(req.ground_contact);
      ROS_INFO_STREAM("p: " << p);

      // x_bar
      // pole COM
      ROS_INFO("Calculating xBar");
      const Vector3d xBar = toVector(req.body_com.position);
      ROS_INFO_STREAM("xBar: " << xBar);

      // r
      ROS_INFO("Calculating r");
      const Vector3d r = p - xBar;
      ROS_INFO_STREAM("r: " << r);

      // N
      // | n_hat     |
      // | r x n_hat |
      ROS_INFO("Calculating N");
      VectorNd N(6);
      N.set_sub_vec(0, nHat);
      N.set_sub_vec(3, r.cross(nHat, tempVector));
      ROS_INFO_STREAM("N: " << N);

      // S
      // | s_hat     |
      // | r x s_hat |
      ROS_INFO("Calculating S");
      VectorNd S(6);
      S.set_sub_vec(0, sHat);
      S.set_sub_vec(3, r.cross(sHat, tempVector));
      ROS_INFO_STREAM("S: " << S);

      // T
      // | t_hat     |
      // | r x t_hat |
      ROS_INFO("Calculating T");
      VectorNd T(6);
      T.set_sub_vec(0, tHat);
      T.set_sub_vec(3, r.cross(tHat, tempVector));
      ROS_INFO_STREAM("T: " << T);

      // Q
      ROS_INFO("Calculating Q");
      VectorNd Q(6);
      Q.set_sub_vec(0, qHat);
      Q.set_sub_vec(3, -r.cross(qHat, tempVector));
      ROS_INFO_STREAM("Q: " << Q);

      // Result vector
      // Torques, f_n, f_s, f_t, f_robot, v_(t + tdelta)
      const int torqueIdx = 0;
      const int fNIdx = req.torque_limits.size();
      const int fSIdx = fNIdx + 1;
      const int fTIdx = fSIdx + 1;
      const int fRobotIdx = fTIdx + 1;
      const int vTDeltaIdx = fRobotIdx + 1;
      VectorNd z(req.torque_limits.size() + 1 + 1 + 1 + 1 + 6);

      // Set up minimization function
      ROS_INFO("Calculating H");
      MatrixNd H(6, z.size());
      H.set_sub_mat(0, vTDeltaIdx, M);
      ROS_INFO_STREAM("H: " << H);

      ROS_INFO("Calculating c");
      VectorNd c(6);
      c.set_zero(c.rows());
      ROS_INFO_STREAM("c: " << c);

      // Linear equality constraints
      ROS_INFO("Calculating A + b");
      MatrixNd A(1 * 2 + req.torque_limits.size() + 6, z.size());
      VectorNd b(1 * 2 + req.torque_limits.size() + 6);

      ROS_INFO("Setting Sv constraint");
      // Sv(t + t_delta) = 0 (no tangent velocity)
      unsigned idx = 0;
      A.set_sub_mat(idx, vTDeltaIdx, S);
      b.set_sub_vec(idx, VectorNd::zero(1));
      idx += 1;

      ROS_INFO("Setting Tv constraint");
      // Tv(t + t_delta) = 0 (no tangent velocity)
      A.set_sub_mat(idx, vTDeltaIdx, T);
      b.set_sub_vec(idx, VectorNd::zero(1));
      idx += 1;

      ROS_INFO("Setting torque constraint");
      // J_robot(transpose) * Q(transpose) * f_robot = torques
      // Transformed to:
      // J_Robot(transpose) * Q(transpose) * f_robot - I * torques = 0
      MatrixNd JQ(req.torque_limits.size(), 1);
      MatrixNd Jt = JRobot;
      Jt.transpose();
      MatrixNd Qt(Q, eTranspose);
      Jt.mult(Qt, JQ);
      A.set_sub_mat(idx, fRobotIdx, JQ);
      A.set_sub_mat(idx, torqueIdx, MatrixNd::identity(req.torque_limits.size()).negate());
      b.set_sub_vec(idx, VectorNd::zero(req.torque_limits.size()));
      idx += req.torque_limits.size();

      ROS_INFO("Setting velocity constraint");
      // v_(t + t_delta) = v_t + M_inv (N_t * f_n + S_t * f_s + T_t * f_t + delta_t * f_ext + Q_t * delta_t * f_robot)
      // Manipulated to fit constraint form
      // -v_t - M_inv * delta_t * f_ext = M_inv * N_t * f_n + M_inv * S_t * f_s + M_inv * T_t * f_t + M_inv * Q_t * delta_t * f_robot + -I * v_(t + t_delta)
      LinAlgd linAlgd;
      MatrixNd MInverse = M;
      linAlgd.pseudo_invert(MInverse);

      MatrixNd MInverseN(MInverse.rows(), N.columns());
      MInverse.mult(N, MInverseN);
      A.set_sub_mat(idx, fNIdx, MInverseN);

      MatrixNd MInverseS(MInverse.rows(), S.columns());
      MInverse.mult(S, MInverseS);
      A.set_sub_mat(idx, fSIdx, MInverseS);

      MatrixNd MInverseT(MInverse.rows(), T.columns());
      MInverse.mult(T, MInverseT);
      A.set_sub_mat(idx, fTIdx, MInverseT);

      MatrixNd MInverseQ(MInverse.rows(), Q.columns());
      MInverse.mult(Q, MInverseQ);
      MInverseQ *= deltaT;
      A.set_sub_mat(idx, fRobotIdx, MInverseQ);

      A.set_sub_mat(idx, vTDeltaIdx, MatrixNd::identity(6).negate());

      VectorNd MInverseFExt(6);
      MInverseFExt.set_zero();
      MInverse.mult(fExt, MInverseFExt, deltaT);
      MInverseFExt.negate() -= v;
      b.set_sub_vec(idx, MInverseFExt);
      ROS_INFO_STREAM("A: " << A);
      ROS_INFO_STREAM("b: " << b);

      // Linear inequality constraints
      ROS_INFO("Calculating Mc and q");
      MatrixNd Mc(6, z.size());
      VectorNd q(6);

      // Nv(t) >= 0 (non-negative normal velocity)
      Mc.set_sub_mat(0, vTDeltaIdx, N);
      q.set_sub_vec(0, VectorNd::zero(6));
      ROS_INFO_STREAM("Mc: " << Mc);
      ROS_INFO_STREAM("q: " << q);

      // Solution variable constraint
      ROS_INFO("Calculating lb and ub");
      VectorNd lb(z.size());
      VectorNd ub(z.size());

      // Torque constraints
      unsigned int bound = 0;
      for (bound; bound < req.torque_limits.size(); ++bound) {
        lb[bound] = req.torque_limits[bound].minimum;
        ub[bound] = req.torque_limits[bound].maximum;
      }

      // f_n >= 0
      lb[bound] = 0;
      ub[bound] = INFINITY;
      ++bound;

      // f_s (no constraints)
      lb[bound] = INFINITY;
      ub[bound] = INFINITY;
      ++bound;

      // f_t (no constraints)
      lb[bound] = INFINITY;
      ub[bound] = INFINITY;
      ++bound;

      // f_robot >= 0
      lb[bound] = 0;
      ub[bound] = INFINITY;
      ++bound;

      // v_t (no constraints)
      for (bound; bound < z.size(); ++bound) {
        lb[bound] = INFINITY;
        ub[bound] = INFINITY;
      }
      ROS_INFO_STREAM("lb: " << lb);
      ROS_INFO_STREAM("ub: " << ub);

      // Call solver
      ROS_INFO("Calling solver");
      Moby::QPOASES qp;
      if (!qp.qp_activeset(H, p, lb, ub, Mc, q, A, b, z)){
            ROS_ERROR("QP failed to find feasible point");
            return false;
      }

      ROS_INFO_STREAM("QP solved successfully: " << z);

      // Copy over result
      res.torques.resize(req.torque_limits.size());
      for (unsigned int i = 0; i < z.size(); ++i) {
        res.torques[i] = z[i];
      }
      return true;
    }
  };
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "balancer");
  Balancer bal;
  ros::spin();
}
<|endoftext|>


<commit_before>#include <yttrium/log.h>
#include <yttrium/log_manager.h>

#include <yttrium/date_time.h>
#include <yttrium/file.h>
#include <yttrium/pointer.h>
#include "instance_guard.h"

namespace Yttrium
{
	class LogManagerImpl;

	using LogManagerGuard = InstanceGuard<LogManagerImpl>;

	class LogManagerImpl : public LogManager
	{
	public:

		LogManagerImpl(const StaticString& file_name, Allocator& allocator)
			: _allocator(allocator)
			, _std_err(File::StdErr, &_allocator)
			, _instance_guard(this, "Duplicate LogManager construction")
		{
			if (!file_name.is_empty())
				_file = File(file_name, File::Write | File::Truncate, &_allocator);
		}

		Allocator& allocator() const
		{
			return _allocator;
		}

		void write(const StaticString& string)
		{
			_std_err.write(string.text(), string.size());
			_file.write(string.text(), string.size());
			_file.flush();
		}

	private:

		Allocator& _allocator;
		File _std_err;
		File _file;
		LogManagerGuard _instance_guard;
	};

	Pointer<LogManager> LogManager::create(const StaticString& file_name, Allocator& allocator)
	{
		return make_pointer<LogManagerImpl>(allocator, file_name, allocator);
	}

	Log::Log()
		: _message(64, LogManagerGuard::instance ? &LogManagerGuard::instance->allocator() : DefaultAllocator)
		// TODO: Lock mutex.
	{
		const auto& now = DateTime::now();
		_message << '[' << dec(now.hour, 2) << ':' << dec(now.minute, 2) << ':' << dec(now.second, 2) << "] "_s;
	}

	Log::~Log()
	{
		_message << "\r\n"_s;
		std::lock_guard<std::mutex> lock(LogManagerGuard::instance_mutex);
		if (LogManagerGuard::instance)
			LogManagerGuard::instance->write(_message);
	}
}
<commit_msg>Logging doesn't need flushing.<commit_after>#include <yttrium/log.h>
#include <yttrium/log_manager.h>

#include <yttrium/date_time.h>
#include <yttrium/file.h>
#include <yttrium/pointer.h>
#include "instance_guard.h"

namespace Yttrium
{
	class LogManagerImpl;

	using LogManagerGuard = InstanceGuard<LogManagerImpl>;

	class LogManagerImpl : public LogManager
	{
	public:

		LogManagerImpl(const StaticString& file_name, Allocator& allocator)
			: _allocator(allocator)
			, _std_err(File::StdErr, &_allocator)
			, _instance_guard(this, "Duplicate LogManager construction")
		{
			if (!file_name.is_empty())
				_file = File(file_name, File::Write | File::Truncate, &_allocator);
		}

		Allocator& allocator() const
		{
			return _allocator;
		}

		void write(const StaticString& string)
		{
			_std_err.write(string.text(), string.size());
			_file.write(string.text(), string.size());
		}

	private:

		Allocator& _allocator;
		File _std_err;
		File _file;
		LogManagerGuard _instance_guard;
	};

	Pointer<LogManager> LogManager::create(const StaticString& file_name, Allocator& allocator)
	{
		return make_pointer<LogManagerImpl>(allocator, file_name, allocator);
	}

	Log::Log()
		: _message(64, LogManagerGuard::instance ? &LogManagerGuard::instance->allocator() : DefaultAllocator)
		// TODO: Lock mutex.
	{
		const auto& now = DateTime::now();
		_message << '[' << dec(now.hour, 2) << ':' << dec(now.minute, 2) << ':' << dec(now.second, 2) << "] "_s;
	}

	Log::~Log()
	{
		_message << "\r\n"_s;
		std::lock_guard<std::mutex> lock(LogManagerGuard::instance_mutex);
		if (LogManagerGuard::instance)
			LogManagerGuard::instance->write(_message);
	}
}
<|endoftext|>