<commit_before>#include <node.h>
#include <node_buffer.h>
#include "libgobgp.h"

using v8::Array;
using v8::Exception;
using v8::FunctionCallbackInfo;
using v8::Isolate;
using v8::Local;
using v8::Object;
using v8::String;
using v8::Value;

void DecodePath(const FunctionCallbackInfo<Value>& args) {
  Isolate* isolate = args.GetIsolate();

  if (args.Length() < 1) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Wrong number of arguments")));
    return;
  }

  if (!args[0]->IsObject()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument")));
    return;
  }
  Local<Object> path_arg = args[0]->ToObject();
  Local<Value> nlri_arg = path_arg->Get(String::NewFromUtf8(isolate, "nlri"));
  Local<Value> pattrs_val = path_arg->Get(String::NewFromUtf8(isolate, "pattrs"));

  if (!pattrs_val->IsArray()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument")));
    return;
  }
  Local<Array> pattrs_arg = Local<Array>::Cast(pattrs_val);

  buf nlri;
  nlri.value = node::Buffer::Data(nlri_arg);
  nlri.len = node::Buffer::Length(nlri_arg);

  buf* pattrs[pattrs_arg->Length()];
  for (int i = 0; i < (int)pattrs_arg->Length(); i++) {
    pattrs[i] = (buf*) malloc(sizeof(buf));
    pattrs[i]->value = node::Buffer::Data(pattrs_arg->Get(i));
    pattrs[i]->len = node::Buffer::Length(pattrs_arg->Get(i));
  }

  path path;
  path.nlri = nlri;
  path.path_attributes = pattrs;
  path.path_attributes_len = pattrs_arg->Length();

  args.GetReturnValue().Set(String::NewFromUtf8(isolate, decode_path(&path)));

  for (int i = 0; i < (int)pattrs_arg->Length(); i++)
    free(pattrs[i]);
}

void SerializePath(const FunctionCallbackInfo<Value>& args) {
  Isolate* isolate = args.GetIsolate();

  if (args.Length() < 2) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Wrong number of arguments")));
    return;
  }

  if (!args[0]->IsNumber()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument")));
    return;
  }
  if (!args[1]->IsString()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument")));
    return;
  }

  path* serialized = serialize_path(args[0]->NumberValue(), *String::Utf8Value(args[1]));

  Local<Object> path = Object::New(isolate);
  path->Set(String::NewFromUtf8(isolate, "nlri"), node::Buffer::New(isolate, serialized->nlri.value, serialized->nlri.len).ToLocalChecked());

  Local<Array> pattrs = Array::New(isolate);
  for (int i = 0; i < serialized->path_attributes_len; i++) {
    pattrs->Set(i, node::Buffer::New(isolate, serialized->path_attributes[i]->value, serialized->path_attributes[i]->len).ToLocalChecked());
  }
  path->Set(String::NewFromUtf8(isolate, "pattrs"), pattrs);

  args.GetReturnValue().Set(path);
}

void init(Local<Object> exports) {
  NODE_SET_METHOD(exports, "decode_path", DecodePath);
  NODE_SET_METHOD(exports, "serialize_path", SerializePath);
}

NODE_MODULE(addon, init)
<commit_msg>Error message should describe the situation in more detail<commit_after>#include <node.h>
#include <node_buffer.h>
#include "libgobgp.h"

using v8::Array;
using v8::Exception;
using v8::FunctionCallbackInfo;
using v8::Isolate;
using v8::Local;
using v8::Object;
using v8::String;
using v8::Value;

void DecodePath(const FunctionCallbackInfo<Value>& args) {
  Isolate* isolate = args.GetIsolate();

  if (args.Length() < 1) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Wrong number of arguments")));
    return;
  }

  if (!args[0]->IsObject()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument: Must be an Object")));
    return;
  }
  Local<Object> path_arg = args[0]->ToObject();
  Local<Value> nlri_arg = path_arg->Get(String::NewFromUtf8(isolate, "nlri"));
  Local<Value> pattrs_val = path_arg->Get(String::NewFromUtf8(isolate, "pattrs"));

  if (!pattrs_val->IsArray()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument: Value of \"pattrs\" must be an Array")));
    return;
  }
  Local<Array> pattrs_arg = Local<Array>::Cast(pattrs_val);

  buf nlri;
  nlri.value = node::Buffer::Data(nlri_arg);
  nlri.len = node::Buffer::Length(nlri_arg);

  buf* pattrs[pattrs_arg->Length()];
  for (int i = 0; i < (int)pattrs_arg->Length(); i++) {
    pattrs[i] = (buf*) malloc(sizeof(buf));
    pattrs[i]->value = node::Buffer::Data(pattrs_arg->Get(i));
    pattrs[i]->len = node::Buffer::Length(pattrs_arg->Get(i));
  }

  path path;
  path.nlri = nlri;
  path.path_attributes = pattrs;
  path.path_attributes_len = pattrs_arg->Length();

  args.GetReturnValue().Set(String::NewFromUtf8(isolate, decode_path(&path)));

  for (int i = 0; i < (int)pattrs_arg->Length(); i++)
    free(pattrs[i]);
}

void SerializePath(const FunctionCallbackInfo<Value>& args) {
  Isolate* isolate = args.GetIsolate();

  if (args.Length() < 2) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Wrong number of arguments")));
    return;
  }

  if (!args[0]->IsNumber()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument: Must be a Number")));
    return;
  }
  if (!args[1]->IsString()) {
    isolate->ThrowException(Exception::TypeError(String::NewFromUtf8(isolate, "Invalid argument: Must be a String")));
    return;
  }

  path* serialized = serialize_path(args[0]->NumberValue(), *String::Utf8Value(args[1]));

  Local<Object> path = Object::New(isolate);
  path->Set(String::NewFromUtf8(isolate, "nlri"), node::Buffer::New(isolate, serialized->nlri.value, serialized->nlri.len).ToLocalChecked());

  Local<Array> pattrs = Array::New(isolate);
  for (int i = 0; i < serialized->path_attributes_len; i++) {
    pattrs->Set(i, node::Buffer::New(isolate, serialized->path_attributes[i]->value, serialized->path_attributes[i]->len).ToLocalChecked());
  }
  path->Set(String::NewFromUtf8(isolate, "pattrs"), pattrs);

  args.GetReturnValue().Set(path);
}

void init(Local<Object> exports) {
  NODE_SET_METHOD(exports, "decode_path", DecodePath);
  NODE_SET_METHOD(exports, "serialize_path", SerializePath);
}

NODE_MODULE(addon, init)
<|endoftext|>


<commit_before>
#include "nmranet/GlobalEventHandler.hxx"
#include "nmranet/NMRAnetEventRegistry.hxx"
#include "if/nmranet_if.h"
#include "core/nmranet_event.h"
#include "nmranet/EventHandlerTemplates.hxx"

/*static*/
GlobalEventFlow* GlobalEventFlow::instance = nullptr;

struct GlobalEventFlow::Impl {
  Impl(int max_event_slots) : pending_sem_(max_event_slots) {}

  // This is the queue of events that are coming from the read thread to the
  // handler thread. Every "released" event is a new incoming event message.
  TypedAllocator<GlobalEventMessage> event_queue_;

  // Statically allocated structure for calling the event handlers from the
  // main event queue.
  EventReport main_event_report_;

  // Each allocated GlobalEventMessage holds one value in this semaphore.
  OSSem pending_sem_;
};

class EventIterator {
 public:

};

GlobalEventFlow::GlobalEventFlow(Executor* executor, int max_event_slots)
    : ControlFlow(executor, CrashNotifiable::DefaultInstance()), impl_(new Impl(max_event_slots)) {
  StartFlowAt(ST(WaitForEvent));
}

ControlFlow::ControlFlowAction GlobalEventFlow::WaitForEvent() {
  return Allocate(&impl_->event_queue_, ST(HandleEvent));
}

void DecodeRange(EventReport* r) {
  uint64_t e = r->event;
  if (e&1) {
    r->mask = (e ^ (e+1)) >> 1;
  } else {
    r->mask = (e ^ (e-1)) >> 1;
  }
  r->event &= ~r->mask;
}

ControlFlow::ControlFlowAction GlobalEventFlow::HandleEvent() {
  GlobalEventMessage* m;
  GetAllocationResult(&m);
  EventReport* rep = &impl_->main_event_report_;
  rep->src_node = m->src_node;
  rep->dst_node = m->dst_node;
  rep->event = m->event;
  rep->mask = EVENT_EXACT_MASK;
  FreeMessage(m);

  EventHandlerFunction fn;
  switch (m->mti) {
    case MTI_EVENT_REPORT:
      fn = &NMRAnetEventHandler::HandleEventReport;
      break;
    case MTI_CONSUMER_IDENTIFY:
      fn = &NMRAnetEventHandler::HandleIdentifyConsumer;
      break;
    case MTI_CONSUMER_IDENTIFIED_RANGE:
      DecodeRange(rep);
      fn = &NMRAnetEventHandler::HandleConsumerRangeIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN:
      rep->state = UNKNOWN;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_VALID:
      rep->state = VALID;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_INVALID:
      rep->state = INVALID;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
      rep->state = RESERVED;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFY:
      fn = &NMRAnetEventHandler::HandleIdentifyProducer;
      break;
    case MTI_PRODUCER_IDENTIFIED_RANGE:
      DecodeRange(rep);
      fn = &NMRAnetEventHandler::HandleProducerRangeIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN:
      rep->state = UNKNOWN;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_VALID:
      rep->state = VALID;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_INVALID:
      rep->state = INVALID;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
      rep->state = RESERVED;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_EVENTS_IDENTIFY_ADDRESSED:
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      fn = &NMRAnetEventHandler::HandleIdentifyGlobal;
      break;
    default:
      DIE("Unexpected message arrived at the global event handler.");
  } //    case
  NMRAnetEventHandler* h = nullptr;
  (h->*fn)(rep, this);
  return YieldAndCall(ST(WaitForEvent));
}

GlobalEventMessage* GlobalEventFlow::AllocateMessage() {
  impl_->pending_sem_.wait();
  return new GlobalEventMessage();
}

void GlobalEventFlow::PostEvent(GlobalEventMessage* message) {
  impl_->event_queue_.TypedRelease(message);
}

void GlobalEventFlow::FreeMessage(GlobalEventMessage* m) {
  delete m;
  impl_->pending_sem_.post();
}

/** Process an event packet.
 * @param mti Message Type Indicator
 * @param node node that the packet is addressed to
 * @param data NMRAnet packet data
 */
extern "C" void nmranet_event_packet_addressed(uint16_t mti,
                                               node_handle_t src,
                                               node_t node,
                                               const void* data) {
  /*struct id_node* id_node = node;
  if (id_node->priv->state == NODE_UNINITIALIZED) {
    return;
    }*/

  GlobalEventMessage* m = GlobalEventFlow::instance->AllocateMessage();
  m->mti = mti;
  m->dst_node = node;
  m->event = 0;
  if (data) {
    memcpy(&m->event, data, sizeof(uint64_t));
    m->event = be64toh(m->event);
  }
  GlobalEventFlow::instance->PostEvent(m);

  switch (mti) {
    default:
      break;
    case MTI_CONSUMER_IDENTIFY:
      //nmranet_identify_consumers(node, event, EVENT_EXACT_MASK);
      break;
    case MTI_CONSUMER_IDENTIFIED_RANGE:
      //nmranet_identify_consumers(node, event, identify_range_mask(event));
      break;
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN: /* fall through */
    case MTI_CONSUMER_IDENTIFIED_VALID:   /* fall through */
    case MTI_CONSUMER_IDENTIFIED_INVALID: /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
      break;
    case MTI_PRODUCER_IDENTIFY:
      //nmranet_identify_producers(node, event, EVENT_EXACT_MASK);
      break;
    case MTI_PRODUCER_IDENTIFIED_RANGE:
      //nmranet_identify_producers(node, event, identify_range_mask(event));
      break;
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN: /* fall through */
    case MTI_PRODUCER_IDENTIFIED_VALID:   /* fall through */
    case MTI_PRODUCER_IDENTIFIED_INVALID: /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
      break;
    case MTI_EVENTS_IDENTIFY_ADDRESSED: /* fall through */
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      //nmranet_identify_consumers(node, 0, EVENT_ALL_MASK);
      //nmranet_identify_producers(node, 0, EVENT_ALL_MASK);
      break;
  }
}

/** Process an event packet.
 * @param mti Message Type Indicator
 * @param src source Node ID
 * @param data NMRAnet packet data
 */
void nmranet_event_packet_global(uint16_t mti,
                                 node_handle_t src,
                                 const void* data) {
  GlobalEventMessage* m = GlobalEventFlow::instance->AllocateMessage();
  m->mti = mti;
  m->dst_node = nullptr;
  m->src_node = src;
  m->event = 0;
  if (data) {
    memcpy(&m->event, data, sizeof(uint64_t));
    m->event = be64toh(m->event);
  }
  GlobalEventFlow::instance->PostEvent(m);

  switch (mti) {
    default:
      break;
    case MTI_EVENT_REPORT: {
      /* to save processing time in instantiations that include a large
       * number of nodes, consumers are sorted at the event level and
       * not at the node level.
       */
      /*struct event_node* event_node;
            struct event_node event_lookup;

      uint64_t event;
      memcpy(&event, data, sizeof(uint64_t));

      event = be64toh(event);
      event_lookup.event = event;

      event_node = RB_FIND(event_tree, &eventHead, &event_lookup);
      if (event_node) {
        for (EventPriv* current = event_node->priv; current != NULL;
             current = current->next) {
          event_post(current->node, src, event);
        }
        }*/
      break;
    }
    case MTI_CONSUMER_IDENTIFY:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RANGE:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_VALID:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_INVALID:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
    /* fall through */
    case MTI_PRODUCER_IDENTIFY:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RANGE:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_VALID:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_INVALID:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
    /* fall through */
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      //      os_mutex_lock(&nodeMutex);
      /* global message, deliver all, non-subscribe */
      /*      for (node_t node = nmranet_node_next(NULL); node != NULL;
           node = nmranet_node_next(node)) {
        nmranet_event_packet_addressed(mti, node, data);
      }
      os_mutex_unlock(&nodeMutex);*/
      break;
  }
}
<commit_msg>Adds an implementation for doing dual iterations in event handler registries; one high-priority for standard events and one low-priority for global identify events. Adds a vector-based event registry implementation.<commit_after>#include <vector>

#include "nmranet/GlobalEventHandler.hxx"
#include "nmranet/NMRAnetEventRegistry.hxx"
#include "if/nmranet_if.h"
#include "core/nmranet_event.h"
#include "nmranet/EventHandlerTemplates.hxx"

/*static*/
GlobalEventFlow* GlobalEventFlow::instance = nullptr;

struct GlobalEventFlow::Impl {
  Impl(int max_event_slots) : pending_sem_(max_event_slots) {}

  // This is the queue of events that are coming from the read thread to the
  // handler thread. Every "released" event is a new incoming event message.
  TypedAllocator<GlobalEventMessage> event_queue_;

  // Statically allocated structure for calling the event handlers from the
  // main event queue.
  EventReport main_event_report_;

  // Each allocated GlobalEventMessage holds one value in this semaphore.
  OSSem pending_sem_;
};

// Abstract class for representing iteration through a container for event
// handlers.
class EventIterator : public Notifiable {
 public:
  EventIterator(Notifiable* parent)
      : event_(nullptr), done_(nullptr), parent_(parent), was_notified_(true) {}

  virtual NMRAnetEventHandler* NextEntry() = 0;

  void InitIterator(EventReport* event, Notifiable* done) {
    HASSERT(event_ == nullptr);
    HASSERT(done_ == nullptr);
    event_ = event;
    done_ = done;
  }
  void ClearIteration() {
    event_ = nullptr;
    done_ = nullptr;
  }
  EventReport* event() { return event_; }
  Notifiable* done() { return done_; }
  Notifiable* NewCallback() {
    HASSERT(was_notified_);
    was_notified_ = false;
    return this;
  }
  virtual void Notify() {
    was_notified_ = true;
    parent_->Notify();
  }
  bool HasBeenNotified() { return was_notified_; }

 private:
  // Stores iteration arguments.
  EventReport* event_;
  // Stores the iteration completion callback.
  Notifiable* done_;
  // Proxies incoming notifications to this parent.
  Notifiable* parent_;
  // When there is an incoming notification, this is set to true.
  bool was_notified_;
};

template <class IT>
class StlIterator : public EventIterator {
 public:
  StlIterator(Notifiable* parent) : EventIterator(parent) {}

  void Set(IT begin, IT end) {
    it_ = begin;
    end_ = end;
  }

  virtual NMRAnetEventHandler* NextEntry() {
    if (it_ == end_)
      return NULL;
    else
      return *it_++;
  }

 private:
  IT it_;
  IT end_;
};

class DualIteratorFlow : public ControlFlow, public ProxyEventHandler {
 public:
  DualIteratorFlow(Executor* executor,
                   EventIterator* standard_iterator,
                   EventIterator* global_iterator)
      : ControlFlow(executor, NULL),
        standard_iterator_(standard_iterator),
        global_iterator_(global_iterator) {
    StartFlowAt(ST(StateInIteration));
  }

  virtual void InitStandardIterator() = 0;

  virtual void HandlerFn(EventHandlerFunction fn,
                         EventReport* event,
                         Notifiable* done) {
    event_handler_mutex.AssertLocked();
    proxy_fn_ = fn;
    standard_iterator_->InitIterator(event, done);
    InitStandardIterator();
    this->Notify();
  }

  virtual void InitGlobalIterator() = 0;

  virtual void HandleIdentifyGlobal(EventReport* event, Notifiable* done) {
    event_handler_mutex.AssertLocked();
    // We immediately release the global iterator mutex and will acquire it
    // inside when we try to call children.
    event_handler_mutex.Unlock();
    global_iterator_->InitIterator(event, done);
    InitGlobalIterator();
    this->Notify();
  }

 protected:
  ControlFlowAction StateInIteration() {
    // We first try a standard iteration.
    if (!standard_iterator_->done())
      return CallImmediately(ST(TryStandardIteration));
    // Then a global iteration.
    if (!global_iterator_->done())
      return CallImmediately(ST(GetGlobalIterationLock));
    // Give up and wait for incoming messages.
    return WaitForNotification();
  }

  ControlFlowAction TryStandardIteration() {
    NMRAnetEventHandler* handler = standard_iterator_->NextEntry();
    if (!handler) {
      // Iteration done.
      standard_iterator_->done()->Notify();
      standard_iterator_->ClearIteration();
      return YieldAndCall(ST(StateInIteration));
    }
    (handler->*proxy_fn_)(standard_iterator_->event(),
                          standard_iterator_->NewCallback());
    return CallImmediately(ST(WaitForStandardReturn));
  }

  ControlFlowAction WaitForStandardReturn() {
    if (!standard_iterator_->HasBeenNotified())
      return WaitForNotification();
    return CallImmediately(ST(TryStandardIteration));
  }

  ControlFlowAction GetGlobalIterationLock() {
    return Allocate(&event_handler_mutex, ST(CallGlobalIteration));
  }

  ControlFlowAction CallGlobalIteration() {
    NMRAnetEventHandler* handler = global_iterator_->NextEntry();
    if (!handler) {
      // Iteration done. We hand back the iterator lock.
      global_iterator_->done()->Notify();
      global_iterator_->ClearIteration();
      return YieldAndCall(ST(StateInIteration));
    }
    handler->HandleIdentifyGlobal(global_iterator_->event(),
                                  global_iterator_->NewCallback());
    return CallImmediately(ST(WaitForGlobalReturn));
  }

  ControlFlowAction WaitForGlobalReturn() {
    if (!global_iterator_->HasBeenNotified())
      return WaitForNotification();
    // Releases the iteration lock and yields to other control flows that might
    // want it.
    event_handler_mutex.Unlock();
    return YieldAndCall(ST(StateInIteration));
  }

 private:
  EventHandlerFunction proxy_fn_;
  EventIterator* standard_iterator_;
  EventIterator* global_iterator_;
};

class VectorEventHandlers : public DualIteratorFlow {
 public:
  VectorEventHandlers(Executor* executor)
      : DualIteratorFlow(executor,
                         &standard_iterator_impl_,
                         &global_iterator_impl_),
        standard_iterator_impl_(this),
        global_iterator_impl_(this) {}

  virtual void InitStandardIterator() {
    standard_iterator_impl_.Set(handlers_.begin(), handlers_.end());
  }

  virtual void InitGlobalIterator() {
    global_iterator_impl_.Set(handlers_.begin(), handlers_.end());
  }

 private:
  typedef std::vector<NMRAnetEventHandler*> HandlersList;
  HandlersList handlers_;
  StlIterator<HandlersList::iterator> standard_iterator_impl_;
  StlIterator<HandlersList::iterator> global_iterator_impl_;
};

GlobalEventFlow::GlobalEventFlow(Executor* executor, int max_event_slots)
    : ControlFlow(executor, CrashNotifiable::DefaultInstance()),
      impl_(new Impl(max_event_slots)) {
  StartFlowAt(ST(WaitForEvent));
}

ControlFlow::ControlFlowAction GlobalEventFlow::WaitForEvent() {
  return Allocate(&impl_->event_queue_, ST(HandleEvent));
}

void DecodeRange(EventReport* r) {
  uint64_t e = r->event;
  if (e & 1) {
    r->mask = (e ^ (e + 1)) >> 1;
  } else {
    r->mask = (e ^ (e - 1)) >> 1;
  }
  r->event &= ~r->mask;
}

ControlFlow::ControlFlowAction GlobalEventFlow::HandleEvent() {
  GlobalEventMessage* m;
  GetAllocationResult(&m);
  EventReport* rep = &impl_->main_event_report_;
  rep->src_node = m->src_node;
  rep->dst_node = m->dst_node;
  rep->event = m->event;
  rep->mask = EVENT_EXACT_MASK;
  FreeMessage(m);

  EventHandlerFunction fn;
  switch (m->mti) {
    case MTI_EVENT_REPORT:
      fn = &NMRAnetEventHandler::HandleEventReport;
      break;
    case MTI_CONSUMER_IDENTIFY:
      fn = &NMRAnetEventHandler::HandleIdentifyConsumer;
      break;
    case MTI_CONSUMER_IDENTIFIED_RANGE:
      DecodeRange(rep);
      fn = &NMRAnetEventHandler::HandleConsumerRangeIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN:
      rep->state = UNKNOWN;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_VALID:
      rep->state = VALID;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_INVALID:
      rep->state = INVALID;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
      rep->state = RESERVED;
      fn = &NMRAnetEventHandler::HandleConsumerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFY:
      fn = &NMRAnetEventHandler::HandleIdentifyProducer;
      break;
    case MTI_PRODUCER_IDENTIFIED_RANGE:
      DecodeRange(rep);
      fn = &NMRAnetEventHandler::HandleProducerRangeIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN:
      rep->state = UNKNOWN;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_VALID:
      rep->state = VALID;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_INVALID:
      rep->state = INVALID;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
      rep->state = RESERVED;
      fn = &NMRAnetEventHandler::HandleProducerIdentified;
      break;
    case MTI_EVENTS_IDENTIFY_ADDRESSED:
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      fn = &NMRAnetEventHandler::HandleIdentifyGlobal;
      break;
    default:
      DIE("Unexpected message arrived at the global event handler.");
  }  //    case
  NMRAnetEventHandler* h = nullptr;
  (h->*fn)(rep, this);
  return YieldAndCall(ST(WaitForEvent));
}

GlobalEventMessage* GlobalEventFlow::AllocateMessage() {
  impl_->pending_sem_.wait();
  return new GlobalEventMessage();
}

void GlobalEventFlow::PostEvent(GlobalEventMessage* message) {
  impl_->event_queue_.TypedRelease(message);
}

void GlobalEventFlow::FreeMessage(GlobalEventMessage* m) {
  delete m;
  impl_->pending_sem_.post();
}

/** Process an event packet.
 * @param mti Message Type Indicator
 * @param node node that the packet is addressed to
 * @param data NMRAnet packet data
 */
extern "C" void nmranet_event_packet_addressed(uint16_t mti,
                                               node_handle_t src,
                                               node_t node,
                                               const void* data) {
  /*struct id_node* id_node = node;
  if (id_node->priv->state == NODE_UNINITIALIZED) {
    return;
    }*/

  GlobalEventMessage* m = GlobalEventFlow::instance->AllocateMessage();
  m->mti = mti;
  m->dst_node = node;
  m->event = 0;
  if (data) {
    memcpy(&m->event, data, sizeof(uint64_t));
    m->event = be64toh(m->event);
  }
  GlobalEventFlow::instance->PostEvent(m);

  switch (mti) {
    default:
      break;
    case MTI_CONSUMER_IDENTIFY:
      // nmranet_identify_consumers(node, event, EVENT_EXACT_MASK);
      break;
    case MTI_CONSUMER_IDENTIFIED_RANGE:
      // nmranet_identify_consumers(node, event, identify_range_mask(event));
      break;
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN: /* fall through */
    case MTI_CONSUMER_IDENTIFIED_VALID:   /* fall through */
    case MTI_CONSUMER_IDENTIFIED_INVALID: /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
      break;
    case MTI_PRODUCER_IDENTIFY:
      // nmranet_identify_producers(node, event, EVENT_EXACT_MASK);
      break;
    case MTI_PRODUCER_IDENTIFIED_RANGE:
      // nmranet_identify_producers(node, event, identify_range_mask(event));
      break;
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN: /* fall through */
    case MTI_PRODUCER_IDENTIFIED_VALID:   /* fall through */
    case MTI_PRODUCER_IDENTIFIED_INVALID: /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
      break;
    case MTI_EVENTS_IDENTIFY_ADDRESSED: /* fall through */
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      // nmranet_identify_consumers(node, 0, EVENT_ALL_MASK);
      // nmranet_identify_producers(node, 0, EVENT_ALL_MASK);
      break;
  }
}

/** Process an event packet.
 * @param mti Message Type Indicator
 * @param src source Node ID
 * @param data NMRAnet packet data
 */
void nmranet_event_packet_global(uint16_t mti,
                                 node_handle_t src,
                                 const void* data) {
  GlobalEventMessage* m = GlobalEventFlow::instance->AllocateMessage();
  m->mti = mti;
  m->dst_node = nullptr;
  m->src_node = src;
  m->event = 0;
  if (data) {
    memcpy(&m->event, data, sizeof(uint64_t));
    m->event = be64toh(m->event);
  }
  GlobalEventFlow::instance->PostEvent(m);

  switch (mti) {
    default:
      break;
    case MTI_EVENT_REPORT: {
      /* to save processing time in instantiations that include a large
       * number of nodes, consumers are sorted at the event level and
       * not at the node level.
       */
      /*struct event_node* event_node;
            struct event_node event_lookup;

      uint64_t event;
      memcpy(&event, data, sizeof(uint64_t));

      event = be64toh(event);
      event_lookup.event = event;

      event_node = RB_FIND(event_tree, &eventHead, &event_lookup);
      if (event_node) {
        for (EventPriv* current = event_node->priv; current != NULL;
             current = current->next) {
          event_post(current->node, src, event);
        }
        }*/
      break;
    }
    case MTI_CONSUMER_IDENTIFY:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RANGE:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_UNKNOWN:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_VALID:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_INVALID:
    /* fall through */
    case MTI_CONSUMER_IDENTIFIED_RESERVED:
    /* fall through */
    case MTI_PRODUCER_IDENTIFY:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RANGE:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_UNKNOWN:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_VALID:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_INVALID:
    /* fall through */
    case MTI_PRODUCER_IDENTIFIED_RESERVED:
    /* fall through */
    case MTI_EVENTS_IDENTIFY_GLOBAL:
      //      os_mutex_lock(&nodeMutex);
      /* global message, deliver all, non-subscribe */
      /*      for (node_t node = nmranet_node_next(NULL); node != NULL;
           node = nmranet_node_next(node)) {
        nmranet_event_packet_addressed(mti, node, data);
      }
      os_mutex_unlock(&nodeMutex);*/
      break;
  }
}
<|endoftext|>


<commit_before>#pragma once

#include "zmsg_types.hpp"

namespace {

template<>
struct zmsg<mid_t::ribbon_set_fs_spec> {
}:

}<commit_msg>补充<commit_after><|endoftext|>


<commit_before>#include <iostream>
#include <cassert>
#include <functional>
#include <type_traits>
#include <cstdint>
#include <vector>
#include <map>
#include <memory>
#include <regex>

// Functional programming

// Function that returns a callable in the form of a lambda
std::function<int (int)> make_adder(int x)
{
    return ([=](int a) -> int{ return x + a; });
}

void do_functional()
{
    // Lambda with no capture
    auto simple_lambda = [](int a){ return a + 3; };
    assert(simple_lambda(5) == 8);

    // Capture by value
    int x = 5;
    auto by_value = [x](int a){ return a + x; };
    assert(by_value(1) == 6);
    x = 7; // change not seen by lambda
    assert(by_value(1) == 6);
    // Also >> at end of template
    assert((std::is_convertible<decltype(by_value),
                                std::function<int(int)>>::value));

    // Capture by reference
    auto by_reference = [&x](int a){ return a + x; };
    assert(by_reference(1) == 8);
    x = 8; // change seen my lambda
    assert(by_reference(1) == 9);

    // Get a callable from a function
    auto add3 = make_adder(3);
    assert(add3(5) == 8);

    auto make_addr_lambda = [](int a) {
        return [a](int b) { return a + b; };
    };

    assert(make_addr_lambda(6)(8) == 14);
}

// Integer types, type traits

template <typename T>
void check_size(size_t size, bool is_signed)
{
    assert(sizeof(T) == size);
    assert(std::is_signed<T>::value == is_signed);
}

void do_inttypes()
{
    // static_assert is a compile-time check
    static_assert(1 == sizeof(int8_t), "int8_t check");
    check_size<int8_t>(1, true);
    check_size<uint8_t>(1, false);
    check_size<int16_t>(2, true);
    check_size<uint16_t>(2, false);
    check_size<int32_t>(4, true);
    check_size<uint32_t>(4, false);
    check_size<int64_t>(8, true);
    check_size<uint64_t>(8, false);

    // auto, decltype
    auto x = 5LL;
    check_size<decltype(x)>(8, true);
    assert((std::is_same<long long, decltype(x)>::value));
}

class A
{
  public:
    static constexpr auto def_value = 5;
    A(int x) :
        x(x)
    {
    }
    // Constructor delegation
    A() : A(def_value)
    {
    }
    int getX() const
    {
        return x;
    }

  private:
    int x;
};

void do_iteration()
{
    // Initializers, foreach syntax, auto for iterators
    std::vector<int> v = { 1, 2, 3, 4 };
    assert(v.size() == 4);
    int sum = 0;
    for (auto i: v)
    {
        sum += i;
    }
    assert(10 == sum);
    for (auto i = v.begin(); i != v.end(); ++i)
    {
        sum += *i;
    }
    assert(20 == sum);

    std::vector<A> v2 = { A(), A(3) };
    assert(5 == v2.at(0).getX());
    assert(3 == v2.at(1).getX());
}

// Variadic template

template<class A1>
void variadic1(A1 const& a1)
{
    assert(a1 == 12);
}

template<class A1, class A2>
void variadic1(A1 const& a1, A2 const& a2)
{
    assert(a1 == a2);
}

template<class ...Args>
void variadic(Args... args)
{
    variadic1(args...);
}

template<class A>
bool pairwise_equal(A const& a, A const& b)
{
    return (a == b);
}

template<class T, class ...Rest>
bool pairwise_equal(T const& a, T const& b, Rest... rest)
{
    return pairwise_equal(a, b) && pairwise_equal(rest...);
}

void do_variadic()
{
    variadic(15, 15);
    variadic(12);
    assert(pairwise_equal(5, 5, 2.0, 2.0, std::string("a"), std::string("a")));
    assert(! pairwise_equal(5, 5, 2.0, 3.0));
}

// deleted, default

class B
{
  public:
    B(int x) :
        x(x)
    {
    }
    B() : B(5)
    {
    }
    int getX() const
    {
        return x;
    }

    virtual ~B() = default;
    B(B const&) = delete;
    B& operator=(B const&) = delete;

  private:
    int x;
};

void do_default_deleted()
{
    B b1;
    assert(5 == b1.getX());
    assert(std::is_copy_constructible<A>::value);
    assert(! std::is_copy_constructible<B>::value);
}

// smart pointers

class C
{
  public:
    C(int id = 0) :
        id(id)
    {
        incr(id);
    }
    ~C()
    {
        decr(id);
    }
    C(C const& rhs) : C(rhs.id)
    {
    }
    C& operator=(C const& rhs)
    {
        if (&rhs != this)
        {
            decr(id);
            id = rhs.id;
            incr(id);
        }
        return *this;
    }
    static void check(size_t size, int v, int count)
    {
        assert(m.size() == size);
        auto p = m.find(v);
        if (p != m.end())
        {
            assert(p->second == count);
        }
    }

  private:
    void incr(int i)
    {
        ++m[i];
    }
    void decr(int i)
    {
        if (--m[i] == 0)
        {
            m.erase(i);
        }
    }

    static std::map<int, int> m;
    int id;
};

std::map<int, int> C::m;

std::shared_ptr<C> make_c(int id)
{
    return std::make_shared<C>(id);
}

std::shared_ptr<C> make_c_array(std::vector<int> const& is)
{
    auto p = std::shared_ptr<C>(new C[is.size()], std::default_delete<C[]>());
    C* pp = p.get();
    for (size_t i = 0; i < is.size(); ++i)
    {
        pp[i] = C(is.at(i));
    }
    return p;
}

void do_smart_pointers()
{
    auto p1 = make_c(1);
    C::check(1, 1, 1);
    auto p2 = make_c_array({2, 3, 4, 5});
    for (auto i: {1, 2, 3, 4, 5})
    {
        C::check(5, i, 1);
    }
    {
        C::check(5, 1, 1);
        C c3(*p1);
        C::check(5, 1, 2);
    }
    C::check(5, 1, 1);
    p1 = nullptr;
    C::check(4, 1, 0);
    p2 = nullptr;
    C::check(0, 0, 0);
    {
        std::unique_ptr<C> p3(new C(6));
        C::check(1, 6, 1);
    }
    C::check(0, 0, 0);
}

// Regular expressions

void do_regex()
{
    // Basic match/search. Match matches whole string; search searches
    // within string. Use std::smatch for matching std::string and
    // std::cmatch for matching char const*.

    std::regex expr1("([0-9]+)");
    std::regex expr2("([0-9]+)\\s*([a-z]+)[[:space:]]*([0-9]+)");
    std::string str1("one 234 fifth 678 nine");
    std::string str2("234 five 678 nine");
    std::string str3("one 234 five 678");
    std::string str4("234");
    std::string str5("234 five 678");
    std::smatch match1;
    assert(! std::regex_match(str1, match1, expr1));
    assert(! std::regex_match(str2, match1, expr1));
    assert(! std::regex_match(str3, match1, expr1));
    assert(std::regex_match(str4, match1, expr1));
    assert(match1[0].first == match1[1].first);
    assert(match1[0].second == match1[1].second);
    std::string s;
    s.assign(match1[1].first, match1[1].second);
    assert("234" == s);
    assert(s == match1[1].str());
    assert(std::regex_match(str5, match1, expr2));
    assert("234 five 678" == match1[0].str());
    assert("234" == match1[1].str());
    assert("five" == match1[2].str());
    assert("678" == match1[3].str());
    assert(std::regex_search(str1, match1, expr2));
    assert("234 fifth 678" == match1[0].str());
    assert("234" == match1[1].str());
    assert("fifth" == match1[2].str());
    assert("678" == match1[3].str());

    // Iterator
    std::regex expr3("[[:digit:]]+");
    std::string str6 = "asdf234erasdf9453.kgdl423asdf";
    std::sregex_iterator m1(str6.begin(), str6.end(), expr3);
    std::sregex_iterator m2;
    s.clear();
    for (std::sregex_iterator iter = m1; iter != m2; ++iter)
    {
	std::smatch const& match2 = *iter;
        s += match2[0].str() + "|";
    }
    assert("234|9453|423|" == s);

    // Submatches
    std::regex expr4("(?:(asdf)|(qwer))");
    char const* str7 = "0asdf1qwer2";
    std::cregex_iterator m3(str7, str7 + std::strlen(str7), expr4);
    assert("asdf" == (*m3)[0].str());
    assert((*m3)[1].matched);
    assert(! (*m3)[2].matched);
    ++m3;
    assert("qwer" == (*m3)[0].str());
    assert(! (*m3)[1].matched);
    assert((*m3)[2].matched);
}

int main()
{
    do_functional();
    do_inttypes();
    do_iteration();
    do_variadic();
    do_default_deleted();
    do_smart_pointers();
    do_regex();
    std::cout << "assertions passed\n";
    return 0;
}
<commit_msg>libtests/cxx11.cc: fix build with gcc 4.8<commit_after>#include <iostream>
#include <cassert>
#include <cstring>
#include <functional>
#include <type_traits>
#include <cstdint>
#include <vector>
#include <map>
#include <memory>
#include <regex>

// Functional programming

// Function that returns a callable in the form of a lambda
std::function<int (int)> make_adder(int x)
{
    return ([=](int a) -> int{ return x + a; });
}

void do_functional()
{
    // Lambda with no capture
    auto simple_lambda = [](int a){ return a + 3; };
    assert(simple_lambda(5) == 8);

    // Capture by value
    int x = 5;
    auto by_value = [x](int a){ return a + x; };
    assert(by_value(1) == 6);
    x = 7; // change not seen by lambda
    assert(by_value(1) == 6);
    // Also >> at end of template
    assert((std::is_convertible<decltype(by_value),
                                std::function<int(int)>>::value));

    // Capture by reference
    auto by_reference = [&x](int a){ return a + x; };
    assert(by_reference(1) == 8);
    x = 8; // change seen my lambda
    assert(by_reference(1) == 9);

    // Get a callable from a function
    auto add3 = make_adder(3);
    assert(add3(5) == 8);

    auto make_addr_lambda = [](int a) {
        return [a](int b) { return a + b; };
    };

    assert(make_addr_lambda(6)(8) == 14);
}

// Integer types, type traits

template <typename T>
void check_size(size_t size, bool is_signed)
{
    assert(sizeof(T) == size);
    assert(std::is_signed<T>::value == is_signed);
}

void do_inttypes()
{
    // static_assert is a compile-time check
    static_assert(1 == sizeof(int8_t), "int8_t check");
    check_size<int8_t>(1, true);
    check_size<uint8_t>(1, false);
    check_size<int16_t>(2, true);
    check_size<uint16_t>(2, false);
    check_size<int32_t>(4, true);
    check_size<uint32_t>(4, false);
    check_size<int64_t>(8, true);
    check_size<uint64_t>(8, false);

    // auto, decltype
    auto x = 5LL;
    check_size<decltype(x)>(8, true);
    assert((std::is_same<long long, decltype(x)>::value));
}

class A
{
  public:
    static constexpr auto def_value = 5;
    A(int x) :
        x(x)
    {
    }
    // Constructor delegation
    A() : A(def_value)
    {
    }
    int getX() const
    {
        return x;
    }

  private:
    int x;
};

void do_iteration()
{
    // Initializers, foreach syntax, auto for iterators
    std::vector<int> v = { 1, 2, 3, 4 };
    assert(v.size() == 4);
    int sum = 0;
    for (auto i: v)
    {
        sum += i;
    }
    assert(10 == sum);
    for (auto i = v.begin(); i != v.end(); ++i)
    {
        sum += *i;
    }
    assert(20 == sum);

    std::vector<A> v2 = { A(), A(3) };
    assert(5 == v2.at(0).getX());
    assert(3 == v2.at(1).getX());
}

// Variadic template

template<class A1>
void variadic1(A1 const& a1)
{
    assert(a1 == 12);
}

template<class A1, class A2>
void variadic1(A1 const& a1, A2 const& a2)
{
    assert(a1 == a2);
}

template<class ...Args>
void variadic(Args... args)
{
    variadic1(args...);
}

template<class A>
bool pairwise_equal(A const& a, A const& b)
{
    return (a == b);
}

template<class T, class ...Rest>
bool pairwise_equal(T const& a, T const& b, Rest... rest)
{
    return pairwise_equal(a, b) && pairwise_equal(rest...);
}

void do_variadic()
{
    variadic(15, 15);
    variadic(12);
    assert(pairwise_equal(5, 5, 2.0, 2.0, std::string("a"), std::string("a")));
    assert(! pairwise_equal(5, 5, 2.0, 3.0));
}

// deleted, default

class B
{
  public:
    B(int x) :
        x(x)
    {
    }
    B() : B(5)
    {
    }
    int getX() const
    {
        return x;
    }

    virtual ~B() = default;
    B(B const&) = delete;
    B& operator=(B const&) = delete;

  private:
    int x;
};

void do_default_deleted()
{
    B b1;
    assert(5 == b1.getX());
    assert(std::is_copy_constructible<A>::value);
    assert(! std::is_copy_constructible<B>::value);
}

// smart pointers

class C
{
  public:
    C(int id = 0) :
        id(id)
    {
        incr(id);
    }
    ~C()
    {
        decr(id);
    }
    C(C const& rhs) : C(rhs.id)
    {
    }
    C& operator=(C const& rhs)
    {
        if (&rhs != this)
        {
            decr(id);
            id = rhs.id;
            incr(id);
        }
        return *this;
    }
    static void check(size_t size, int v, int count)
    {
        assert(m.size() == size);
        auto p = m.find(v);
        if (p != m.end())
        {
            assert(p->second == count);
        }
    }

  private:
    void incr(int i)
    {
        ++m[i];
    }
    void decr(int i)
    {
        if (--m[i] == 0)
        {
            m.erase(i);
        }
    }

    static std::map<int, int> m;
    int id;
};

std::map<int, int> C::m;

std::shared_ptr<C> make_c(int id)
{
    return std::make_shared<C>(id);
}

std::shared_ptr<C> make_c_array(std::vector<int> const& is)
{
    auto p = std::shared_ptr<C>(new C[is.size()], std::default_delete<C[]>());
    C* pp = p.get();
    for (size_t i = 0; i < is.size(); ++i)
    {
        pp[i] = C(is.at(i));
    }
    return p;
}

void do_smart_pointers()
{
    auto p1 = make_c(1);
    C::check(1, 1, 1);
    auto p2 = make_c_array({2, 3, 4, 5});
    for (auto i: {1, 2, 3, 4, 5})
    {
        C::check(5, i, 1);
    }
    {
        C::check(5, 1, 1);
        C c3(*p1);
        C::check(5, 1, 2);
    }
    C::check(5, 1, 1);
    p1 = nullptr;
    C::check(4, 1, 0);
    p2 = nullptr;
    C::check(0, 0, 0);
    {
        std::unique_ptr<C> p3(new C(6));
        C::check(1, 6, 1);
    }
    C::check(0, 0, 0);
}

// Regular expressions

void do_regex()
{
    // Basic match/search. Match matches whole string; search searches
    // within string. Use std::smatch for matching std::string and
    // std::cmatch for matching char const*.

    std::regex expr1("([0-9]+)");
    std::regex expr2("([0-9]+)\\s*([a-z]+)[[:space:]]*([0-9]+)");
    std::string str1("one 234 fifth 678 nine");
    std::string str2("234 five 678 nine");
    std::string str3("one 234 five 678");
    std::string str4("234");
    std::string str5("234 five 678");
    std::smatch match1;
    assert(! std::regex_match(str1, match1, expr1));
    assert(! std::regex_match(str2, match1, expr1));
    assert(! std::regex_match(str3, match1, expr1));
    assert(std::regex_match(str4, match1, expr1));
    assert(match1[0].first == match1[1].first);
    assert(match1[0].second == match1[1].second);
    std::string s;
    s.assign(match1[1].first, match1[1].second);
    assert("234" == s);
    assert(s == match1[1].str());
    assert(std::regex_match(str5, match1, expr2));
    assert("234 five 678" == match1[0].str());
    assert("234" == match1[1].str());
    assert("five" == match1[2].str());
    assert("678" == match1[3].str());
    assert(std::regex_search(str1, match1, expr2));
    assert("234 fifth 678" == match1[0].str());
    assert("234" == match1[1].str());
    assert("fifth" == match1[2].str());
    assert("678" == match1[3].str());

    // Iterator
    std::regex expr3("[[:digit:]]+");
    std::string str6 = "asdf234erasdf9453.kgdl423asdf";
    std::sregex_iterator m1(str6.begin(), str6.end(), expr3);
    std::sregex_iterator m2;
    s.clear();
    for (std::sregex_iterator iter = m1; iter != m2; ++iter)
    {
	std::smatch const& match2 = *iter;
        s += match2[0].str() + "|";
    }
    assert("234|9453|423|" == s);

    // Submatches
    std::regex expr4("(?:(asdf)|(qwer))");
    char const* str7 = "0asdf1qwer2";
    std::cregex_iterator m3(str7, str7 + std::strlen(str7), expr4);
    assert("asdf" == (*m3)[0].str());
    assert((*m3)[1].matched);
    assert(! (*m3)[2].matched);
    ++m3;
    assert("qwer" == (*m3)[0].str());
    assert(! (*m3)[1].matched);
    assert((*m3)[2].matched);
}

int main()
{
    do_functional();
    do_inttypes();
    do_iteration();
    do_variadic();
    do_default_deleted();
    do_smart_pointers();
    do_regex();
    std::cout << "assertions passed\n";
    return 0;
}
<|endoftext|>


<commit_before>/**
 * guard.cc: Functions for thread-safe static initialisation.
 *
 * Static values in C++ can be initialised lazily their first use.  This file
 * contains functions that are used to ensure that two threads attempting to
 * initialize the same static do not call the constructor twice.  This is
 * important because constructors can have side effects, so calling the
 * constructor twice may be very bad.
 *
 * Statics that require initialisation are protected by a 64-bit value.  Any
 * platform that can do 32-bit atomic test and set operations can use this
 * value as a low-overhead lock.  Because statics (in most sane code) are
 * accessed far more times than they are initialised, this lock implementation
 * is heavily optimised towards the case where the static has already been
 * initialised.
 */
#include <stdint.h>
#include <pthread.h>
#include <assert.h>

#ifdef __arm__
// ARM ABI - 32-bit guards.

/**
 * Acquires a lock on a guard, returning 0 if the object has already been
 * initialised, and 1 if it has not.  If the object is already constructed then
 * this function just needs to read a byte from memory and return.
 */
extern "C" int __cxa_guard_acquire(volatile int32_t *guard_object)
{
	if ((1<<31) == *guard_object) { return 0; }
	// If we can atomically move the value from 0 -> 1, then this is
	// uninitialised.
	if (__sync_bool_compare_and_swap(guard_object, 0, 1))
	{
		return 1;
	}
	// If the value is not 0, some other thread was initialising this.  Spin
	// until it's finished.
	while (__sync_bool_compare_and_swap(guard_object, (1<<31), (1<<31)))
	{
		// If the other thread aborted, then we grab the lock
		if (__sync_bool_compare_and_swap(guard_object, 0, 1))
		{
			return 1;
		}
		sched_yield();
	}
	return 0;
}

/**
 * Releases the lock without marking the object as initialised.  This function
 * is called if initialising a static causes an exception to be thrown.
 */
extern "C" void __cxa_guard_abort(int32_t *guard_object)
{
	assert(__sync_bool_compare_and_swap(guard_object, 1, 0));
}
/**
 * Releases the guard and marks the object as initialised.  This function is
 * called after successful initialisation of a static.
 */
extern "C" void __cxa_guard_release(int32_t *guard_object)
{
	assert(__sync_bool_compare_and_swap(guard_object, 1, (1<<31)));
}

#else
// Itanium ABI: 64-bit guards

/**
 * Returns a pointer to the low 32 bits in a 64-bit value, respecting the
 * platform's byte order.
 */
static int32_t *low_32_bits(volatile int64_t *ptr)
{
	int32_t *low= (int32_t*)ptr;
	// Test if the machine is big endian - constant propagation at compile time
	// should eliminate this completely.
	int one = 1;
	if (*(char*)&one != 1)
	{
		low++;
	}
	return low;
}

/**
 * Acquires a lock on a guard, returning 0 if the object has already been
 * initialised, and 1 if it has not.  If the object is already constructed then
 * this function just needs to read a byte from memory and return.
 */
extern "C" int __cxa_guard_acquire(volatile int64_t *guard_object)
{
	char first_byte = (*guard_object) >> 56;
	if (1 == first_byte) { return 0; }
	int32_t *lock = low_32_bits(guard_object);
	// Simple spin lock using the low 32 bits.  We assume that concurrent
	// attempts to initialize statics are very rare, so we don't need to
	// optimise for the case where we have lots of threads trying to acquire
	// the lock at the same time.
	while (!__sync_bool_compare_and_swap_4(lock, 0, 1))
	{
		sched_yield();
	}
	// We have to test the guard again, in case another thread has performed
	// the initialisation while we were trying to acquire the lock.
	first_byte = (*guard_object) >> 56;
	return (1 != first_byte);
}

/**
 * Releases the lock without marking the object as initialised.  This function
 * is called if initialising a static causes an exception to be thrown.
 */
extern "C" void __cxa_guard_abort(int64_t *guard_object)
{
	int32_t *lock = low_32_bits(guard_object);
	*lock = 0;
}
/**
 * Releases the guard and marks the object as initialised.  This function is
 * called after successful initialisation of a static.
 */
extern "C" void __cxa_guard_release(int64_t *guard_object)
{
	// Set the first byte to 1
	*guard_object |= ((int64_t)1) << 56;
	__cxa_guard_abort(guard_object);
}

#endif
<commit_msg>Fix a potential race in guard logic.<commit_after>/**
 * guard.cc: Functions for thread-safe static initialisation.
 *
 * Static values in C++ can be initialised lazily their first use.  This file
 * contains functions that are used to ensure that two threads attempting to
 * initialize the same static do not call the constructor twice.  This is
 * important because constructors can have side effects, so calling the
 * constructor twice may be very bad.
 *
 * Statics that require initialisation are protected by a 64-bit value.  Any
 * platform that can do 32-bit atomic test and set operations can use this
 * value as a low-overhead lock.  Because statics (in most sane code) are
 * accessed far more times than they are initialised, this lock implementation
 * is heavily optimised towards the case where the static has already been
 * initialised.
 */
#include <stdint.h>
#include <pthread.h>
#include <assert.h>

#ifdef __arm__
// ARM ABI - 32-bit guards.

/**
 * Acquires a lock on a guard, returning 0 if the object has already been
 * initialised, and 1 if it has not.  If the object is already constructed then
 * this function just needs to read a byte from memory and return.
 */
extern "C" int __cxa_guard_acquire(volatile int32_t *guard_object)
{
	if ((1<<31) == *guard_object) { return 0; }
	// If we can atomically move the value from 0 -> 1, then this is
	// uninitialised.
	if (__sync_bool_compare_and_swap(guard_object, 0, 1))
	{
		return 1;
	}
	// If the value is not 0, some other thread was initialising this.  Spin
	// until it's finished.
	while (__sync_bool_compare_and_swap(guard_object, (1<<31), (1<<31)))
	{
		// If the other thread aborted, then we grab the lock
		if (__sync_bool_compare_and_swap(guard_object, 0, 1))
		{
			return 1;
		}
		sched_yield();
	}
	return 0;
}

/**
 * Releases the lock without marking the object as initialised.  This function
 * is called if initialising a static causes an exception to be thrown.
 */
extern "C" void __cxa_guard_abort(int32_t *guard_object)
{
	assert(__sync_bool_compare_and_swap(guard_object, 1, 0));
}
/**
 * Releases the guard and marks the object as initialised.  This function is
 * called after successful initialisation of a static.
 */
extern "C" void __cxa_guard_release(int32_t *guard_object)
{
	assert(__sync_bool_compare_and_swap(guard_object, 1, (1<<31)));
}

#else
// Itanium ABI: 64-bit guards

/**
 * Returns a pointer to the low 32 bits in a 64-bit value, respecting the
 * platform's byte order.
 */
static int32_t *low_32_bits(volatile int64_t *ptr)
{
	int32_t *low= (int32_t*)ptr;
	// Test if the machine is big endian - constant propagation at compile time
	// should eliminate this completely.
	int one = 1;
	if (*(char*)&one != 1)
	{
		low++;
	}
	return low;
}

/**
 * Acquires a lock on a guard, returning 0 if the object has already been
 * initialised, and 1 if it has not.  If the object is already constructed then
 * this function just needs to read a byte from memory and return.
 */
extern "C" int __cxa_guard_acquire(volatile int64_t *guard_object)
{
	char first_byte = (*guard_object) >> 56;
	if (1 == first_byte) { return 0; }
	int32_t *lock = low_32_bits(guard_object);
	// Simple spin lock using the low 32 bits.  We assume that concurrent
	// attempts to initialize statics are very rare, so we don't need to
	// optimise for the case where we have lots of threads trying to acquire
	// the lock at the same time.
	while (!__sync_bool_compare_and_swap_4(lock, 0, 1))
	{
		if (1 == ((*guard_object) >> 56))
		{
			break;
		}
		sched_yield();
	}
	// We have to test the guard again, in case another thread has performed
	// the initialisation while we were trying to acquire the lock.
	first_byte = (*guard_object) >> 56;
	return (1 != first_byte);
}

/**
 * Releases the lock without marking the object as initialised.  This function
 * is called if initialising a static causes an exception to be thrown.
 */
extern "C" void __cxa_guard_abort(int64_t *guard_object)
{
	int32_t *lock = low_32_bits(guard_object);
	*lock = 0;
}
/**
 * Releases the guard and marks the object as initialised.  This function is
 * called after successful initialisation of a static.
 */
extern "C" void __cxa_guard_release(int64_t *guard_object)
{
	// Set the first byte to 1
	*guard_object |= ((int64_t)1) << 56;
	__cxa_guard_abort(guard_object);
}

#endif
<|endoftext|>


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

#include <hector_move_group_plugins/octomap_raycast_capability.h>
#include <moveit/move_group/capability_names.h>

#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>

move_group::OctomapRaycastCapability::OctomapRaycastCapability():
  MoveGroupCapability("OctomapRaycastCapability")
{
}

void move_group::OctomapRaycastCapability::initialize()
{

  node_handle_.param("octomap_min_distance_to_obstacle", octo_min_distance_ ,0.05);

  ros::AdvertiseServiceOptions ops=ros::AdvertiseServiceOptions::create<hector_nav_msgs::GetDistanceToObstacle>("get_distance_to_obstacle", boost::bind(&move_group::OctomapRaycastCapability::lookupServiceCallback, this,_1,_2),ros::VoidConstPtr(),&service_queue_);
  dist_lookup_srv_server_ = node_handle_.advertiseService(ops);

  service_thread_=boost::thread(boost::bind(&move_group::OctomapRaycastCapability::serviceThread,this));

  m_markerPub = node_handle_.advertise<visualization_msgs::MarkerArray>("distance_to_obstacle_debug_markers", 1, false);

  tf_ = this->context_->planning_scene_monitor_->getTFClient();
}

void move_group::OctomapRaycastCapability::serviceThread(){
    ros::Rate rate(100.0);
    while (ros::ok()){
        service_queue_.callAvailable(ros::WallDuration(1.0));
        rate.sleep();
    }
}

bool move_group::OctomapRaycastCapability::lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request  &req,
                                                                 hector_nav_msgs::GetDistanceToObstacle::Response &res )
{

  ROS_DEBUG("Octomap distance lookup service called");
  tf::StampedTransform camera_transform;

  const std::string target_frame = context_->planning_scene_monitor_->getPlanningScene()->getPlanningFrame();

  try{
    tf_->waitForTransform(target_frame ,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0));
    tf_->lookupTransform(target_frame, req.point.header.frame_id, req.point.header.stamp, camera_transform);
  }catch(tf::TransformException e){
    ROS_ERROR("Transform failed in lookup distance service call: %s",e.what());
    return false;
  }

  bool useOutliers = true;

  const octomap::point3d origin = octomap::pointTfToOctomap(camera_transform.getOrigin());

  tf::Point end_point = camera_transform * tf::Point(req.point.point.x, req.point.point.y, req.point.point.z);
  tf::Vector3 direction = end_point - camera_transform.getOrigin();

  const octomap::point3d directionOc = octomap::pointTfToOctomap(direction);
  std::vector<octomap::point3d> endPoints;
  std::vector<float> distances;
  int n=3;
  endPoints.resize(1);
  std::vector<octomap::point3d> directions;
  directions.push_back(directionOc);

  planning_scene_monitor::LockedPlanningSceneRO ls (context_->planning_scene_monitor_);

  // Below is inspired by
  // https://github.com/ros-planning/moveit_core/blob/jade-devel/planning_scene/src/planning_scene.cpp#L850
  // and quite hacky. There should be a better way to access the octomap (at least read-only)?
  collision_detection::CollisionWorld::ObjectConstPtr map = ls.getPlanningSceneMonitor()->getPlanningScene()->getWorld()->getObject("<octomap>");
  const shapes::OcTree* octree_shape = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
  const boost::shared_ptr<const octomap::OcTree> octree_ = octree_shape->octree;

  if(octree_->castRay(origin,directions[0],endPoints[0],true,5.0)) {
    distances.push_back(origin.distance(endPoints[0]));
  }

  if (distances.size()!=0) {
    int count_outliers;
    endPoints.resize(1+(n*2));
    get_endpoints(origin, *octree_, distances[0], direction, directions, endPoints, n);
    if (useOutliers) {
      double distance_threshold=0.7;
      count_outliers=0;
      for (size_t i =1;i<endPoints.size();i++){
        ROS_DEBUG("distance to checkpoints %f",endPoints[0].distance(endPoints[i]));
        if(endPoints[0].distance(endPoints[i])>distance_threshold) {
          count_outliers++;
        }
      }
      ROS_DEBUG("corner case: number of outliers: %d ",count_outliers);
    }

    res.distance = distances[0];

    res.end_point.header.frame_id = target_frame;
    res.end_point.header.stamp = req.point.header.stamp;
    res.end_point.point.x = endPoints[0].x();
    res.end_point.point.y = endPoints[0].y();
    res.end_point.point.z = endPoints[0].z();

    if (res.distance < octo_min_distance_) {
      res.distance = -1.0;
      ROS_WARN("Octomap GetDistanceToObstacle got distance under min_Distance -> returning -1.0");
    }

    if (useOutliers) {
      if (count_outliers>=n-1) {
        res.distance=-1.0;
        ROS_DEBUG("Octomap GetDistanceToObstacle Corner Case");
      }
    }
  } else {
    res.distance=-1.0;
  }

  ROS_DEBUG("Result: getDistanceObstacle_Octo: distance: %f",res.distance);
  if (m_markerPub.getNumSubscribers() > 0){
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker marker;
    marker.header.stamp = req.point.header.stamp;
    marker.header.frame_id = target_frame;
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.color.r= 1.0;
    marker.color.a = 1.0;
    marker.scale.x = 0.02;
    marker.ns ="";
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;

    std::vector<geometry_msgs::Point> point_vector;
    for (size_t i = 0; i < endPoints.size(); ++i){
      geometry_msgs::Point tmp = octomap::pointOctomapToMsg(origin);
      point_vector.push_back(tmp);

      tmp = octomap::pointOctomapToMsg(endPoints[i]);
      point_vector.push_back(tmp);
    }
    marker.points=point_vector;
    marker_array.markers.push_back(marker);
    m_markerPub.publish(marker_array);
  }

  return true;

}

void move_group::OctomapRaycastCapability::get_endpoints(const octomap::point3d& origin,
                                                         const octomap::OcTree& octree,
                                                         const float reference_distance,
                                                         const tf::Vector3& direction,
                                                         std::vector<octomap::point3d>& directions,
                                                         std::vector<octomap::point3d>& endPoints,
                                                         int n){
    tf::Vector3 z_axis(0,0,1);

    double tolerance=octree.getResolution()*0.05;

    for (int i=1;i<=n;i++){
        double angle=std::atan2((octree.getResolution()*i)+tolerance,reference_distance);

        tf::Vector3 direction_z_plus = direction.rotate(z_axis, +angle);
        tf::Vector3 direction_z_minus = direction.rotate(z_axis, -angle);

        const octomap::point3d direction_z_plus_Oc = octomap::pointTfToOctomap(direction_z_plus);
        const octomap::point3d direction_z_minus_Oc = octomap::pointTfToOctomap(direction_z_minus);

        directions.push_back(direction_z_plus_Oc);
        directions.push_back(direction_z_minus_Oc);

        octree.castRay(origin,directions[2*i-1],endPoints[2*i-1],true,5.0);
        octree.castRay(origin,directions[2*i],endPoints[2*i],true,5.0);

    }
}

/*
void move_group::OctomapRaycastCapability::visTimerCallback(const ros::TimerEvent& event)
{
  if (octomap_full_pub_.getNumSubscribers() > 0){

    moveit_msgs::PlanningScene tmp;
    moveit_msgs::PlanningSceneComponents comp;
    std::string octomap_frame_id;
    comp.components = moveit_msgs::PlanningSceneComponents::OCTOMAP;

    {
      planning_scene_monitor::LockedPlanningSceneRO ls (context_->planning_scene_monitor_);
      ls.getPlanningSceneMonitor()->getPlanningScene()->getPlanningSceneMsg(tmp, comp);
      octomap_frame_id = ls.getPlanningSceneMonitor()->getPlanningScene()->getPlanningFrame();
    }

    tmp.world.octomap.octomap.header.frame_id = octomap_frame_id;

    octomap_full_pub_.publish(tmp.world.octomap.octomap);
  }
}
*/

//bool move_group::OctomapAccessCapability::clearOctomap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
//{
//  if (!context_->planning_scene_monitor_)
//  {
//    ROS_ERROR("Cannot clear octomap since planning_scene_monitor_ does not exist.");
//    return true;
//  }

//  ROS_INFO("Clearing octomap...");
//  context_->planning_scene_monitor_->clearOctomap();
//  ROS_INFO("Octomap cleared.");
//  return true;
//}

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

#include <hector_move_group_plugins/octomap_raycast_capability.h>
#include <moveit/move_group/capability_names.h>

#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>

move_group::OctomapRaycastCapability::OctomapRaycastCapability():
  MoveGroupCapability("OctomapRaycastCapability")
{
}

void move_group::OctomapRaycastCapability::initialize()
{

  node_handle_.param("octomap_min_distance_to_obstacle", octo_min_distance_ ,0.05);

  ros::AdvertiseServiceOptions ops=ros::AdvertiseServiceOptions::create<hector_nav_msgs::GetDistanceToObstacle>("get_distance_to_obstacle", boost::bind(&move_group::OctomapRaycastCapability::lookupServiceCallback, this,_1,_2),ros::VoidConstPtr(),&service_queue_);
  dist_lookup_srv_server_ = node_handle_.advertiseService(ops);

  service_thread_=boost::thread(boost::bind(&move_group::OctomapRaycastCapability::serviceThread,this));

  m_markerPub = node_handle_.advertise<visualization_msgs::MarkerArray>("distance_to_obstacle_debug_markers", 1, false);

  tf_ = this->context_->planning_scene_monitor_->getTFClient();
}

void move_group::OctomapRaycastCapability::serviceThread(){
    ros::Rate rate(100.0);
    while (ros::ok()){
        service_queue_.callAvailable(ros::WallDuration(1.0));
        rate.sleep();
    }
}

bool move_group::OctomapRaycastCapability::lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request  &req,
                                                                 hector_nav_msgs::GetDistanceToObstacle::Response &res )
{

  ROS_DEBUG("Octomap distance lookup service called");
  tf::StampedTransform camera_transform;

  const std::string target_frame = context_->planning_scene_monitor_->getPlanningScene()->getPlanningFrame();

  try{
    tf_->waitForTransform(target_frame ,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0));
    tf_->lookupTransform(target_frame, req.point.header.frame_id, req.point.header.stamp, camera_transform);
  }catch(tf::TransformException e){
    ROS_ERROR("Transform failed in lookup distance service call: %s",e.what());
    return false;
  }

  bool useOutliers = true;

  const octomap::point3d origin = octomap::pointTfToOctomap(camera_transform.getOrigin());

  tf::Point end_point = camera_transform * tf::Point(req.point.point.x, req.point.point.y, req.point.point.z);
  tf::Vector3 direction = end_point - camera_transform.getOrigin();

  const octomap::point3d directionOc = octomap::pointTfToOctomap(direction);
  std::vector<octomap::point3d> endPoints;
  std::vector<float> distances;
  int n=3;
  endPoints.resize(1);
  std::vector<octomap::point3d> directions;
  directions.push_back(directionOc);

  planning_scene_monitor::LockedPlanningSceneRO ls (context_->planning_scene_monitor_);

  // Below is inspired by
  // https://github.com/ros-planning/moveit_core/blob/jade-devel/planning_scene/src/planning_scene.cpp#L850
  // and quite hacky. There should be a better way to access the octomap (at least read-only)?
  collision_detection::CollisionWorld::ObjectConstPtr map = ls.getPlanningSceneMonitor()->getPlanningScene()->getWorld()->getObject("<octomap>");
  const shapes::OcTree* octree_shape = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
  const boost::shared_ptr<const octomap::OcTree> octree_ = octree_shape->octree;

  if(octree_->castRay(origin,directions[0],endPoints[0],true,5.0)) {
    distances.push_back(origin.distance(endPoints[0]));
  }

  if (distances.size()!=0) {
    int count_outliers;
    endPoints.resize(1+(n*2));
    get_endpoints(origin, *octree_, distances[0], direction, directions, endPoints, n);
    if (useOutliers) {
      double distance_threshold=0.7;
      count_outliers=0;
      for (size_t i =1;i<endPoints.size();i++){
        ROS_DEBUG("distance to checkpoints %f",endPoints[0].distance(endPoints[i]));
        if(endPoints[0].distance(endPoints[i])>distance_threshold) {
          count_outliers++;
        }
      }
      ROS_DEBUG("corner case: number of outliers: %d ",count_outliers);
    }

    res.distance = distances[0];

    res.end_point.header.frame_id = target_frame;
    res.end_point.header.stamp = req.point.header.stamp;
    res.end_point.point.x = endPoints[0].x();
    res.end_point.point.y = endPoints[0].y();
    res.end_point.point.z = endPoints[0].z();

    if (res.distance < octo_min_distance_) {
      res.distance = -1.0;
      ROS_WARN("Octomap GetDistanceToObstacle got distance under min_Distance -> returning -1.0");
    }

    if (useOutliers) {
      if (count_outliers>=n-1) {
        res.distance=-1.0;
        ROS_DEBUG("Octomap GetDistanceToObstacle Corner Case");
      }
    }
  } else {
    res.distance=-1.0;
  }

  ROS_DEBUG("Result: getDistanceObstacle_Octo: distance: %f",res.distance);
  if (m_markerPub.getNumSubscribers() > 0){
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker marker;
    marker.header.stamp = req.point.header.stamp;
    marker.header.frame_id = target_frame;
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.color.r= 1.0;
    marker.color.a = 1.0;
    marker.scale.x = 0.02;
    marker.ns ="";
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;

    std::vector<geometry_msgs::Point> point_vector;
    for (size_t i = 0; i < endPoints.size(); ++i){
      geometry_msgs::Point tmp = octomap::pointOctomapToMsg(origin);
      point_vector.push_back(tmp);

      tmp = octomap::pointOctomapToMsg(endPoints[i]);
      point_vector.push_back(tmp);
    }
    marker.points=point_vector;
    marker_array.markers.push_back(marker);
    m_markerPub.publish(marker_array);
  }

  return true;

}

void move_group::OctomapRaycastCapability::get_endpoints(const octomap::point3d& origin,
                                                         const octomap::OcTree& octree,
                                                         const float reference_distance,
                                                         const tf::Vector3& direction,
                                                         std::vector<octomap::point3d>& directions,
                                                         std::vector<octomap::point3d>& endPoints,
                                                         int n){
    tf::Vector3 z_axis(0,0,1);

    double tolerance=octree.getResolution()*0.05;

    for (int i=1;i<=n;i++){
        double angle=std::atan2((octree.getResolution()*i)+tolerance,reference_distance);

        tf::Vector3 direction_z_plus = direction.rotate(z_axis, +angle);
        tf::Vector3 direction_z_minus = direction.rotate(z_axis, -angle);

        const octomap::point3d direction_z_plus_Oc = octomap::pointTfToOctomap(direction_z_plus);
        const octomap::point3d direction_z_minus_Oc = octomap::pointTfToOctomap(direction_z_minus);

        directions.push_back(direction_z_plus_Oc);
        directions.push_back(direction_z_minus_Oc);

        octree.castRay(origin,directions[2*i-1],endPoints[2*i-1],true,5.0);
        octree.castRay(origin,directions[2*i],endPoints[2*i],true,5.0);

    }
}

#include <class_loader/class_loader.h>
CLASS_LOADER_REGISTER_CLASS(move_group::OctomapRaycastCapability, move_group::MoveGroupCapability)
<|endoftext|>


<commit_before>#include "hrwplayer.h"

QString moduleFileName;

HrwPlayer::HrwPlayer()
{
    setupUi(this);

    audioOutput = new Phonon::AudioOutput(Phonon::MusicCategory, this);
    mediaObject = new Phonon::MediaObject(this);
    metaInformationResolver = new Phonon::MediaObject(this);

    mediaObject->setTickInterval(1000); // for remaining time display

    Phonon::createPath(mediaObject, audioOutput);

    PlayButton->setIcon(QIcon::fromTheme("media-playback-start"));
    StopButton->setIcon(QIcon::fromTheme("media-playback-stop"));
    PauseButton->setIcon(QIcon::fromTheme("media-playback-pause"));
    NextButton->setIcon(QIcon::fromTheme("media-skip-forward"));
    PrevButton->setIcon(QIcon::fromTheme("media-skip-backward"));
    FavoriteButton->setIcon(QIcon::fromTheme("bookmarks"));

    progressBar->setVisible(false);
    DoConnects();
    InitializeSongsList();
}

void HrwPlayer::DoConnects()
{
    connect(mediaObject, SIGNAL(tick(qint64)), this, SLOT(tick(qint64)));
    connect(mediaObject, SIGNAL(stateChanged(Phonon::State,Phonon::State)),
	    this, SLOT(StateChanged(Phonon::State,Phonon::State)));
    //    connect(metaInformationResolver, SIGNAL(stateChanged(Phonon::State,Phonon::State)),
    //            this, SLOT(metaStateChanged(Phonon::State,Phonon::State)));
    //    connect(mediaObject, SIGNAL(currentSourceChanged(Phonon::MediaSource)),
    //            this, SLOT(sourceChanged(Phonon::MediaSource)));
    connect(mediaObject, SIGNAL(finished()), this, SLOT(FinishedPlaying()));

    connect(SongsList,   SIGNAL(itemClicked(QListWidgetItem*)), this,        SLOT(PlaySelected(QListWidgetItem*)));
    connect(AuthorsList, SIGNAL(itemClicked(QListWidgetItem*)), this,        SLOT(PopulateSongs(QListWidgetItem*)));
    connect(PlayButton,  SIGNAL(clicked()), mediaObject, SLOT(play()));
    connect(PauseButton, SIGNAL(clicked()), mediaObject, SLOT(pause()) );
    connect(StopButton,  SIGNAL(clicked()), mediaObject, SLOT(stop()));
    connect(NextButton,  SIGNAL(clicked()), this, SLOT(FinishedPlaying()));
    connect(FavoriteButton,  SIGNAL(clicked()), this, SLOT(handleFavorite()));

    seekSlider->setMediaObject(mediaObject);
    volumeSlider->setAudioOutput(audioOutput);
}

void HrwPlayer::InitializeSongsList()
{
    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setDatabaseName("utwory.sqlite");
    db.open();

    QSqlQuery query("SELECT DISTINCT(author) as author FROM songs ORDER BY author");
    QStringList authors;
    while (query.next()) {
	authors << query.value(0).toString();
    }
    AuthorsList->insertItems(0, authors);
    PopulateSongs(AuthorsList->item(0));
};

HrwPlayer::~HrwPlayer() {};

void HrwPlayer::JustPlay(QString fileName)
{
    mediaObject->setCurrentSource(fileName);
    QFileInfo fileinfo(fileName);
    TitleLabel->setText("Playing \"" + fileinfo.baseName() + "\" by " + CurrentAuthor);
    TimeLabel->setText("00:00");
    mediaObject->play();
}

void HrwPlayer::StateChanged(Phonon::State newState, Phonon::State /* oldState */)
{
    switch (newState)
    {
	case Phonon::ErrorState:
	    if (mediaObject->errorType() == Phonon::FatalError)
	    {
		QMessageBox::warning(this, tr("Fatal Error"),
			mediaObject->errorString());
	    }
	    else
	    {
		QMessageBox::warning(this, tr("Error"),
			mediaObject->errorString());
	    }
	    break;
	case Phonon::PlayingState:
	    PlayButton->setEnabled(false);
	    PauseButton->setEnabled(true);
	    StopButton->setEnabled(true);
	    break;
	case Phonon::StoppedState:
	    StopButton->setEnabled(false);
	    PlayButton->setEnabled(true);
	    PauseButton->setEnabled(false);
	    break;
	case Phonon::PausedState:
	    PauseButton->setEnabled(false);
	    StopButton->setEnabled(true);
	    PlayButton->setEnabled(true);
	    break;
	case Phonon::BufferingState:
	    break;
	default:
	    ;
    }
}

void HrwPlayer::PlaySelected(QListWidgetItem* selectedItem)
{
    QString fileName = buildModuleName(selectedItem->text());

    qDebug() << "PlaySelected - module to play: " << fileName ;

    if(!QFile::exists(fileName))
    {
	qDebug() << "PlaySelected - module not available";

	FetchSong(buildModuleName(selectedItem->text(), false));
    }
    else
    {
	JustPlay(fileName);
    }
}

void HrwPlayer::PopulateSongs(QListWidgetItem* selectedItem)
{
    CurrentAuthor = selectedItem->text();
    QSqlQuery query("SELECT title FROM songs WHERE author = '" + CurrentAuthor + "' ORDER BY title");

    qDebug() << "PopulateSongs - switching to author: " << CurrentAuthor;

    QStringList songs;
    while (query.next()) {
	songs << query.value(0).toString().remove(QRegExp(".mod$"));
    }

    SongsList->clear();
    SongsList->insertItems(0, songs);
}

void HrwPlayer::FinishedPlaying()
{
    QListWidgetItem* selectedItem;

    if(SongsList->currentRow() == (SongsList->count() - 1))
    {
	// for looping over one author
//        selectedItem = SongsList->item(0);
//        SongsList->setCurrentRow(0);
	PopulateSongs(AuthorsList->item(AuthorsList->currentRow() + 1));
	selectedItem =  SongsList->item(0);
	AuthorsList->setCurrentRow(AuthorsList->currentRow() + 1);
	SongsList->setCurrentRow(0);
    }
    else
    {
	selectedItem =  SongsList->item(SongsList->currentRow() + 1);
	SongsList->setCurrentRow(SongsList->currentRow() + 1);
    }

    qDebug() << "play?";
    PlaySelected(selectedItem);
}

void HrwPlayer::tick(qint64 time)
{
    QTime displayTime(0, (time / 60000) % 60, (time / 1000) % 60);

    TimeLabel->setText(displayTime.toString("mm:ss"));
}

void HrwPlayer::FetchSong(QString fileName)
{
    QNetworkAccessManager *manager = new QNetworkAccessManager(this);
    connect(manager, SIGNAL(finished(QNetworkReply*)), this, SLOT(downloadFinished(QNetworkReply*)));

    QString urlSong = "ftp://ftp.amigascne.org/mirrors/ftp.modland.com/pub/modules/Protracker/" + fileName ;

    qDebug() << "FetchSong - module to fetch: " << urlSong ;

    QNetworkReply* reply = manager->get(QNetworkRequest(QUrl(urlSong)));

    progressBar->reset();
    connect(reply, SIGNAL(downloadProgress(qint64, qint64)), this, SLOT(handleProgressBar(qint64, qint64)));
    progressBar->setVisible(true);
}

void HrwPlayer::handleProgressBar(qint64 bytesfetched, qint64 bytestotal)
{
    progressBar->setMaximum(bytestotal);
    progressBar->setValue(bytesfetched);
}

void HrwPlayer::downloadFinished(QNetworkReply *reply)
{
    QUrl url = reply->url();

    if(reply->error())
    {
	//TODO
	qDebug() << "downloadFinished todo ";
    }
    else
    {
	QDir katalog("modules/");
	katalog.mkpath(CurrentAuthor);

	QFileInfo fileinfo(url.path());

	QString fileName = buildModuleName(fileinfo.baseName());

	QFile file(fileName);
	if(file.open(QIODevice::WriteOnly))
	{
	    file.write(reply->readAll());
	    file.close();

	    qDebug() << "downloadFinished - module fetched";

	    JustPlay(fileName);
	}
    }
    progressBar->setVisible(false);
}

QString HrwPlayer::buildModuleName(QString title, bool localName)
{
    QString fullName;

    if(localName)
	fullName.append("modules/");

    fullName += CurrentAuthor + "/" + title + ".mod";

    return fullName;
}

void HrwPlayer::handleFavorite()
{
}
<commit_msg>added some debugs<commit_after>#include "hrwplayer.h"

QString moduleFileName;

HrwPlayer::HrwPlayer()
{
    qDebug() << "HrwPlayer::HrwPlayer()";

    setupUi(this);

    audioOutput = new Phonon::AudioOutput(Phonon::MusicCategory, this);
    mediaObject = new Phonon::MediaObject(this);
    metaInformationResolver = new Phonon::MediaObject(this);

    mediaObject->setTickInterval(1000); // for remaining time display

    Phonon::createPath(mediaObject, audioOutput);

    PlayButton->setIcon(QIcon::fromTheme("media-playback-start"));
    StopButton->setIcon(QIcon::fromTheme("media-playback-stop"));
    PauseButton->setIcon(QIcon::fromTheme("media-playback-pause"));
    NextButton->setIcon(QIcon::fromTheme("media-skip-forward"));
    PrevButton->setIcon(QIcon::fromTheme("media-skip-backward"));
    FavoriteButton->setIcon(QIcon::fromTheme("bookmarks"));

    progressBar->setVisible(false);
    DoConnects();
    InitializeSongsList();
}

void HrwPlayer::DoConnects()
{
    qDebug() << "HrwPlayer::DoConnects()";

    connect(mediaObject, SIGNAL(tick(qint64)), this, SLOT(tick(qint64)));
    connect(mediaObject, SIGNAL(stateChanged(Phonon::State,Phonon::State)),
	    this, SLOT(StateChanged(Phonon::State,Phonon::State)));
    //    connect(metaInformationResolver, SIGNAL(stateChanged(Phonon::State,Phonon::State)),
    //            this, SLOT(metaStateChanged(Phonon::State,Phonon::State)));
    //    connect(mediaObject, SIGNAL(currentSourceChanged(Phonon::MediaSource)),
    //            this, SLOT(sourceChanged(Phonon::MediaSource)));
    connect(mediaObject, SIGNAL(finished()), this, SLOT(FinishedPlaying()));

    connect(SongsList,   SIGNAL(itemClicked(QListWidgetItem*)), this,        SLOT(PlaySelected(QListWidgetItem*)));
    connect(AuthorsList, SIGNAL(itemClicked(QListWidgetItem*)), this,        SLOT(PopulateSongs(QListWidgetItem*)));
    connect(PlayButton,  SIGNAL(clicked()), mediaObject, SLOT(play()));
    connect(PauseButton, SIGNAL(clicked()), mediaObject, SLOT(pause()) );
    connect(StopButton,  SIGNAL(clicked()), mediaObject, SLOT(stop()));
    connect(NextButton,  SIGNAL(clicked()), this, SLOT(FinishedPlaying()));
    connect(FavoriteButton,  SIGNAL(clicked()), this, SLOT(handleFavorite()));

    seekSlider->setMediaObject(mediaObject);
    volumeSlider->setAudioOutput(audioOutput);
}

void HrwPlayer::InitializeSongsList()
{
    qDebug() << "HrwPlayer::InitializeSongsList()";

    QSqlDatabase db = QSqlDatabase::addDatabase("QSQLITE");
    db.setDatabaseName("utwory.sqlite");
    db.open();

    QSqlQuery query("SELECT DISTINCT(author) as author FROM songs ORDER BY author");
    QStringList authors;
    while (query.next()) {
	authors << query.value(0).toString();
    }
    AuthorsList->insertItems(0, authors);
    PopulateSongs(AuthorsList->item(0));
};

HrwPlayer::~HrwPlayer() {};

void HrwPlayer::JustPlay(QString fileName)
{
    qDebug() << "HrwPlayer::JustPlay()";

    mediaObject->setCurrentSource(fileName);
    QFileInfo fileinfo(fileName);
    TitleLabel->setText("Playing \"" + fileinfo.baseName() + "\" by " + CurrentAuthor);
    TimeLabel->setText("00:00");
    mediaObject->play();
}

void HrwPlayer::StateChanged(Phonon::State newState, Phonon::State /* oldState */)
{
    qDebug() << "HrwPlayer::StateChanged()";

    switch (newState)
    {
	case Phonon::ErrorState:
	    if (mediaObject->errorType() == Phonon::FatalError)
	    {
		QMessageBox::warning(this, tr("Fatal Error"),
			mediaObject->errorString());
	    }
	    else
	    {
		QMessageBox::warning(this, tr("Error"),
			mediaObject->errorString());
	    }
	    break;
	case Phonon::PlayingState:
	    PlayButton->setEnabled(false);
	    PauseButton->setEnabled(true);
	    StopButton->setEnabled(true);
	    break;
	case Phonon::StoppedState:
	    StopButton->setEnabled(false);
	    PlayButton->setEnabled(true);
	    PauseButton->setEnabled(false);
	    break;
	case Phonon::PausedState:
	    PauseButton->setEnabled(false);
	    StopButton->setEnabled(true);
	    PlayButton->setEnabled(true);
	    break;
	case Phonon::BufferingState:
	    break;
	default:
	    ;
    }
}

void HrwPlayer::PlaySelected(QListWidgetItem* selectedItem)
{
    qDebug() << "HrwPlayer::PlaySelected()";

    QString fileName = buildModuleName(selectedItem->text());

    qDebug() << "\t" << "PlaySelected - module to play: " << fileName ;

    if(!QFile::exists(fileName))
    {
	qDebug() << "\t" << "PlaySelected - module not available";

	FetchSong(buildModuleName(selectedItem->text(), false));
    }
    else
    {
	JustPlay(fileName);
    }
}

void HrwPlayer::PopulateSongs(QListWidgetItem* selectedItem)
{
    qDebug() << "HrwPlayer::PopulateSongs()";

    CurrentAuthor = selectedItem->text();
    QSqlQuery query("SELECT title FROM songs WHERE author = '" + CurrentAuthor + "' ORDER BY title");

    qDebug() << "\t" << "PopulateSongs - switching to author: " << CurrentAuthor;

    QStringList songs;
    while (query.next()) {
	songs << query.value(0).toString().remove(QRegExp(".mod$"));
    }

    SongsList->clear();
    SongsList->insertItems(0, songs);
}

void HrwPlayer::FinishedPlaying()
{
    qDebug() << "HrwPlayer::FinishedPlaying()";

    QListWidgetItem* selectedItem;

    if(SongsList->currentRow() == (SongsList->count() - 1))
    {
	// for looping over one author
//        selectedItem = SongsList->item(0);
//        SongsList->setCurrentRow(0);
	PopulateSongs(AuthorsList->item(AuthorsList->currentRow() + 1));
	selectedItem =  SongsList->item(0);
	AuthorsList->setCurrentRow(AuthorsList->currentRow() + 1);
	SongsList->setCurrentRow(0);
    }
    else
    {
	selectedItem =  SongsList->item(SongsList->currentRow() + 1);
	SongsList->setCurrentRow(SongsList->currentRow() + 1);
    }

    qDebug() << "\t" << "play?";
    PlaySelected(selectedItem);
}

void HrwPlayer::tick(qint64 time)
{
    qDebug() << "HrwPlayer::tick()";

    QTime displayTime(0, (time / 60000) % 60, (time / 1000) % 60);

    TimeLabel->setText(displayTime.toString("mm:ss"));
}

void HrwPlayer::FetchSong(QString fileName)
{
    qDebug() << "HrwPlayer::FetchSong()";

    QNetworkAccessManager *manager = new QNetworkAccessManager(this);
    connect(manager, SIGNAL(finished(QNetworkReply*)), this, SLOT(downloadFinished(QNetworkReply*)));

    QString urlSong = "ftp://ftp.amigascne.org/mirrors/ftp.modland.com/pub/modules/Protracker/" + fileName ;

    qDebug() << "\t" << "FetchSong - module to fetch: " << urlSong ;

    QNetworkReply* reply = manager->get(QNetworkRequest(QUrl(urlSong)));

    progressBar->reset();
    connect(reply, SIGNAL(downloadProgress(qint64, qint64)), this, SLOT(handleProgressBar(qint64, qint64)));
    progressBar->setVisible(true);
}

void HrwPlayer::handleProgressBar(qint64 bytesfetched, qint64 bytestotal)
{
    qDebug() << "HrwPlayer::handleProgressBar()";

    progressBar->setMaximum(bytestotal);
    progressBar->setValue(bytesfetched);
}

void HrwPlayer::downloadFinished(QNetworkReply *reply)
{
    qDebug() << "HrwPlayer::downloadFinished()";

    QUrl url = reply->url();

    if(reply->error())
    {
	//TODO
	qDebug() << "\t" << "downloadFinished todo ";
    }
    else
    {
	QDir katalog("modules/");
	katalog.mkpath(CurrentAuthor);

	QFileInfo fileinfo(url.path());

	QString fileName = buildModuleName(fileinfo.baseName());

	QFile file(fileName);
	if(file.open(QIODevice::WriteOnly))
	{
	    file.write(reply->readAll());
	    file.close();

	    qDebug() << "\t" << "downloadFinished - module fetched";

	    JustPlay(fileName);
	}
    }
    progressBar->setVisible(false);
}

QString HrwPlayer::buildModuleName(QString title, bool localName)
{
    QString fullName;

    if(localName)
	fullName.append("modules/");

    fullName += CurrentAuthor + "/" + title + ".mod";

    return fullName;
}

void HrwPlayer::handleFavorite()
{
    qDebug() << "HrwPlayer::handleFavorite()";

}
<|endoftext|>