<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.

#include "chrome/views/controls/label.h"

#include <math.h>

#include "app/gfx/chrome_canvas.h"
#include "app/gfx/chrome_font.h"
#include "app/gfx/insets.h"
#include "app/l10n_util.h"
#include "app/resource_bundle.h"
#include "base/logging.h"
#include "base/string_util.h"
#include "chrome/common/gfx/text_elider.h"
#include "chrome/views/background.h"

namespace views {

const char Label::kViewClassName[] = "chrome/views/Label";

static const SkColor kEnabledColor = SK_ColorBLACK;
static const SkColor kDisabledColor = SkColorSetRGB(161, 161, 146);
static const int kFocusBorderPadding = 1;

Label::Label() {
  Init(L"", GetDefaultFont());
}

Label::Label(const std::wstring& text) {
  Init(text, GetDefaultFont());
}

Label::Label(const std::wstring& text, const ChromeFont& font) {
  Init(text, font);
}

void Label::Init(const std::wstring& text, const ChromeFont& font) {
  contains_mouse_ = false;
  font_ = font;
  text_size_valid_ = false;
  SetText(text);
  url_set_ = false;
  color_ = kEnabledColor;
  horiz_alignment_ = ALIGN_CENTER;
  is_multi_line_ = false;
  allow_character_break_ = false;
  collapse_when_hidden_ = false;
  rtl_alignment_mode_ = USE_UI_ALIGNMENT;
  paint_as_focused_ = false;
  has_focus_border_ = false;
}

Label::~Label() {
}

gfx::Size Label::GetPreferredSize() {
  gfx::Size prefsize;

  // Return a size of (0, 0) if the label is not visible and if the
  // collapse_when_hidden_ flag is set.
  // TODO(munjal): This logic probably belongs to the View class. But for now,
  // put it here since putting it in View class means all inheriting classes
  // need ot respect the collapse_when_hidden_ flag.
  if (!IsVisible() && collapse_when_hidden_)
    return prefsize;

  if (is_multi_line_) {
    int w = width(), h = 0;
    ChromeCanvas::SizeStringInt(text_, font_, &w, &h, ComputeMultiLineFlags());
    prefsize.SetSize(w, h);
  } else {
    prefsize = GetTextSize();
  }

  gfx::Insets insets = GetInsets();
  prefsize.Enlarge(insets.width(), insets.height());
  return prefsize;
}

int Label::ComputeMultiLineFlags() {
  int flags = ChromeCanvas::MULTI_LINE;
  if (allow_character_break_)
    flags |= ChromeCanvas::CHARACTER_BREAK;
  switch (horiz_alignment_) {
    case ALIGN_LEFT:
      flags |= ChromeCanvas::TEXT_ALIGN_LEFT;
      break;
    case ALIGN_CENTER:
      flags |= ChromeCanvas::TEXT_ALIGN_CENTER;
      break;
    case ALIGN_RIGHT:
      flags |= ChromeCanvas::TEXT_ALIGN_RIGHT;
      break;
  }
  return flags;
}

void Label::CalculateDrawStringParams(
    std::wstring* paint_text, gfx::Rect* text_bounds, int* flags) {
  DCHECK(paint_text && text_bounds && flags);

  if (url_set_) {
    // TODO(jungshik) : Figure out how to get 'intl.accept_languages'
    // preference and use it when calling ElideUrl.
    *paint_text = gfx::ElideUrl(url_, font_, width(), std::wstring());

    // An URLs is always treated as an LTR text and therefore we should
    // explicitly mark it as such if the locale is RTL so that URLs containing
    // Hebrew or Arabic characters are displayed correctly.
    //
    // Note that we don't check the View's UI layout setting in order to
    // determine whether or not to insert the special Unicode formatting
    // characters. We use the locale settings because an URL is always treated
    // as an LTR string, even if its containing view does not use an RTL UI
    // layout.
    if (l10n_util::GetTextDirection() == l10n_util::RIGHT_TO_LEFT)
      l10n_util::WrapStringWithLTRFormatting(paint_text);
  } else {
    *paint_text = text_;
  }

  if (is_multi_line_) {
    gfx::Insets insets = GetInsets();
    text_bounds->SetRect(insets.left(),
                         insets.top(),
                         width() - insets.width(),
                         height() - insets.height());
    *flags = ComputeMultiLineFlags();
  } else {
    *text_bounds = GetTextBounds();
    *flags = 0;
  }
}

void Label::Paint(ChromeCanvas* canvas) {
  PaintBackground(canvas);
  std::wstring paint_text;
  gfx::Rect text_bounds;
  int flags = 0;
  CalculateDrawStringParams(&paint_text, &text_bounds, &flags);
  canvas->DrawStringInt(paint_text,
                        font_,
                        color_,
                        text_bounds.x(),
                        text_bounds.y(),
                        text_bounds.width(),
                        text_bounds.height(),
                        flags);

  // The focus border always hugs the text, regardless of the label's bounds.
  if (HasFocus() || paint_as_focused_) {
    int w = text_bounds.width();
    int h = 0;
    // We explicitly OR in MULTI_LINE here since SizeStringInt seems to return
    // an incorrect height for single line text when the MULTI_LINE flag isn't
    // specified. o_O...
    ChromeCanvas::SizeStringInt(paint_text, font_, &w, &h,
                                flags | ChromeCanvas::MULTI_LINE);
    gfx::Rect focus_rect = text_bounds;
    focus_rect.set_width(w);
    focus_rect.set_height(h);
    focus_rect.Inset(-kFocusBorderPadding, -kFocusBorderPadding);
    canvas->DrawFocusRect(focus_rect.x(), focus_rect.y(), focus_rect.width(),
                          focus_rect.height());
  }
}

void Label::PaintBackground(ChromeCanvas* canvas) {
  const Background* bg = contains_mouse_ ? GetMouseOverBackground() : NULL;
  if (!bg)
    bg = background();
  if (bg)
    bg->Paint(canvas, this);
}

void Label::SetFont(const ChromeFont& font) {
  font_ = font;
  text_size_valid_ = false;
  SchedulePaint();
}

ChromeFont Label::GetFont() const {
  return font_;
}

void Label::SetText(const std::wstring& text) {
  text_ = text;
  url_set_ = false;
  text_size_valid_ = false;
  SchedulePaint();
}

void Label::SetURL(const GURL& url) {
  url_ = url;
  text_ = UTF8ToWide(url_.spec());
  url_set_ = true;
  text_size_valid_ = false;
  SchedulePaint();
}

const std::wstring Label::GetText() const {
  if (url_set_)
    return UTF8ToWide(url_.spec());
  else
    return text_;
}

const GURL Label::GetURL() const {
  if (url_set_)
    return url_;
  else
    return GURL(WideToUTF8(text_));
}

gfx::Size Label::GetTextSize() {
  if (!text_size_valid_) {
    text_size_.SetSize(font_.GetStringWidth(text_), font_.height());
    text_size_valid_ = true;
  }

  if (text_size_valid_)
    return text_size_;
  return gfx::Size();
}

int Label::GetHeightForWidth(int w) {
  if (is_multi_line_) {
    gfx::Insets insets = GetInsets();
    w = std::max<int>(0, w - insets.width());
    int h = 0;
    ChromeCanvas cc(0, 0, true);
    cc.SizeStringInt(text_, font_, &w, &h, ComputeMultiLineFlags());
    return h + insets.height();
  }

  return View::GetHeightForWidth(w);
}

std::string Label::GetClassName() const {
  return kViewClassName;
}

void Label::SetColor(const SkColor& color) {
  color_ = color;
}

const SkColor Label::GetColor() const {
  return color_;
}

void Label::SetHorizontalAlignment(Alignment a) {
  // If the View's UI layout is right-to-left and rtl_alignment_mode_ is
  // USE_UI_ALIGNMENT, we need to flip the alignment so that the alignment
  // settings take into account the text directionality.
  if (UILayoutIsRightToLeft() && rtl_alignment_mode_ == USE_UI_ALIGNMENT) {
    if (a == ALIGN_LEFT)
      a = ALIGN_RIGHT;
    else if (a == ALIGN_RIGHT)
      a = ALIGN_LEFT;
  }
  if (horiz_alignment_ != a) {
    horiz_alignment_ = a;
    SchedulePaint();
  }
}

Label::Alignment Label::GetHorizontalAlignment() const {
  return horiz_alignment_;
}

void Label::SetRTLAlignmentMode(RTLAlignmentMode mode) {
  rtl_alignment_mode_ = mode;
}

Label::RTLAlignmentMode Label::GetRTLAlignmentMode() const {
  return rtl_alignment_mode_;
}

void Label::SetMultiLine(bool f) {
  if (f != is_multi_line_) {
    is_multi_line_ = f;
    SchedulePaint();
  }
}

void Label::SetAllowCharacterBreak(bool f) {
  if (f != allow_character_break_) {
    allow_character_break_ = f;
    SchedulePaint();
  }
}

bool Label::IsMultiLine() {
  return is_multi_line_;
}

void Label::SetTooltipText(const std::wstring& tooltip_text) {
  tooltip_text_ = tooltip_text;
}

bool Label::GetTooltipText(int x, int y, std::wstring* tooltip) {
  DCHECK(tooltip);

  // If a tooltip has been explicitly set, use it.
  if (!tooltip_text_.empty()) {
    tooltip->assign(tooltip_text_);
    return true;
  }

  // Show the full text if the text does not fit.
  if (!is_multi_line_ && font_.GetStringWidth(text_) > width()) {
    *tooltip = text_;
    return true;
  }
  return false;
}

void Label::OnMouseMoved(const MouseEvent& e) {
  UpdateContainsMouse(e);
}

void Label::OnMouseEntered(const MouseEvent& event) {
  UpdateContainsMouse(event);
}

void Label::OnMouseExited(const MouseEvent& event) {
  SetContainsMouse(false);
}

void Label::SetMouseOverBackground(Background* background) {
  mouse_over_background_.reset(background);
}

const Background* Label::GetMouseOverBackground() const {
  return mouse_over_background_.get();
}

void Label::SetEnabled(bool enabled) {
  if (enabled == enabled_)
    return;
  View::SetEnabled(enabled);
  SetColor(enabled ? kEnabledColor : kDisabledColor);
}

gfx::Insets Label::GetInsets() const {
  gfx::Insets insets = View::GetInsets();
  if (IsFocusable() || has_focus_border_)  {
    insets += gfx::Insets(kFocusBorderPadding, kFocusBorderPadding,
                          kFocusBorderPadding, kFocusBorderPadding);
  }
  return insets;
}

// static
ChromeFont Label::GetDefaultFont() {
  return ResourceBundle::GetSharedInstance().GetFont(ResourceBundle::BaseFont);
}

void Label::UpdateContainsMouse(const MouseEvent& event) {
  SetContainsMouse(GetTextBounds().Contains(event.x(), event.y()));
}

void Label::SetContainsMouse(bool contains_mouse) {
  if (contains_mouse_ == contains_mouse)
    return;
  contains_mouse_ = contains_mouse;
  if (GetMouseOverBackground())
    SchedulePaint();
}

gfx::Rect Label::GetTextBounds() {
  gfx::Size text_size = GetTextSize();
  gfx::Insets insets = GetInsets();
  int avail_width = width() - insets.width();
  // Respect the size set by the owner view
  text_size.set_width(std::min(avail_width, text_size.width()));

  int text_y = insets.top() +
      (height() - text_size.height() - insets.height()) / 2;
  int text_x;
  switch (horiz_alignment_) {
    case ALIGN_LEFT:
      text_x = insets.left();
      break;
    case ALIGN_CENTER:
      // We put any extra margin pixel on the left rather than the right, since
      // GetTextExtentPoint32() can report a value one too large on the right.
      text_x = insets.left() + (avail_width + 1 - text_size.width()) / 2;
      break;
    case ALIGN_RIGHT:
      text_x = width() - insets.right() - text_size.width();
      break;
    default:
      NOTREACHED();
      text_x = 0;
      break;
  }
  return gfx::Rect(text_x, text_y, text_size.width(), text_size.height());
}

void Label::SizeToFit(int max_width) {
  DCHECK(is_multi_line_);

  std::vector<std::wstring> lines;
  SplitString(text_, L'\n', &lines);

  int label_width = 0;
  for (std::vector<std::wstring>::const_iterator iter = lines.begin();
       iter != lines.end(); ++iter) {
    label_width = std::max(label_width, font_.GetStringWidth(*iter));
  }

  gfx::Insets insets = GetInsets();
  label_width += insets.width();

  if (max_width > 0)
    label_width = std::min(label_width, max_width);

  SetBounds(x(), y(), label_width, 0);
  SizeToPreferredSize();
}

bool Label::GetAccessibleRole(AccessibilityTypes::Role* role) {
  DCHECK(role);

  *role = AccessibilityTypes::ROLE_TEXT;
  return true;
}

bool Label::GetAccessibleName(std::wstring* name) {
  *name = GetText();
  return true;
}

bool Label::GetAccessibleState(AccessibilityTypes::State* state) {
  DCHECK(state);

  *state = AccessibilityTypes::STATE_READONLY;
  return true;
}

}  // namespace views
<commit_msg>This CL fixes issue 4376 -- Press "Tab" key to navigate - Text focus is showing at wrong place in Hebrew and Arabic. (http://crbug.com/4376)<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.

#include "chrome/views/controls/label.h"

#include <math.h>

#include "app/gfx/chrome_canvas.h"
#include "app/gfx/chrome_font.h"
#include "app/gfx/insets.h"
#include "app/l10n_util.h"
#include "app/resource_bundle.h"
#include "base/logging.h"
#include "base/string_util.h"
#include "chrome/common/gfx/text_elider.h"
#include "chrome/views/background.h"

namespace views {

const char Label::kViewClassName[] = "chrome/views/Label";

static const SkColor kEnabledColor = SK_ColorBLACK;
static const SkColor kDisabledColor = SkColorSetRGB(161, 161, 146);
static const int kFocusBorderPadding = 1;

Label::Label() {
  Init(L"", GetDefaultFont());
}

Label::Label(const std::wstring& text) {
  Init(text, GetDefaultFont());
}

Label::Label(const std::wstring& text, const ChromeFont& font) {
  Init(text, font);
}

void Label::Init(const std::wstring& text, const ChromeFont& font) {
  contains_mouse_ = false;
  font_ = font;
  text_size_valid_ = false;
  SetText(text);
  url_set_ = false;
  color_ = kEnabledColor;
  horiz_alignment_ = ALIGN_CENTER;
  is_multi_line_ = false;
  allow_character_break_ = false;
  collapse_when_hidden_ = false;
  rtl_alignment_mode_ = USE_UI_ALIGNMENT;
  paint_as_focused_ = false;
  has_focus_border_ = false;
}

Label::~Label() {
}

gfx::Size Label::GetPreferredSize() {
  gfx::Size prefsize;

  // Return a size of (0, 0) if the label is not visible and if the
  // collapse_when_hidden_ flag is set.
  // TODO(munjal): This logic probably belongs to the View class. But for now,
  // put it here since putting it in View class means all inheriting classes
  // need ot respect the collapse_when_hidden_ flag.
  if (!IsVisible() && collapse_when_hidden_)
    return prefsize;

  if (is_multi_line_) {
    int w = width(), h = 0;
    ChromeCanvas::SizeStringInt(text_, font_, &w, &h, ComputeMultiLineFlags());
    prefsize.SetSize(w, h);
  } else {
    prefsize = GetTextSize();
  }

  gfx::Insets insets = GetInsets();
  prefsize.Enlarge(insets.width(), insets.height());
  return prefsize;
}

int Label::ComputeMultiLineFlags() {
  int flags = ChromeCanvas::MULTI_LINE;
  if (allow_character_break_)
    flags |= ChromeCanvas::CHARACTER_BREAK;
  switch (horiz_alignment_) {
    case ALIGN_LEFT:
      flags |= ChromeCanvas::TEXT_ALIGN_LEFT;
      break;
    case ALIGN_CENTER:
      flags |= ChromeCanvas::TEXT_ALIGN_CENTER;
      break;
    case ALIGN_RIGHT:
      flags |= ChromeCanvas::TEXT_ALIGN_RIGHT;
      break;
  }
  return flags;
}

void Label::CalculateDrawStringParams(
    std::wstring* paint_text, gfx::Rect* text_bounds, int* flags) {
  DCHECK(paint_text && text_bounds && flags);

  if (url_set_) {
    // TODO(jungshik) : Figure out how to get 'intl.accept_languages'
    // preference and use it when calling ElideUrl.
    *paint_text = gfx::ElideUrl(url_, font_, width(), std::wstring());

    // An URLs is always treated as an LTR text and therefore we should
    // explicitly mark it as such if the locale is RTL so that URLs containing
    // Hebrew or Arabic characters are displayed correctly.
    //
    // Note that we don't check the View's UI layout setting in order to
    // determine whether or not to insert the special Unicode formatting
    // characters. We use the locale settings because an URL is always treated
    // as an LTR string, even if its containing view does not use an RTL UI
    // layout.
    if (l10n_util::GetTextDirection() == l10n_util::RIGHT_TO_LEFT)
      l10n_util::WrapStringWithLTRFormatting(paint_text);
  } else {
    *paint_text = text_;
  }

  if (is_multi_line_) {
    gfx::Insets insets = GetInsets();
    text_bounds->SetRect(insets.left(),
                         insets.top(),
                         width() - insets.width(),
                         height() - insets.height());
    *flags = ComputeMultiLineFlags();
  } else {
    *text_bounds = GetTextBounds();
    *flags = 0;
  }
}

void Label::Paint(ChromeCanvas* canvas) {
  PaintBackground(canvas);
  std::wstring paint_text;
  gfx::Rect text_bounds;
  int flags = 0;
  CalculateDrawStringParams(&paint_text, &text_bounds, &flags);
  canvas->DrawStringInt(paint_text,
                        font_,
                        color_,
                        text_bounds.x(),
                        text_bounds.y(),
                        text_bounds.width(),
                        text_bounds.height(),
                        flags);

  // The focus border always hugs the text, regardless of the label's bounds.
  if (HasFocus() || paint_as_focused_) {
    int w = text_bounds.width();
    int h = 0;
    // We explicitly OR in MULTI_LINE here since SizeStringInt seems to return
    // an incorrect height for single line text when the MULTI_LINE flag isn't
    // specified. o_O...
    ChromeCanvas::SizeStringInt(paint_text, font_, &w, &h,
                                flags | ChromeCanvas::MULTI_LINE);
    gfx::Rect focus_rect = text_bounds;
    focus_rect.set_width(w);
    focus_rect.set_height(h);
    focus_rect.Inset(-kFocusBorderPadding, -kFocusBorderPadding);
    canvas->DrawFocusRect(MirroredLeftPointForRect(focus_rect), focus_rect.y(),
                          focus_rect.width(), focus_rect.height());
  }
}

void Label::PaintBackground(ChromeCanvas* canvas) {
  const Background* bg = contains_mouse_ ? GetMouseOverBackground() : NULL;
  if (!bg)
    bg = background();
  if (bg)
    bg->Paint(canvas, this);
}

void Label::SetFont(const ChromeFont& font) {
  font_ = font;
  text_size_valid_ = false;
  SchedulePaint();
}

ChromeFont Label::GetFont() const {
  return font_;
}

void Label::SetText(const std::wstring& text) {
  text_ = text;
  url_set_ = false;
  text_size_valid_ = false;
  SchedulePaint();
}

void Label::SetURL(const GURL& url) {
  url_ = url;
  text_ = UTF8ToWide(url_.spec());
  url_set_ = true;
  text_size_valid_ = false;
  SchedulePaint();
}

const std::wstring Label::GetText() const {
  if (url_set_)
    return UTF8ToWide(url_.spec());
  else
    return text_;
}

const GURL Label::GetURL() const {
  if (url_set_)
    return url_;
  else
    return GURL(WideToUTF8(text_));
}

gfx::Size Label::GetTextSize() {
  if (!text_size_valid_) {
    text_size_.SetSize(font_.GetStringWidth(text_), font_.height());
    text_size_valid_ = true;
  }

  if (text_size_valid_)
    return text_size_;
  return gfx::Size();
}

int Label::GetHeightForWidth(int w) {
  if (is_multi_line_) {
    gfx::Insets insets = GetInsets();
    w = std::max<int>(0, w - insets.width());
    int h = 0;
    ChromeCanvas cc(0, 0, true);
    cc.SizeStringInt(text_, font_, &w, &h, ComputeMultiLineFlags());
    return h + insets.height();
  }

  return View::GetHeightForWidth(w);
}

std::string Label::GetClassName() const {
  return kViewClassName;
}

void Label::SetColor(const SkColor& color) {
  color_ = color;
}

const SkColor Label::GetColor() const {
  return color_;
}

void Label::SetHorizontalAlignment(Alignment a) {
  // If the View's UI layout is right-to-left and rtl_alignment_mode_ is
  // USE_UI_ALIGNMENT, we need to flip the alignment so that the alignment
  // settings take into account the text directionality.
  if (UILayoutIsRightToLeft() && rtl_alignment_mode_ == USE_UI_ALIGNMENT) {
    if (a == ALIGN_LEFT)
      a = ALIGN_RIGHT;
    else if (a == ALIGN_RIGHT)
      a = ALIGN_LEFT;
  }
  if (horiz_alignment_ != a) {
    horiz_alignment_ = a;
    SchedulePaint();
  }
}

Label::Alignment Label::GetHorizontalAlignment() const {
  return horiz_alignment_;
}

void Label::SetRTLAlignmentMode(RTLAlignmentMode mode) {
  rtl_alignment_mode_ = mode;
}

Label::RTLAlignmentMode Label::GetRTLAlignmentMode() const {
  return rtl_alignment_mode_;
}

void Label::SetMultiLine(bool f) {
  if (f != is_multi_line_) {
    is_multi_line_ = f;
    SchedulePaint();
  }
}

void Label::SetAllowCharacterBreak(bool f) {
  if (f != allow_character_break_) {
    allow_character_break_ = f;
    SchedulePaint();
  }
}

bool Label::IsMultiLine() {
  return is_multi_line_;
}

void Label::SetTooltipText(const std::wstring& tooltip_text) {
  tooltip_text_ = tooltip_text;
}

bool Label::GetTooltipText(int x, int y, std::wstring* tooltip) {
  DCHECK(tooltip);

  // If a tooltip has been explicitly set, use it.
  if (!tooltip_text_.empty()) {
    tooltip->assign(tooltip_text_);
    return true;
  }

  // Show the full text if the text does not fit.
  if (!is_multi_line_ && font_.GetStringWidth(text_) > width()) {
    *tooltip = text_;
    return true;
  }
  return false;
}

void Label::OnMouseMoved(const MouseEvent& e) {
  UpdateContainsMouse(e);
}

void Label::OnMouseEntered(const MouseEvent& event) {
  UpdateContainsMouse(event);
}

void Label::OnMouseExited(const MouseEvent& event) {
  SetContainsMouse(false);
}

void Label::SetMouseOverBackground(Background* background) {
  mouse_over_background_.reset(background);
}

const Background* Label::GetMouseOverBackground() const {
  return mouse_over_background_.get();
}

void Label::SetEnabled(bool enabled) {
  if (enabled == enabled_)
    return;
  View::SetEnabled(enabled);
  SetColor(enabled ? kEnabledColor : kDisabledColor);
}

gfx::Insets Label::GetInsets() const {
  gfx::Insets insets = View::GetInsets();
  if (IsFocusable() || has_focus_border_)  {
    insets += gfx::Insets(kFocusBorderPadding, kFocusBorderPadding,
                          kFocusBorderPadding, kFocusBorderPadding);
  }
  return insets;
}

// static
ChromeFont Label::GetDefaultFont() {
  return ResourceBundle::GetSharedInstance().GetFont(ResourceBundle::BaseFont);
}

void Label::UpdateContainsMouse(const MouseEvent& event) {
  SetContainsMouse(GetTextBounds().Contains(event.x(), event.y()));
}

void Label::SetContainsMouse(bool contains_mouse) {
  if (contains_mouse_ == contains_mouse)
    return;
  contains_mouse_ = contains_mouse;
  if (GetMouseOverBackground())
    SchedulePaint();
}

gfx::Rect Label::GetTextBounds() {
  gfx::Size text_size = GetTextSize();
  gfx::Insets insets = GetInsets();
  int avail_width = width() - insets.width();
  // Respect the size set by the owner view
  text_size.set_width(std::min(avail_width, text_size.width()));

  int text_y = insets.top() +
      (height() - text_size.height() - insets.height()) / 2;
  int text_x;
  switch (horiz_alignment_) {
    case ALIGN_LEFT:
      text_x = insets.left();
      break;
    case ALIGN_CENTER:
      // We put any extra margin pixel on the left rather than the right, since
      // GetTextExtentPoint32() can report a value one too large on the right.
      text_x = insets.left() + (avail_width + 1 - text_size.width()) / 2;
      break;
    case ALIGN_RIGHT:
      text_x = width() - insets.right() - text_size.width();
      break;
    default:
      NOTREACHED();
      text_x = 0;
      break;
  }
  return gfx::Rect(text_x, text_y, text_size.width(), text_size.height());
}

void Label::SizeToFit(int max_width) {
  DCHECK(is_multi_line_);

  std::vector<std::wstring> lines;
  SplitString(text_, L'\n', &lines);

  int label_width = 0;
  for (std::vector<std::wstring>::const_iterator iter = lines.begin();
       iter != lines.end(); ++iter) {
    label_width = std::max(label_width, font_.GetStringWidth(*iter));
  }

  gfx::Insets insets = GetInsets();
  label_width += insets.width();

  if (max_width > 0)
    label_width = std::min(label_width, max_width);

  SetBounds(x(), y(), label_width, 0);
  SizeToPreferredSize();
}

bool Label::GetAccessibleRole(AccessibilityTypes::Role* role) {
  DCHECK(role);

  *role = AccessibilityTypes::ROLE_TEXT;
  return true;
}

bool Label::GetAccessibleName(std::wstring* name) {
  *name = GetText();
  return true;
}

bool Label::GetAccessibleState(AccessibilityTypes::State* state) {
  DCHECK(state);

  *state = AccessibilityTypes::STATE_READONLY;
  return true;
}

}  // namespace views
<|endoftext|>


<commit_before>#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>

#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <lwr_controllers/one_task_inverse_kinematics.h>

namespace lwr_controllers
{
	OneTaskInverseKinematics::OneTaskInverseKinematics() {}
	OneTaskInverseKinematics::~OneTaskInverseKinematics() {}

	bool OneTaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
		nh_ = n;

		// get URDF and name of root and tip from the parameter server
		std::string robot_description, root_name, tip_name;

		if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
		    return false;
		}
		if (!nh_.getParam("root_name", root_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
		    return false;
		}
		if (!nh_.getParam("tip_name", tip_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
		    return false;
		}

		// Get the gravity vector (direction and magnitude)
		KDL::Vector gravity_ = KDL::Vector::Zero();
		gravity_(2) = -9.81;

		// Construct an URDF model from the xml string
		std::string xml_string;

		if (n.hasParam(robot_description))
			n.getParam(robot_description.c_str(), xml_string);
		else
		{
		    ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		if (xml_string.size() == 0)
		{
			ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());

		// Get urdf model out of robot_description
		urdf::Model model;
		if (!model.initString(xml_string))
		{
		    ROS_ERROR("Failed to parse urdf file");
		    n.shutdown();
		    return false;
		}
		ROS_INFO("Successfully parsed urdf file");

		KDL::Tree kdl_tree_;
		if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
		{
		    ROS_ERROR("Failed to construct kdl tree");
		    n.shutdown();
		    return false;
		}

		// Parsing joint limits from urdf model
		boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
    	boost::shared_ptr<const urdf::Joint> joint_;
    	joint_limits_.min.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.max.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.center.resize(kdl_tree_.getNrOfJoints());
		int index;

    	for (int i = 0; i < kdl_tree_.getNrOfJoints() && link_; i++)
    	{
    		joint_ = model.getJoint(link_->parent_joint->name);
    		index = kdl_tree_.getNrOfJoints() - i - 1;

    		joint_limits_.min(index) = joint_->limits->lower;
    		joint_limits_.max(index) = joint_->limits->upper;
    		joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;

    		link_ = model.getLink(link_->getParent()->name);
    	}

		// Populate the KDL chain
		if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
		{
		    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
		    ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
		    ROS_ERROR_STREAM("  The segments are:");

		    KDL::SegmentMap segment_map = kdl_tree_.getSegments();
		    KDL::SegmentMap::iterator it;

		    for( it=segment_map.begin(); it != segment_map.end(); it++ )
		      ROS_ERROR_STREAM( "    "<<(*it).first);

		  	return false;
		}

		ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
		ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());

		// Get joint handles for all of the joints in the chain
		for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
		{
		    joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
		    ROS_DEBUG("%s", it->getJoint().getName().c_str() );
		}

		ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
		ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
		ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));

		joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
		joint_des_states_.resize(kdl_chain_.getNrOfJoints());
		tau_cmd_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		PIDs_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command_configuration", 1, &OneTaskInverseKinematics::command_configuration, this);
		sub_gains_ = nh_.subscribe("set_gains", 1, &OneTaskInverseKinematics::set_gains, this);

		return true;
	}

	void OneTaskInverseKinematics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    	}

    	Kp = 60;
    	Ki = 1.2;
    	Kd = 10;

    	for (int i = 0; i < PIDs_.size(); i++)
    		PIDs_[i].initPid(Kp,Ki,Kd,0.3,-0.3);
    	ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd);

    	// computing forward kinematics
    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

    	//Desired posture
    	x_des_ = x_;

    	cmd_flag_ = 0;
	}

	void OneTaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
	{

		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	if (cmd_flag_)
    	{
    		// ANALYTIC METHOD FOR INVERSE KINEMATICS
	    	// computing Jacobian
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing J_pinv_
	    	pseudo_inverse(J_.data,J_pinv_);

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	// end-effector position/orientation error
	    	x_err_.vel = x_des_.p - x_.p;

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;

	    	// computing q_dot
	    	for (int i = 0; i < J_pinv_.rows(); i++)
	    	{
	    		joint_des_states_.qdot(i) = 0.0;
	    		for (int k = 0; k < J_pinv_.cols(); k++)
	    			joint_des_states_.qdot(i) += .3*J_pinv_(i,k)*x_err_(k);

	    	}

	    	// integrating q_dot -> getting q (Euler method)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);

        for (int i =0;  i < joint_handles_.size(); i++)
        {
          if (joint_des_states_.q(i) < joint_limits_.min(i) )
            joint_des_states_.q(i) = joint_limits_.min(i);
          if (joint_des_states_.q(i) > joint_limits_.max(i) )
            joint_des_states_.q(i) = joint_limits_.max(i);
        }

	    	// computing differential inverse kinematics
	    //	ik_pos_solver_->CartToJnt(joint_msr_states_.q,x_des_,joint_des_states_.q);

			// computing forward kinematics
	    //	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.025))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    	}
	    }

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
		   	joint_handles_[i].setCommand(tau_cmd_(i));
    	}
	}

	void OneTaskInverseKinematics::command_configuration(const lwr_controllers::PoseRPY::ConstPtr &msg)
	{
		KDL::Frame frame_des_;

		switch(msg->id)
		{
			case 0:
			frame_des_ = KDL::Frame(
					KDL::Rotation::RPY(msg->orientation.roll,
						 			  msg->orientation.pitch,
								 	  msg->orientation.yaw),
					KDL::Vector(msg->position.x,
								msg->position.y,
								msg->position.z));
			break;

			case 1: // position only
			frame_des_ = KDL::Frame(
				KDL::Vector(msg->position.x,
							msg->position.y,
							msg->position.z));
			break;

			case 2: // orientation only
			frame_des_ = KDL::Frame(
				KDL::Rotation::RPY(msg->orientation.roll,
				   	 			   msg->orientation.pitch,
								   msg->orientation.yaw));
			break;

			default:
			ROS_INFO("Wrong message ID");
			return;
		}

		x_des_ = frame_des_;
		cmd_flag_ = 1;
	}

	void OneTaskInverseKinematics::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{
		if(msg->data.size() == 3)
		{
			for(int i = 0; i < PIDs_.size(); i++)
				PIDs_[i].setGains(msg->data[0],msg->data[1],msg->data[2],0.3,-0.3);
			ROS_INFO("New gains set: Kp = %f, Ki = %f, Kd = %f",msg->data[0],msg->data[1],msg->data[2]);
		}
		else
			ROS_INFO("PIDs gains needed are 3 (Kp, Ki and Kd)");
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseKinematics, controller_interface::ControllerBase)
<commit_msg>small tweaks to tau and gains<commit_after>#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>

#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>
#include <lwr_controllers/one_task_inverse_kinematics.h>

namespace lwr_controllers
{
	OneTaskInverseKinematics::OneTaskInverseKinematics() {}
	OneTaskInverseKinematics::~OneTaskInverseKinematics() {}

	bool OneTaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
		nh_ = n;

		// get URDF and name of root and tip from the parameter server
		std::string robot_description, root_name, tip_name;

		if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
		    return false;
		}
		if (!nh_.getParam("root_name", root_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
		    return false;
		}
		if (!nh_.getParam("tip_name", tip_name))
		{
		    ROS_ERROR_STREAM("OneTaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
		    return false;
		}

		// Get the gravity vector (direction and magnitude)
		KDL::Vector gravity_ = KDL::Vector::Zero();
		gravity_(2) = -9.81;

		// Construct an URDF model from the xml string
		std::string xml_string;

		if (n.hasParam(robot_description))
			n.getParam(robot_description.c_str(), xml_string);
		else
		{
		    ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		if (xml_string.size() == 0)
		{
			ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
		    n.shutdown();
		    return false;
		}

		ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());

		// Get urdf model out of robot_description
		urdf::Model model;
		if (!model.initString(xml_string))
		{
		    ROS_ERROR("Failed to parse urdf file");
		    n.shutdown();
		    return false;
		}
		ROS_INFO("Successfully parsed urdf file");

		KDL::Tree kdl_tree_;
		if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
		{
		    ROS_ERROR("Failed to construct kdl tree");
		    n.shutdown();
		    return false;
		}

		// Parsing joint limits from urdf model
		boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
    	boost::shared_ptr<const urdf::Joint> joint_;
    	joint_limits_.min.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.max.resize(kdl_tree_.getNrOfJoints());
		joint_limits_.center.resize(kdl_tree_.getNrOfJoints());
		int index;

    	for (int i = 0; i < kdl_tree_.getNrOfJoints() && link_; i++)
    	{
    		joint_ = model.getJoint(link_->parent_joint->name);
    		index = kdl_tree_.getNrOfJoints() - i - 1;

    		joint_limits_.min(index) = joint_->limits->lower;
    		joint_limits_.max(index) = joint_->limits->upper;
    		joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;

    		link_ = model.getLink(link_->getParent()->name);
    	}

		// Populate the KDL chain
		if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
		{
		    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
		    ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
		    ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
		    ROS_ERROR_STREAM("  The segments are:");

		    KDL::SegmentMap segment_map = kdl_tree_.getSegments();
		    KDL::SegmentMap::iterator it;

		    for( it=segment_map.begin(); it != segment_map.end(); it++ )
		      ROS_ERROR_STREAM( "    "<<(*it).first);

		  	return false;
		}

		ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
		ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());

		// Get joint handles for all of the joints in the chain
		for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
		{
		    joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
		    ROS_DEBUG("%s", it->getJoint().getName().c_str() );
		}

		ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
		ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
		ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));

		joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
		joint_des_states_.resize(kdl_chain_.getNrOfJoints());
		tau_cmd_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		PIDs_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command_configuration", 1, &OneTaskInverseKinematics::command_configuration, this);
		sub_gains_ = nh_.subscribe("set_gains", 1, &OneTaskInverseKinematics::set_gains, this);

		return true;
	}

	void OneTaskInverseKinematics::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    	}

    	Kp = 60;
    	Ki = 1.2;
    	Kd = 10;

    	for (int i = 0; i < PIDs_.size(); i++)
      {
    		PIDs_[i].initPid(Kp,Ki,Kd,0.3,-0.3);
      }
    	ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd);

    	// computing forward kinematics
    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

    	//Desired posture
    	x_des_ = x_;

    	cmd_flag_ = 0;
	}

	void OneTaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
	{

		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	if (cmd_flag_)
    	{
    		// ANALYTIC METHOD FOR INVERSE KINEMATICS
	    	// computing Jacobian
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing J_pinv_
	    	pseudo_inverse(J_.data,J_pinv_);

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	// end-effector position/orientation error
	    	x_err_.vel = x_des_.p - x_.p;

	    	// getting quaternion from rotation matrix
	    	x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
	    	x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);

	    	skew_symmetric(quat_des_.v,skew_);

	    	for (int i = 0; i < skew_.rows(); i++)
	    	{
	    		v_temp_(i) = 0.0;
	    		for (int k = 0; k < skew_.cols(); k++)
	    			v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
	    	}

	    	x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;

	    	// computing q_dot
	    	for (int i = 0; i < J_pinv_.rows(); i++)
	    	{
	    		joint_des_states_.qdot(i) = 0.0;
	    		for (int k = 0; k < J_pinv_.cols(); k++)
	    			joint_des_states_.qdot(i) += .7*J_pinv_(i,k)*x_err_(k);

	    	}

	    	// integrating q_dot -> getting q (Euler method)
	    	for (int i = 0; i < joint_handles_.size(); i++)
	    		joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);

        for (int i =0;  i < joint_handles_.size(); i++)
        {
          if (joint_des_states_.q(i) < joint_limits_.min(i) )
            joint_des_states_.q(i) = joint_limits_.min(i);
          if (joint_des_states_.q(i) > joint_limits_.max(i) )
            joint_des_states_.q(i) = joint_limits_.max(i);
        }

	    	// computing differential inverse kinematics
	    //	ik_pos_solver_->CartToJnt(joint_msr_states_.q,x_des_,joint_des_states_.q);

			// computing forward kinematics
	    //	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.005))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    	}
	    }

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
		   	joint_handles_[i].setCommand(tau_cmd_(i));
    	}
	}

	void OneTaskInverseKinematics::command_configuration(const lwr_controllers::PoseRPY::ConstPtr &msg)
	{
		KDL::Frame frame_des_;

		switch(msg->id)
		{
			case 0:
			frame_des_ = KDL::Frame(
					KDL::Rotation::RPY(msg->orientation.roll,
						 			  msg->orientation.pitch,
								 	  msg->orientation.yaw),
					KDL::Vector(msg->position.x,
								msg->position.y,
								msg->position.z));
			break;

			case 1: // position only
			frame_des_ = KDL::Frame(
				KDL::Vector(msg->position.x,
							msg->position.y,
							msg->position.z));
			break;

			case 2: // orientation only
			frame_des_ = KDL::Frame(
				KDL::Rotation::RPY(msg->orientation.roll,
				   	 			   msg->orientation.pitch,
								   msg->orientation.yaw));
			break;

			default:
			ROS_INFO("Wrong message ID");
			return;
		}

		x_des_ = frame_des_;
		cmd_flag_ = 1;
	}

	void OneTaskInverseKinematics::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{
		if(msg->data.size() == 3)
		{
			for(int i = 0; i < PIDs_.size(); i++)
				PIDs_[i].setGains(msg->data[0],msg->data[1],msg->data[2],0.3,-0.3);
			ROS_INFO("New gains set: Kp = %f, Ki = %f, Kd = %f",msg->data[0],msg->data[1],msg->data[2]);
		}
		else
			ROS_INFO("PIDs gains needed are 3 (Kp, Ki and Kd)");
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseKinematics, controller_interface::ControllerBase)
<|endoftext|>


<commit_before>#include "resizer.h"

Resizer::Resizer(QObject *parent) : QObject(parent)
{

}

int Resizer::width() const
{
    return m_width;
}

int Resizer::height() const
{
    return m_height;
}

void Resizer::process()
{
    m_outputImage = QImage(m_path).
            scaled(width(), height(), Qt::KeepAspectRatio).
            convertToFormat(QImage::Format_ARGB32_Premultiplied);
    emit finished();
    emit finished(m_outputImage);
}

void Resizer::setInputImagePath(QString path)
{
    m_path = path;
}

void Resizer::setWidth(int width)
{
    if(m_width != width)
    {
        m_width = width;
    }
}

void Resizer::setHeight(int height)
{
    if(m_height != height)
    {
        m_height = height;
    }
}
<commit_msg>Changed resizing algorithm<commit_after>#include "resizer.h"

#include <QImageReader>

#include <QDebug>

Resizer::Resizer(QObject *parent) : QObject(parent)
{

}

int Resizer::width() const
{
    return m_width;
}

int Resizer::height() const
{
    return m_height;
}

void Resizer::process()
{
    QImageReader imageReader(m_path);

    QSize originalSize(imageReader.size());
    QSize scaledSize = originalSize;

    if((originalSize.width() || originalSize.height()) == 0)
    {
        return;
    }

//    qDebug() << "!!!!!!!!!" << originalSize.width() / width() <<
//                originalSize.height() / height();

    // Source form coefficient
    double kfs = static_cast<double>(originalSize.width()) /
                 static_cast<double>(originalSize.height());

    // Destination form coefficient
    double kfd = static_cast<double>(width()) /
                 static_cast<double>(height());

    double scaleFactor = 1.0;

    if(kfs > kfd)
    {
        scaleFactor = static_cast<double>(width()) /
                static_cast<double>(originalSize.width());
        scaledSize.setWidth(width());
        scaledSize.setHeight(originalSize.height() * scaleFactor);
        //qDebug() << "kfs > kfd";
    }
    else
    {
        scaleFactor = static_cast<double>(height()) /
                static_cast<double>(originalSize.height());
        scaledSize.setWidth(originalSize.width() * scaleFactor);
        scaledSize.setHeight(height());
        //qDebug() << "kfs < kfd";
    }

//    qDebug() << "Original size: " << originalSize;
//    qDebug() << "Scaled size: " << scaledSize;
//    qDebug() << "kfs =" << kfs;
//    qDebug() << "kfd =" << kfd;

    imageReader.setScaledSize(scaledSize);
    imageReader.read(&m_outputImage);

    m_outputImage = m_outputImage.convertToFormat(QImage::Format_ARGB32_Premultiplied);

    emit finished();
    emit finished(m_outputImage);
}

void Resizer::setInputImagePath(QString path)
{
    m_path = path;
}

void Resizer::setWidth(int width)
{
    if(m_width != width)
    {
        m_width = width;
    }
}

void Resizer::setHeight(int height)
{
    if(m_height != height)
    {
        m_height = height;
    }
}
<|endoftext|>


<commit_before>#include "chrono_parallel/physics/ChSystemParallel.h"
#include <omp.h>

using namespace chrono;
using namespace chrono::collision;

ChSystemParallelDEM::ChSystemParallelDEM(unsigned int                       max_objects,
                                         ChContactDEM::NormalForceModel     normal_model,
                                         ChContactDEM::TangentialForceModel tangential_model)
: ChSystemParallel(max_objects),
  normal_force_model(normal_model),
  tangential_force_model(tangential_model)
{
  LCP_descriptor = new ChLcpSystemDescriptorParallelDEM();
  LCP_solver_speed = new ChLcpSolverParallelDEM();
  ((ChLcpSystemDescriptorParallelDEM*) LCP_descriptor)->data_container = data_manager;
  ((ChLcpSolverParallel*) LCP_solver_speed)->data_container = data_manager;

  ((ChCollisionSystemParallel *) collision_system)->SetCollisionEnvelope(0);

  //Set this so that the CD can check what type of system it is (needed for narrowphase)
  data_manager->settings.system_type = SYSTEM_DEM;
  data_manager->system_timer.AddTimer("ChLcpSolverParallelDEM_ProcessContact");
}

void ChSystemParallelDEM::LoadMaterialSurfaceData(ChSharedPtr<ChBody> newbody)
{
  assert(typeid(*newbody.get_ptr()) == typeid(ChBodyDEM));

  ChSharedPtr<ChMaterialSurfaceDEM>& mat = ((ChBodyDEM*) newbody.get_ptr())->GetMaterialSurfaceDEM();

  data_manager->host_data.elastic_moduli.push_back(R2(mat->GetYoungModulus(), mat->GetPoissonRatio()));
  data_manager->host_data.mu.push_back(mat->GetSfriction());
  data_manager->host_data.cohesion_data.push_back(mat->GetCohesion());

  switch (normal_force_model) {
  case ChContactDEM::HuntCrossley:
    data_manager->host_data.alpha.push_back(mat->GetDissipationFactor());
    break;
  }

  //gpu_data_manager->host_data.cr.push_back(mat->GetRestitution());
}

void ChSystemParallelDEM::UpdateBodies()
{
  real3 *vel_pointer = data_manager->host_data.vel_data.data();
  real3 *omg_pointer = data_manager->host_data.omg_data.data();
  real3 *pos_pointer = data_manager->host_data.pos_data.data();
  real4 *rot_pointer = data_manager->host_data.rot_data.data();
  real3 *inr_pointer = data_manager->host_data.inr_data.data();
  real3 *frc_pointer = data_manager->host_data.frc_data.data();
  real3 *trq_pointer = data_manager->host_data.trq_data.data();
  bool *active_pointer = data_manager->host_data.active_data.data();
  real *mass_pointer = data_manager->host_data.mass_data.data();
  real3 *lim_pointer = data_manager->host_data.lim_data.data();

  real2* elastic_moduli = data_manager->host_data.elastic_moduli.data();
  real*  mu             = data_manager->host_data.mu.data();
  real*  alpha          = data_manager->host_data.alpha.data();
  real*  cr             = data_manager->host_data.cr.data();
  real*  cohesion       = data_manager->host_data.cohesion_data.data();

#pragma omp parallel for
  for (int i = 0; i < bodylist.size(); i++) {
    bodylist[i]->UpdateTime(ChTime);
    //bodylist[i]->TrySleeping();     // See if the body can fall asleep; if so, put it to sleeping
    //bodylist[i]->ClampSpeed();      // Apply limits (if in speed clamping mode) to speeds.
    bodylist[i]->ComputeGyro();     // Set the gyroscopic momentum.
    bodylist[i]->UpdateForces(ChTime);
    bodylist[i]->VariablesFbReset();
    bodylist[i]->VariablesFbLoadForces(GetStep());
    bodylist[i]->VariablesQbLoadSpeed();
  }

  for (int i = 0; i < bodylist.size(); i++) {
    bodylist[i]->UpdateMarkers(ChTime);
    //bodylist[i]->InjectVariables(*this->LCP_descriptor);
  }

#pragma omp parallel for
  for (int i = 0; i < bodylist.size(); i++) {
    ChMatrix<>&     qb = bodylist[i]->Variables().Get_qb();
    ChMatrix<>&     fb = bodylist[i]->Variables().Get_fb();
    ChVector<>&     pos = bodylist[i]->GetPos();
    ChQuaternion<>& rot = bodylist[i]->GetRot();
    ChMatrix33<>&   inertia = bodylist[i]->VariablesBody().GetBodyInvInertia();

    vel_pointer[i] = (R3(qb.ElementN(0), qb.ElementN(1), qb.ElementN(2)));
    omg_pointer[i] = (R3(qb.ElementN(3), qb.ElementN(4), qb.ElementN(5)));
    pos_pointer[i] = (R3(pos.x, pos.y, pos.z));
    rot_pointer[i] = (R4(rot.e0, rot.e1, rot.e2, rot.e3));
    inr_pointer[i] = (R3(inertia.GetElement(0, 0), inertia.GetElement(1, 1), inertia.GetElement(2, 2)));
    frc_pointer[i] = (R3(fb.ElementN(0), fb.ElementN(1), fb.ElementN(2)));     //forces
    trq_pointer[i] = (R3(fb.ElementN(3), fb.ElementN(4), fb.ElementN(5)));     //torques
    active_pointer[i] = bodylist[i]->IsActive();
    mass_pointer[i] = 1.0f / bodylist[i]->VariablesBody().GetBodyMass();
    lim_pointer[i] = (R3(bodylist[i]->GetLimitSpeed(), .05 / GetStep(), .05 / GetStep()));

    ChSharedPtr<ChMaterialSurfaceDEM>& mat = ((ChBodyDEM*) bodylist[i])->GetMaterialSurfaceDEM();

    elastic_moduli[i] = R2(mat->GetYoungModulus(), mat->GetPoissonRatio());
    mu[i]             = mat->GetSfriction();
    cohesion[i]       = mat->GetCohesion();
    //cr[i]             = mat->GetRestitution();

    switch (normal_force_model) {
    case ChContactDEM::HuntCrossley:
      alpha[i] = mat->GetDissipationFactor();
      break;
    }

    bodylist[i]->GetCollisionModel()->SyncPosition();
  }
}

void ChSystemParallelDEM::ChangeCollisionSystem(ChCollisionSystem* newcollsystem)
{
  ChSystemParallel::ChangeCollisionSystem(newcollsystem);

  if (ChCollisionSystemParallel* coll_sys = dynamic_cast<ChCollisionSystemParallel*>(collision_system))
    coll_sys->SetCollisionEnvelope(0);
}

void ChSystemParallelDEM::PrintStepStats()
{
  double timer_solver_setup = data_manager->system_timer.GetTime("ChLcpSolverParallel_Setup");
  double timer_solver_rhs   = data_manager->system_timer.GetTime("ChLcpSolverParallel_RHS");
  double timer_solver_stab  = data_manager->system_timer.GetTime("ChLcpSolverParallel_Stab");

  std::cout << std::endl;
  std::cout << "System Information" << std::endl;
  std::cout << "------------------" << std::endl;
  std::cout << "  Number of bodies     " << GetNumBodies() << std::endl;
  std::cout << "  Number of contacts   " << GetNcontacts() << std::endl;
  std::cout << "  Number of bilaterals " << GetNumBilaterals() << std::endl;
  std::cout << std::endl;
  std::cout << "Timing Information" << std::endl;
  std::cout << "------------------" << std::endl;
  std::cout << "Simulation time        " << GetTimerStep() << std::endl;
  std::cout << "  Collision detection    " << GetTimerCollision() << std::endl;
  std::cout << "    broad phase            " << GetTimerCollisionBroad() << std::endl;
  std::cout << "    narrow phase           " << GetTimerCollisionNarrow() << std::endl;
  std::cout << "  Update                 " << GetTimerUpdate() << std::endl;
  std::cout << "  Solver                 " << GetTimerLcp() << std::endl;
  std::cout << "    contact force calc     " << GetTimerProcessContact() << std::endl;
  std::cout << "    setup                  " << timer_solver_setup << std::endl;
  std::cout << "      RHS                    " << timer_solver_rhs << std::endl;
  std::cout << "    stabilization          " << timer_solver_stab << std::endl;
  std::cout << std::endl;
}
<commit_msg>Use a single parallel for loop for body updates in a ChsystemParallelDEM.<commit_after>#include "chrono_parallel/physics/ChSystemParallel.h"
#include <omp.h>

using namespace chrono;
using namespace chrono::collision;

ChSystemParallelDEM::ChSystemParallelDEM(unsigned int                       max_objects,
                                         ChContactDEM::NormalForceModel     normal_model,
                                         ChContactDEM::TangentialForceModel tangential_model)
: ChSystemParallel(max_objects),
  normal_force_model(normal_model),
  tangential_force_model(tangential_model)
{
  LCP_descriptor = new ChLcpSystemDescriptorParallelDEM();
  LCP_solver_speed = new ChLcpSolverParallelDEM();
  ((ChLcpSystemDescriptorParallelDEM*) LCP_descriptor)->data_container = data_manager;
  ((ChLcpSolverParallel*) LCP_solver_speed)->data_container = data_manager;

  ((ChCollisionSystemParallel *) collision_system)->SetCollisionEnvelope(0);

  //Set this so that the CD can check what type of system it is (needed for narrowphase)
  data_manager->settings.system_type = SYSTEM_DEM;
  data_manager->system_timer.AddTimer("ChLcpSolverParallelDEM_ProcessContact");
}

void ChSystemParallelDEM::LoadMaterialSurfaceData(ChSharedPtr<ChBody> newbody)
{
  assert(typeid(*newbody.get_ptr()) == typeid(ChBodyDEM));

  ChSharedPtr<ChMaterialSurfaceDEM>& mat = ((ChBodyDEM*) newbody.get_ptr())->GetMaterialSurfaceDEM();

  data_manager->host_data.elastic_moduli.push_back(R2(mat->GetYoungModulus(), mat->GetPoissonRatio()));
  data_manager->host_data.mu.push_back(mat->GetSfriction());
  data_manager->host_data.cohesion_data.push_back(mat->GetCohesion());

  switch (normal_force_model) {
  case ChContactDEM::HuntCrossley:
    data_manager->host_data.alpha.push_back(mat->GetDissipationFactor());
    break;
  }

  //gpu_data_manager->host_data.cr.push_back(mat->GetRestitution());
}

void ChSystemParallelDEM::UpdateBodies()
{
  real3 *vel_pointer = data_manager->host_data.vel_data.data();
  real3 *omg_pointer = data_manager->host_data.omg_data.data();
  real3 *pos_pointer = data_manager->host_data.pos_data.data();
  real4 *rot_pointer = data_manager->host_data.rot_data.data();
  real3 *inr_pointer = data_manager->host_data.inr_data.data();
  real3 *frc_pointer = data_manager->host_data.frc_data.data();
  real3 *trq_pointer = data_manager->host_data.trq_data.data();
  bool *active_pointer = data_manager->host_data.active_data.data();
  real *mass_pointer = data_manager->host_data.mass_data.data();
  real3 *lim_pointer = data_manager->host_data.lim_data.data();

  real2* elastic_moduli = data_manager->host_data.elastic_moduli.data();
  real*  mu             = data_manager->host_data.mu.data();
  real*  alpha          = data_manager->host_data.alpha.data();
  real*  cr             = data_manager->host_data.cr.data();
  real*  cohesion       = data_manager->host_data.cohesion_data.data();

#pragma omp parallel for
  for (int i = 0; i < bodylist.size(); i++) {
    bodylist[i]->Update(ChTime);
    bodylist[i]->VariablesFbLoadForces(GetStep());
    bodylist[i]->VariablesQbLoadSpeed();

    ChMatrix<>&     qb = bodylist[i]->Variables().Get_qb();
    ChMatrix<>&     fb = bodylist[i]->Variables().Get_fb();
    ChVector<>&     pos = bodylist[i]->GetPos();
    ChQuaternion<>& rot = bodylist[i]->GetRot();
    ChMatrix33<>&   inertia = bodylist[i]->VariablesBody().GetBodyInvInertia();

    vel_pointer[i] = (R3(qb.ElementN(0), qb.ElementN(1), qb.ElementN(2)));
    omg_pointer[i] = (R3(qb.ElementN(3), qb.ElementN(4), qb.ElementN(5)));
    pos_pointer[i] = (R3(pos.x, pos.y, pos.z));
    rot_pointer[i] = (R4(rot.e0, rot.e1, rot.e2, rot.e3));
    inr_pointer[i] = (R3(inertia.GetElement(0, 0), inertia.GetElement(1, 1), inertia.GetElement(2, 2)));
    frc_pointer[i] = (R3(fb.ElementN(0), fb.ElementN(1), fb.ElementN(2)));     //forces
    trq_pointer[i] = (R3(fb.ElementN(3), fb.ElementN(4), fb.ElementN(5)));     //torques
    active_pointer[i] = bodylist[i]->IsActive();
    mass_pointer[i] = 1.0f / bodylist[i]->VariablesBody().GetBodyMass();
    lim_pointer[i] = (R3(bodylist[i]->GetLimitSpeed(), .05 / GetStep(), .05 / GetStep()));

    ChSharedPtr<ChMaterialSurfaceDEM>& mat = ((ChBodyDEM*) bodylist[i])->GetMaterialSurfaceDEM();

    elastic_moduli[i] = R2(mat->GetYoungModulus(), mat->GetPoissonRatio());
    mu[i]             = mat->GetSfriction();
    cohesion[i]       = mat->GetCohesion();
    //cr[i]             = mat->GetRestitution();

    switch (normal_force_model) {
    case ChContactDEM::HuntCrossley:
      alpha[i] = mat->GetDissipationFactor();
      break;
    }

    bodylist[i]->GetCollisionModel()->SyncPosition();
  }
}

void ChSystemParallelDEM::ChangeCollisionSystem(ChCollisionSystem* newcollsystem)
{
  ChSystemParallel::ChangeCollisionSystem(newcollsystem);

  if (ChCollisionSystemParallel* coll_sys = dynamic_cast<ChCollisionSystemParallel*>(collision_system))
    coll_sys->SetCollisionEnvelope(0);
}

void ChSystemParallelDEM::PrintStepStats()
{
  double timer_solver_setup = data_manager->system_timer.GetTime("ChLcpSolverParallel_Setup");
  double timer_solver_rhs   = data_manager->system_timer.GetTime("ChLcpSolverParallel_RHS");
  double timer_solver_stab  = data_manager->system_timer.GetTime("ChLcpSolverParallel_Stab");

  std::cout << std::endl;
  std::cout << "System Information" << std::endl;
  std::cout << "------------------" << std::endl;
  std::cout << "  Number of bodies     " << GetNumBodies() << std::endl;
  std::cout << "  Number of contacts   " << GetNcontacts() << std::endl;
  std::cout << "  Number of bilaterals " << GetNumBilaterals() << std::endl;
  std::cout << std::endl;
  std::cout << "Timing Information" << std::endl;
  std::cout << "------------------" << std::endl;
  std::cout << "Simulation time        " << GetTimerStep() << std::endl;
  std::cout << "  Collision detection    " << GetTimerCollision() << std::endl;
  std::cout << "    broad phase            " << GetTimerCollisionBroad() << std::endl;
  std::cout << "    narrow phase           " << GetTimerCollisionNarrow() << std::endl;
  std::cout << "  Update                 " << GetTimerUpdate() << std::endl;
  std::cout << "  Solver                 " << GetTimerLcp() << std::endl;
  std::cout << "    contact force calc     " << GetTimerProcessContact() << std::endl;
  std::cout << "    setup                  " << timer_solver_setup << std::endl;
  std::cout << "      RHS                    " << timer_solver_rhs << std::endl;
  std::cout << "    stabilization          " << timer_solver_stab << std::endl;
  std::cout << std::endl;
}
<|endoftext|>


<commit_before>#include "UltrasonicObstacleDetector2020.h"

UltrasonicDetector2020::UltrasonicDetector2020()
{
    DEBUG_REQUEST_REGISTER("UltrasonicDetector2020:draw3D:measurements", "", false);

    rightReceiverInTorso.translation = {47.7, -41.6, 50.9};
    leftReceiverInTorso.translation  = {47.7,  41.6, 50.9};

    rightReceiverInTorso.rotation = RotationMatrix::getRotationZ(-0.4363)
                                    * RotationMatrix::getRotationY(0.2618);
    leftReceiverInTorso.rotation = RotationMatrix::getRotationZ(0.4363)
                                   * RotationMatrix::getRotationY(0.2618);

    rightInWorld.reserve(UltraSoundReceiveData::numOfUSEcho);
    leftInWorld.reserve(UltraSoundReceiveData::numOfUSEcho);

    getDebugParameterList().add(&parameter);
}

void UltrasonicDetector2020::execute()
{
    // reset percept representation and measuremnt vectors
    getUltrasonicObstaclePercept().location_on_ground.clear();
    leftInWorld.clear();
    rightInWorld.clear();

    // transform measurements into world coordinates if not too far away
    Pose3D torso = getKinematicChain().theLinks[KinematicChain::Torso].M;
    for(int i = 0; i < UltraSoundReceiveData::numOfUSEcho; i++) {
        if(getUltraSoundReceiveData().dataLeft[i] < 5.0) { // == 5.0 means no echo
            leftInWorld.push_back(torso * leftReceiverInTorso  * Vector3d(getUltraSoundReceiveData().dataLeft[i] * 1000,0,0));
        }
        if(getUltraSoundReceiveData().dataRight[i] < 5.0) { // == 5.0 means no echo
            rightInWorld.push_back(torso * rightReceiverInTorso  * Vector3d(getUltraSoundReceiveData().dataRight[i] * 1000,0,0));
        }
    }

    // filter out ground (-> later assume reflection?)
    for(auto& point : leftInWorld){
        if(point.z > parameter.ground_threshold){
            getUltrasonicObstaclePercept().location_on_ground.push_back({point.x, point.y});
        }
    }

    for(auto& point : rightInWorld){
        if(point.z > parameter.ground_threshold){
            getUltrasonicObstaclePercept().location_on_ground.push_back({point.x, point.y});
        }
    }

    // 3. provide obstacle model measurements

    draw();
}

//bool UltrasonicObstacleLocator2020::is_new_data_avaliable() const
//{
//  for(int i = 0; i < UltraSoundReceiveData::numOfUSEcho; i++) {
//    if(getUltraSoundReceiveData().dataLeft[i] != lastValidUltraSoundReceiveData.dataLeft[i] ||
//       getUltraSoundReceiveData().dataRight[i] != lastValidUltraSoundReceiveData.dataRight[i])
//    {
//      return true;
//    }
//  }
//  return false;
//}

void UltrasonicDetector2020::draw() const
{
    DEBUG_REQUEST("UltrasonicObstacleLocation2020:draw3D:measurements",
        for(auto& point : leftInWorld){
            SPHERE("0000FF", 10, point);
        }
        for(auto& point : rightInWorld){
            SPHERE("FF0000", 10, point);
        }
    );
}
<commit_msg>fix debug request<commit_after>#include "UltrasonicObstacleDetector2020.h"

UltrasonicDetector2020::UltrasonicDetector2020()
{
    DEBUG_REQUEST_REGISTER("UltrasonicDetector2020:draw3D:measurements", "", false);

    rightReceiverInTorso.translation = {47.7, -41.6, 50.9};
    leftReceiverInTorso.translation  = {47.7,  41.6, 50.9};

    rightReceiverInTorso.rotation = RotationMatrix::getRotationZ(-0.4363)
                                    * RotationMatrix::getRotationY(0.2618);
    leftReceiverInTorso.rotation = RotationMatrix::getRotationZ(0.4363)
                                   * RotationMatrix::getRotationY(0.2618);

    rightInWorld.reserve(UltraSoundReceiveData::numOfUSEcho);
    leftInWorld.reserve(UltraSoundReceiveData::numOfUSEcho);

    getDebugParameterList().add(&parameter);
}

void UltrasonicDetector2020::execute()
{
    // reset percept representation and measuremnt vectors
    getUltrasonicObstaclePercept().location_on_ground.clear();
    leftInWorld.clear();
    rightInWorld.clear();

    // transform measurements into world coordinates if not too far away
    Pose3D torso = getKinematicChain().theLinks[KinematicChain::Torso].M;
    for(int i = 0; i < UltraSoundReceiveData::numOfUSEcho; i++) {
        if(getUltraSoundReceiveData().dataLeft[i] < 5.0) { // == 5.0 means no echo
            leftInWorld.push_back(torso * leftReceiverInTorso  * Vector3d(getUltraSoundReceiveData().dataLeft[i] * 1000,0,0));
        }
        if(getUltraSoundReceiveData().dataRight[i] < 5.0) { // == 5.0 means no echo
            rightInWorld.push_back(torso * rightReceiverInTorso  * Vector3d(getUltraSoundReceiveData().dataRight[i] * 1000,0,0));
        }
    }

    // filter out ground (-> later assume reflection?)
    for(auto& point : leftInWorld){
        if(point.z > parameter.ground_threshold){
            getUltrasonicObstaclePercept().location_on_ground.push_back({point.x, point.y});
        }
    }

    for(auto& point : rightInWorld){
        if(point.z > parameter.ground_threshold){
            getUltrasonicObstaclePercept().location_on_ground.push_back({point.x, point.y});
        }
    }

    // 3. provide obstacle model measurements

    draw();
}

//bool UltrasonicObstacleLocator2020::is_new_data_avaliable() const
//{
//  for(int i = 0; i < UltraSoundReceiveData::numOfUSEcho; i++) {
//    if(getUltraSoundReceiveData().dataLeft[i] != lastValidUltraSoundReceiveData.dataLeft[i] ||
//       getUltraSoundReceiveData().dataRight[i] != lastValidUltraSoundReceiveData.dataRight[i])
//    {
//      return true;
//    }
//  }
//  return false;
//}

void UltrasonicDetector2020::draw() const
{
    DEBUG_REQUEST("UltrasonicDetector2020:draw3D:measurements",
        for(auto& point : leftInWorld){
            SPHERE("0000FF", 10, point);
        }
        for(auto& point : rightInWorld){
            SPHERE("FF0000", 10, point);
        }
    );
}
<|endoftext|>


<commit_before>/************************************************************
USAGE:  This file is used to test a new implementation of an
ecmdDataBuffer Member Function.  It runs the new implementation
and compares the result and performance to the original/old
implementation.
Look for 'CHANGE_CODE_HERE' flags below to see where you have
to update this file
************************************************************/
// Change Log *********************************************************
//
//  Flag Reason   Vers Date     Coder     Description
//  ---- -------- ---- -------- -----     -----------------------------
//  initial Creation   04/20/07 Shashank  Common TestCase framework to compare
//                                        and analyze old and new function in ecmdDataBuffer.C
// End Change Log *****************************************************

#include <list>
#include <string>
#include <stdio.h>
#include <unistd.h>
#include <sys/times.h>
#include <stdlib.h> // for random()

// eCMD Includes
#include <ecmdClientCapi.H>
#include <ecmdDataBuffer.H>
#include <ecmdUtils.H>
#include <ecmdSharedUtils.H>
#include <ecmdStructs.H>

/************************************************************
CHANGE_CODE_HERE
Include the headers and define the macros required for the
preferred function(old version) which user want to analyze
and compare with the new version of that fucntion.The old
function is defined later (see bottom).
************************************************************/

const char* program_name ;

void print_usage(FILE* stream, int exit_code)
{
    fprintf(stream,"Usage: %s  :options \n",program_name);
    fprintf(stream,
            "  help               Display the usage information\n"
            " -bitMax     value   The maximum number of bits user want to set in the DataBuffer\n"
            " -increment  value   The value of increment in the for loop\n"
            " -seed       value   Value for random() # seed that creates test array"
            "  eg. \n"
            "  ecmd_DataBuffer_tc help \n"
            "  ecmd_DataBuffer_tc -bitMax 300 -increment 2 \n");
    exit(exit_code);
}

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

  printf("***Start main***\n");
  uint32_t rc = ECMD_SUCCESS;

  printf("***Start of Testcase...\n");
  printf("***TestCase : To test, compare, and analyze the new function with the old function\n");

  /****************************************
  parse the CommandLine options
  *****************************************/

  program_name=argv[0];

  char * curArg;

  // Default Values
  // this variable is used for the increment value user want to use in the "for" loop
  int increment=1;

  // this variable is used for the maximum bit size user want to set in databuffer.
  int bitMax=100;

  // this variable is used for the 'seed' of srandom
  int seed=1;

  //To display help
  if (argc > 1)
  {
    //To display help
    if ( ( !strcasecmp(argv[1], "help") || !strcasecmp(argv[1], "-h") || !strcasecmp(argv[1], "--help"))
       && (argc > 1) )
      print_usage(stdout,0);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-bitMax");
    if(curArg!=NULL)
        bitMax = (uint32_t)atoi(curArg);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-increment");
    if(curArg!=NULL)
        increment = (uint32_t)atoi(curArg);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-seed");
    if(curArg!=NULL)
        seed = (uint32_t)atoi(curArg);
  } // end of argc>1 check

  printf("***Value of bitMax = %d, increment = %d, seed = %d\n***(Note: These values can be set via cmdline - see 'help')\n",bitMax, increment, seed);

  /****************************************
  Create Variables
  *****************************************/

  //Declaration of DataBuffer
  ecmdDataBuffer buf_old,buf_new;

  //variable declared to record the time taken by the function in CPU ticks
  clock_t start_new,stop_new,start_old,stop_old;
  static clock_t old_time = 0,new_time = 0;

  //Variable used to record the number of times the function is called.
  static uint32_t FunCnt_old = 0,FunCnt_new = 0;

  // Create test array
  uint32_t wordMax = bitMax % 32 ? (bitMax / 32 + 1) : bitMax / 32;
  uint32_t * test_array = new uint32_t[wordMax];
  srandom(seed); // set random 'seed'
  for (uint32_t test_array_loop = 0; test_array_loop < wordMax ; test_array_loop++)
    test_array[test_array_loop] = random();

  /****************************************
  Test Loop
  *****************************************/

  for(int i=0;i<bitMax ; i++){
        for(int j=0 ;j< i+1;j=j+increment){

            //bit size is set in the databuffer for new function
            buf_new.setBitLength(j+1);

            //start time stamp for new function
            start_new=times(NULL);

            /*****************************************************************
            CHANGE_CODE_HERE
            Call new version of function that user want to analyze
            eg.
            rc=buf_new.insert(array,0,j+1);
            This function need to be defined in the ecmaDataBuffer.C
            *****************************************************************/

            //stop time stamp for new function
            stop_new=times(NULL);

            //calculate the time taken by new function in the processing
            new_time=new_time + (stop_new-start_new);

            //calculate the number of times the new function is called
            FunCnt_new=FunCnt_new+1;

            //check if there is no error in the called new function.
            if(rc){
                    printf("\nnew function failed at break point j= %d for i= %d, rc= 0x%x",j,i,rc);
                    break;
            }
            //otherwise process the old function
            else {
                    //bit size is set in the databuffer for the old function
                    buf_old.setBitLength(j+1);

                    //start time stamp for the old function
                    start_old=times(NULL);

                    /**************************************************************
                    CHANGE_CODE_HERE
                    Call old version of function that user want to analyze
                    eg.
                        rc=buf_old.old_insert( array,0,j+1);
                    This function need to be defined here in the test case only(see
                    bottom).
                    **************************************************************/

                    //stop time stamp for the old function.
                    stop_old=times(NULL);

                    //calculate the time taken by old function in the processing
                    old_time=old_time + (stop_old-start_old);

                    //calculate the number of times the new function is called
                    FunCnt_old=FunCnt_old+1;

                    //check if there is no error in the called old function.
                    if(rc){
                            printf("\nold function failed at break point j= %d for i= %d, rc= 0x%x\n",j,i,rc);
                            break;
                    }
                    //comparison is made to check whether the new function and old function give the same result
                    if(buf_old!=buf_new){
                            printf("comparison Failed:the new function and old function do not match\n"
                                   "comparison failed at break point j= %d for i= %d\n"
                                   "the string should be:(0x%s)\n"
                                   "but the string is :(0x%s)\n",
                                    j,i,buf_old.genHexLeftStr().c_str(),buf_new.genHexLeftStr().c_str());
                            rc= ECMD_FAILURE;
                            break;
                    }
            }

        }
  }

  printf("TOTAL TIME in CPU ticks for old_function = %lu for Function Count = %d\n",old_time,FunCnt_old);

  printf("TOTAL TIME in CPU ticks for new_function = %lu for Function Count = %d\n",new_time,FunCnt_new);

  if (rc != ECMD_SUCCESS)
        printf("\n****ERROR: TESTCASE FAILED ,exit returning rc (0x%08x)***\n",rc);
  else {
        printf("\n***Testcase Passed***");
        printf("\n***End of Testcase***\n");
  }
  return rc;

}

/*******************************************************************************************************************
CHANGE_CODE_HERE
 Define here the old version of the function that user want to analyze and compare.This function has been
 copied from the ecmdDataBuffer.C

 eg.

 uint32_t ecmdDataBuffer::old_insert(const uint32_t *i_dataIn, uint32_t i_targetStart, uint32_t i_len, uint32_t i_sourceStart){

    uint32_t rc = ECMD_DBUF_SUCCESS;

    if (i_targetStart+i_len > iv_NumBits) {
        ETRAC3("**** ERROR : ecmdDataBuffer::insert: i_targetStart %d + i_len %d > iv_NumBits (%d)", i_targetStart, i_len, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else if (i_targetStart >= iv_NumBits) {
        ETRAC2("**** ERROR : ecmdDataBuffer::insert: i_targetStart %d >= iv_NumBits (%d)", i_targetStart, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else if (i_len > iv_NumBits) {
        ETRAC2("**** ERROR : ecmdDataBuffer::insert: i_len %d > iv_NumBits (%d)", i_len, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else {

            uint32_t mask = 0x80000000 >> (i_sourceStart % 32);
            const uint32_t * sourcePtr = i_dataIn;
            for (uint32_t i = 0; i < i_len; i++) {
                    if (sourcePtr[(i+i_sourceStart)/32] & mask) {
                        rc = this->setBit(i_targetStart+i);
                    }
                    else {
                        rc = this->clearBit(i_targetStart+i);
                    }

                    mask >>= 1;
                    if (mask == 0x00000000) {
                        mask = 0x80000000;
                    }
                    if (rc) break;
            }
    }
    return rc;
}

********************************************************************************************************************/
<commit_msg>usage instructions update<commit_after>/************************************************************
USAGE:  This file is used to test a new implementation of an
ecmdDataBuffer Member Function.  It runs the new implementation
and compares the result and performance to the original/old
implementation.
Look for 'CHANGE_CODE_HERE' flags below to see where you have
to update this file

************************************************************/
// Change Log *********************************************************
//
//  Flag Reason   Vers Date     Coder     Description
//  ---- -------- ---- -------- -----     -----------------------------
//  initial Creation   04/20/07 Shashank  Common TestCase framework to compare
//                                        and analyze old and new function in ecmdDataBuffer.C
// End Change Log *****************************************************

#include <list>
#include <string>
#include <stdio.h>
#include <unistd.h>
#include <sys/times.h>
#include <stdlib.h> // for random()

// eCMD Includes
#include <ecmdClientCapi.H>
#include <ecmdDataBuffer.H>
#include <ecmdUtils.H>
#include <ecmdSharedUtils.H>
#include <ecmdStructs.H>

/************************************************************
CHANGE_CODE_HERE
Include the headers and define the macros required for the
preferred function(old version) which user want to analyze
and compare with the new version of that fucntion.The old
function is defined later (see bottom).
************************************************************/

const char* program_name ;

void print_usage(FILE* stream, int exit_code)
{
    fprintf(stream,"Usage: %s  :options \n",program_name);
    fprintf(stream,
            "  help               Display the usage information\n"
            " -bitMax     value   The maximum number of bits user want to set in the DataBuffer\n"
            " -increment  value   The value of increment in the for loop\n"
            " -seed       value   Value for random() # seed that creates test array"
            "  eg. \n"
            "  ecmd_DataBuffer_tc help \n"
            "  ecmd_DataBuffer_tc -bitMax 300 -increment 2 \n");
    exit(exit_code);
}

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

  printf("***Start main***\n");
  uint32_t rc = ECMD_SUCCESS;

  printf("***Start of Testcase...\n");
  printf("***TestCase : To test, compare, and analyze the new function with the old function\n");

  /****************************************
  parse the CommandLine options
  *****************************************/

  program_name=argv[0];

  char * curArg;

  // Default Values
  // this variable is used for the increment value user want to use in the "for" loop
  int increment=1;

  // this variable is used for the maximum bit size user want to set in databuffer.
  int bitMax=100;

  // this variable is used for the 'seed' of srandom
  int seed=1;

  //To display help
  if (argc > 1)
  {
    //To display help
    if ( ( !strcasecmp(argv[1], "help") || !strcasecmp(argv[1], "-h") || !strcasecmp(argv[1], "--help"))
       && (argc > 1) )
      print_usage(stdout,0);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-bitMax");
    if(curArg!=NULL)
        bitMax = (uint32_t)atoi(curArg);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-increment");
    if(curArg!=NULL)
        increment = (uint32_t)atoi(curArg);

    curArg = ecmdParseOptionWithArgs(&argc, &argv, "-seed");
    if(curArg!=NULL)
        seed = (uint32_t)atoi(curArg);
  } // end of argc>1 check

  printf("***Value of bitMax = %d, increment = %d, seed = %d\n***(Note: These values can be set via cmdline - see 'help')\n",bitMax, increment, seed);

  /****************************************
  Create Variables
  *****************************************/

  //Declaration of DataBuffer
  ecmdDataBuffer buf_old,buf_new;

  //variable declared to record the time taken by the function in CPU ticks
  clock_t start_new,stop_new,start_old,stop_old;
  static clock_t old_time = 0,new_time = 0;

  //Variable used to record the number of times the function is called.
  static uint32_t FunCnt_old = 0,FunCnt_new = 0;

  // Create test array
  uint32_t wordMax = bitMax % 32 ? (bitMax / 32 + 1) : bitMax / 32;
  uint32_t * test_array = new uint32_t[wordMax];
  srandom(seed); // set random 'seed'
  for (uint32_t test_array_loop = 0; test_array_loop < wordMax ; test_array_loop++)
    test_array[test_array_loop] = random();

  /****************************************
  Test Loop
  *****************************************/

  for(int i=0;i<bitMax ; i++){
        for(int j=0 ;j< i+1;j=j+increment){

            //bit size is set in the databuffer for new function
            buf_new.setBitLength(j+1);

            //start time stamp for new function
            start_new=times(NULL);

            /*****************************************************************
            CHANGE_CODE_HERE
            Call new version of function that user want to analyze
            eg.
            rc=buf_new.insert(array,0,j+1);
            This function need to be defined in the ecmaDataBuffer.C
            *****************************************************************/

            //stop time stamp for new function
            stop_new=times(NULL);

            //calculate the time taken by new function in the processing
            new_time=new_time + (stop_new-start_new);

            //calculate the number of times the new function is called
            FunCnt_new=FunCnt_new+1;

            //check if there is no error in the called new function.
            if(rc){
                    printf("\nnew function failed at break point j= %d for i= %d, rc= 0x%x",j,i,rc);
                    break;
            }
            //otherwise process the old function
            else {
                    //bit size is set in the databuffer for the old function
                    buf_old.setBitLength(j+1);

                    //start time stamp for the old function
                    start_old=times(NULL);

                    /**************************************************************
                    CHANGE_CODE_HERE
                    Call old version of function that user want to analyze
                    eg.
                        rc=buf_old.old_insert( array,0,j+1);
                    This function need to be defined here in the test case only(see
                    bottom).
                    **************************************************************/

                    //stop time stamp for the old function.
                    stop_old=times(NULL);

                    //calculate the time taken by old function in the processing
                    old_time=old_time + (stop_old-start_old);

                    //calculate the number of times the new function is called
                    FunCnt_old=FunCnt_old+1;

                    //check if there is no error in the called old function.
                    if(rc){
                            printf("\nold function failed at break point j= %d for i= %d, rc= 0x%x\n",j,i,rc);
                            break;
                    }
                    //comparison is made to check whether the new function and old function give the same result
                    if(buf_old!=buf_new){
                            printf("comparison Failed:the new function and old function do not match\n"
                                   "comparison failed at break point j= %d for i= %d\n"
                                   "the string should be:(0x%s)\n"
                                   "but the string is :(0x%s)\n",
                                    j,i,buf_old.genHexLeftStr().c_str(),buf_new.genHexLeftStr().c_str());
                            rc= ECMD_FAILURE;
                            break;
                    }
            }

        }
  }

  printf("TOTAL TIME in CPU ticks for old_function = %lu for Function Count = %d\n",old_time,FunCnt_old);

  printf("TOTAL TIME in CPU ticks for new_function = %lu for Function Count = %d\n",new_time,FunCnt_new);

  if (rc != ECMD_SUCCESS)
        printf("\n****ERROR: TESTCASE FAILED ,exit returning rc (0x%08x)***\n",rc);
  else {
        printf("\n***Testcase Passed***");
        printf("\n***End of Testcase***\n");
  }
  return rc;

}

/*******************************************************************************************************************
CHANGE_CODE_HERE
1) Define here the old version of the function that user want to analyze and compare.  This function has been
 copied from the ecmdDataBuffer.C
2) For compile reasons, the old function must also be defined in a local copy of ecmdDataBuffer.H.

 For Example:

 uint32_t ecmdDataBuffer::old_insert(const uint32_t *i_dataIn, uint32_t i_targetStart, uint32_t i_len, uint32_t i_sourceStart){

    uint32_t rc = ECMD_DBUF_SUCCESS;

    if (i_targetStart+i_len > iv_NumBits) {
        ETRAC3("**** ERROR : ecmdDataBuffer::insert: i_targetStart %d + i_len %d > iv_NumBits (%d)", i_targetStart, i_len, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else if (i_targetStart >= iv_NumBits) {
        ETRAC2("**** ERROR : ecmdDataBuffer::insert: i_targetStart %d >= iv_NumBits (%d)", i_targetStart, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else if (i_len > iv_NumBits) {
        ETRAC2("**** ERROR : ecmdDataBuffer::insert: i_len %d > iv_NumBits (%d)", i_len, iv_NumBits);
        RETURN_ERROR(ECMD_DBUF_BUFFER_OVERFLOW);
    } else {

            uint32_t mask = 0x80000000 >> (i_sourceStart % 32);
            const uint32_t * sourcePtr = i_dataIn;
            for (uint32_t i = 0; i < i_len; i++) {
                    if (sourcePtr[(i+i_sourceStart)/32] & mask) {
                        rc = this->setBit(i_targetStart+i);
                    }
                    else {
                        rc = this->clearBit(i_targetStart+i);
                    }

                    mask >>= 1;
                    if (mask == 0x00000000) {
                        mask = 0x80000000;
                    }
                    if (rc) break;
            }
    }
    return rc;
}

********************************************************************************************************************/
<|endoftext|>


<commit_before>#ifndef AUTOCHECK_REPORTER_HPP
#define AUTOCHECK_REPORTER_HPP

#include <iostream>
#include <sstream>
#include <cmath>

#include "distribution.hpp"

namespace autocheck {

  class reporter {
    public:
      virtual void success(size_t tests, size_t max_tests,
          size_t trivial = 0,
          distribution&& dist = distribution()) const = 0;
      virtual void failure(size_t tests, const char* reason) const = 0;
      virtual ~reporter() {}
  };

  int round_percentage(size_t a, size_t b) {
    return static_cast<int>(ceil(100.0 * a / b));
  }

  void report_success(std::ostream& out, size_t tests, size_t max_tests,
      size_t trivial, distribution&& dist)
  {
    if (tests < max_tests) {
      out << "Arguments exhausted after " << tests << " tests."
        << std::endl;
    } else {
      assert(tests == max_tests);
      out << "OK, passed " << tests << " tests";
      if (trivial) {
        out << " (" << round_percentage(trivial, tests) << "% trivial)";
      }
      out << "." << std::endl;
    }

    /* Sort tags in descending order by size. */
    std::sort(dist.begin(), dist.end(),
        [] (const dist_tag& a, const dist_tag& b) {
          return (std::get<1>(a) == std::get<1>(b))
          ? (std::get<0>(a) < std::get<0>(b))
          : (std::get<1>(a) > std::get<1>(b));
        });
    for (const dist_tag& tag : dist) {
      out << round_percentage(std::get<1>(tag), tests) << "% "
        << std::get<0>(tag) << "." << std::endl;
    }
  }

  void report_failure(std::ostream& out, size_t tests, const char* reason) {
    out << "Falsifiable, after " << tests << " tests:" << std::endl
      << reason << std::endl;
  }

  class ostream_reporter : public reporter {
    private:
      std::ostream& out;

    public:
      ostream_reporter(std::ostream& out = std::cout) : out(out) {}

      virtual void success(size_t tests, size_t max_tests,
          size_t trivial, distribution&& dist) const
      {
        report_success(out, tests, max_tests, trivial, std::move(dist));
      }

      virtual void failure(size_t tests, const char* reason) const {
        report_failure(out, tests, reason);
      }
  };

#ifdef ASSERT_TRUE

  class gtest_reporter : public reporter {
    public:
      virtual void success(size_t tests, size_t max_tests,
          size_t trivial, distribution&& dist) const
      {
        report_success(std::clog, tests, max_tests, trivial, std::move(dist));
        ASSERT_TRUE(true);
      }

      virtual void failure(size_t tests, const char* reason) const {
        std::ostringstream out;
        report_failure(out, tests, reason);
        static const bool AUTOCHECK_SUCCESS = false;
        ASSERT_TRUE(AUTOCHECK_SUCCESS) << out.str();
      }
  };

#endif // ASSERT_TRUE

}

#endif

<commit_msg>inlining functions<commit_after>#ifndef AUTOCHECK_REPORTER_HPP
#define AUTOCHECK_REPORTER_HPP

#include <iostream>
#include <sstream>
#include <cmath>

#include "distribution.hpp"

namespace autocheck {

  class reporter {
    public:
      virtual void success(size_t tests, size_t max_tests,
          size_t trivial = 0,
          distribution&& dist = distribution()) const = 0;
      virtual void failure(size_t tests, const char* reason) const = 0;
      virtual ~reporter() {}
  };

  inline int round_percentage(size_t a, size_t b) {
    return static_cast<int>(ceil(100.0 * a / b));
  }

  inline void report_success(std::ostream& out, size_t tests, size_t max_tests,
      size_t trivial, distribution&& dist)
  {
    if (tests < max_tests) {
      out << "Arguments exhausted after " << tests << " tests."
        << std::endl;
    } else {
      assert(tests == max_tests);
      out << "OK, passed " << tests << " tests";
      if (trivial) {
        out << " (" << round_percentage(trivial, tests) << "% trivial)";
      }
      out << "." << std::endl;
    }

    /* Sort tags in descending order by size. */
    std::sort(dist.begin(), dist.end(),
        [] (const dist_tag& a, const dist_tag& b) {
          return (std::get<1>(a) == std::get<1>(b))
          ? (std::get<0>(a) < std::get<0>(b))
          : (std::get<1>(a) > std::get<1>(b));
        });
    for (const dist_tag& tag : dist) {
      out << round_percentage(std::get<1>(tag), tests) << "% "
        << std::get<0>(tag) << "." << std::endl;
    }
  }

  inline void report_failure(std::ostream& out, size_t tests, const char* reason) {
    out << "Falsifiable, after " << tests << " tests:" << std::endl
      << reason << std::endl;
  }

  class ostream_reporter : public reporter {
    private:
      std::ostream& out;

    public:
      ostream_reporter(std::ostream& out = std::cout) : out(out) {}

      virtual void success(size_t tests, size_t max_tests,
          size_t trivial, distribution&& dist) const
      {
        report_success(out, tests, max_tests, trivial, std::move(dist));
      }

      virtual void failure(size_t tests, const char* reason) const {
        report_failure(out, tests, reason);
      }
  };

#ifdef ASSERT_TRUE

  class gtest_reporter : public reporter {
    public:
      virtual void success(size_t tests, size_t max_tests,
          size_t trivial, distribution&& dist) const
      {
        report_success(std::clog, tests, max_tests, trivial, std::move(dist));
        ASSERT_TRUE(true);
      }

      virtual void failure(size_t tests, const char* reason) const {
        std::ostringstream out;
        report_failure(out, tests, reason);
        static const bool AUTOCHECK_SUCCESS = false;
        ASSERT_TRUE(AUTOCHECK_SUCCESS) << out.str();
      }
  };

#endif // ASSERT_TRUE

}

#endif

<|endoftext|>


<commit_before>/**
 * Created by Jian Chen
 * Inspired by the book: C++ Concurrency In Action
 * Thanks to its author: Anthony Williams
 * @since  2016.05.15
 * @author Jian Chen <admin@chensoft.com>
 * @link   http://chensoft.com
 * @thread-safe
 */
#pragma once

#include "semaphore.hpp"
#include "callable.hpp"
#include <functional>
#include <future>
#include <thread>
#include <atomic>
#include <queue>

namespace chen
{
    class threadpool final
    {
    public:
        /**
         * Construct with thread count
         */
        threadpool(std::size_t count = 0);
        ~threadpool();

    public:
        /**
         * Post a function to pool
         */
        template <typename F>
        std::future<typename std::result_of<F()>::type> post(F f)
        {
            typedef typename std::result_of<F()>::type result_type;

            std::packaged_task<result_type()> task(std::move(f));
            std::future<result_type> future(task.get_future());

            std::unique_lock<std::mutex> lock(this->_mutex);
            this->_queue.push(std::move(task));
            lock.unlock();

            this->_semaphore.post();

            return future;
        }

    private:
        /**
         * Working thread
         */
        void run();

        /**
         * Clean pool
         */
        void clean();

    private:
        std::mutex _mutex;
        std::queue<callable> _queue;

        std::atomic<bool> _exit;
        std::vector<std::thread> _pool;

        chen::semaphore _semaphore;

    private:
        threadpool(const threadpool&) = delete;
        threadpool& operator=(const threadpool&) = delete;
    };
}<commit_msg>misc: use emplace<commit_after>/**
 * Created by Jian Chen
 * Inspired by the book: C++ Concurrency In Action
 * Thanks to its author: Anthony Williams
 * @since  2016.05.15
 * @author Jian Chen <admin@chensoft.com>
 * @link   http://chensoft.com
 * @thread-safe
 */
#pragma once

#include "semaphore.hpp"
#include "callable.hpp"
#include <functional>
#include <future>
#include <thread>
#include <atomic>
#include <queue>

namespace chen
{
    class threadpool final
    {
    public:
        /**
         * Construct with thread count
         */
        threadpool(std::size_t count = 0);
        ~threadpool();

    public:
        /**
         * Post a function to pool
         */
        template <typename F>
        std::future<typename std::result_of<F()>::type> post(F f)
        {
            typedef typename std::result_of<F()>::type result_type;

            std::packaged_task<result_type()> task(std::move(f));
            std::future<result_type> future(task.get_future());

            std::unique_lock<std::mutex> lock(this->_mutex);
            this->_queue.emplace(std::move(task));
            lock.unlock();

            this->_semaphore.post();

            return future;
        }

    private:
        /**
         * Working thread
         */
        void run();

        /**
         * Clean pool
         */
        void clean();

    private:
        std::mutex _mutex;
        std::queue<callable> _queue;

        std::atomic<bool> _exit;
        std::vector<std::thread> _pool;

        chen::semaphore _semaphore;

    private:
        threadpool(const threadpool&) = delete;
        threadpool& operator=(const threadpool&) = delete;
    };
}<|endoftext|>