text:
/*
 * Copyright 2015 David A. Boyuka II
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * test-index-io.cpp
 *
 *  Created on: May 10, 2014
 *      Author: David A. Boyuka II
 */

#include <cstdint>
#include <cassert>
#include <cstdlib>
#include <vector>
#include <map>
#include <set>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include "pique/data/grid.hpp"
#include "pique/data/dataset.hpp"
#include "pique/region/region-encoding.hpp"
#include "pique/indexing/binned-index.hpp"
#include "pique/indexing/index-builder.hpp"
#include "pique/io/index-io.hpp"
#include "pique/io/posix/posix-index-io.hpp"
#include "pique/util/universal-value.hpp"

#include "pique/region/ii/ii.hpp"
#include "pique/region/ii/ii-encode.hpp"
#include "pique/region/cii/cii.hpp"
#include "pique/region/cii/cii-encode.hpp"
#include "pique/region/cblq/cblq.hpp"
#include "pique/region/cblq/cblq-encode.hpp"
#include "pique/region/wah/wah.hpp"
#include "pique/region/wah/wah-encode.hpp"

#include "make-index.hpp"
#include "write-and-verify-index.hpp"
#include "standard-datasets.hpp"

template<typename RegionEncoderT>
static void test_index_io(typename RegionEncoderT::RegionEncoderConfig conf, const std::vector<int> &domain, std::string indexfile = "test-index-io.idx") {
	typedef typename RegionEncoderT::RegionEncodingOutT RegionEncodingT;
	using QKeyType = typename SigbitsBinningSpecification< int >::QKeyType;
	using region_id_t = BinnedIndexTypes::region_id_t;

	// Build an index to serialize
	boost::shared_ptr< BinnedIndex > index = make_index< RegionEncoderT, int >(conf, domain);

	write_and_verify_index(index, indexfile);

#ifdef VERBOSE_TESTS
	printf("Index IO test %s passed!\n", typeid(RegionEncodingT).name());
#endif
}

int main(int argc, char **argv) {
	char *tempdir_cstr = std::getenv("testworkdir");
	std::string tempdir = (tempdir_cstr ? tempdir_cstr : "test.work.dir");

	std::string tempfile = tempdir + "/test-index-io.idx";

	test_index_io< IIRegionEncoder >(IIRegionEncoderConfig(), SMALL_DOMAIN, tempfile);
	test_index_io< CIIRegionEncoder >(CIIRegionEncoderConfig(), SMALL_DOMAIN, tempfile);
	test_index_io< WAHRegionEncoder >(WAHRegionEncoderConfig(), SMALL_DOMAIN, tempfile);
	test_index_io< CBLQRegionEncoder<2> >(CBLQRegionEncoderConfig(false), SMALL_DOMAIN, tempfile);
	test_index_io< CBLQRegionEncoder<2> >(CBLQRegionEncoderConfig(true), SMALL_DOMAIN, tempfile);

	srand(12345);
	std::vector<int> big_domain = make_big_domain(1ULL << 14, 30);

	test_index_io< IIRegionEncoder >(IIRegionEncoderConfig(), big_domain, tempfile);
	test_index_io< CIIRegionEncoder >(CIIRegionEncoderConfig(), big_domain, tempfile);
	test_index_io< WAHRegionEncoder >(WAHRegionEncoderConfig(), big_domain, tempfile);
	test_index_io< CBLQRegionEncoder<2> >(CBLQRegionEncoderConfig(false), big_domain, tempfile);
	test_index_io< CBLQRegionEncoder<2> >(CBLQRegionEncoderConfig(true), big_domain, tempfile);
}

ext: cpp

max_stars_repo_name: daboyuka/PIQUE

max_issues_repo_name: daboyuka/PIQUE

max_forks_repo_name: daboyuka/PIQUE


text:
#include <NTL/ZZVec.h>

NTL_START_IMPL

void ZZVec::SetSize(long n, long d)
{
   if (n < 0 || d <= 0) LogicError("bad args to ZZVec::SetSize()");

   if (v)
      LogicError("illegal ZZVec initialization");

   if (n == 0) {
      len = n;
      bsize = d;
      return;
   }

   ZZVec tmp;
   tmp.len = 0;
   tmp.bsize = d;

   tmp.v = (ZZ*) NTL_SNS_MALLOC(n, sizeof(ZZ), 0);
   if (!tmp.v) MemoryError();

   long i = 0;
   long m;
   long j;

   while (i < n) {
      m = ZZ_BlockConstructAlloc(tmp.v[i], d, n-i);
      for (j = 1; j < m; j++)
         ZZ_BlockConstructSet(tmp.v[i], tmp.v[i+j], j);
      i += m;
      tmp.len = i;
   }

   tmp.swap(*this);
}

void ZZVec::kill()
{
   long n = len;
   long i = 0;
   while (i < n) {
      long m = ZZ_BlockDestroy(v[i]);
      i += m;
   }

   len = 0;
   bsize = 0;
   if (v) {
      free(v);
      v = 0;
   }
}

ZZVec& ZZVec::operator=(const ZZVec& a)
{
   if (this == &a) return *this;
   ZZVec tmp(a);
   tmp.swap(*this);
   return *this;
}

ZZVec::ZZVec(const ZZVec& a) : v(0), len(0), bsize(0)
{
   SetSize(a.len, a.bsize);

   long i;
   for (i = 0; i < a.len; i++)
      v[i] = (a.v)[i];
}

NTL_END_IMPL


text:
// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.

#include <algorithm>
#include <boost/optional.hpp>
#include <iterator>
#include <stdint.h>
#include <utility>
#include <vector>

#include "kudu/common/common.pb.h"
#include "kudu/common/partial_row.h"
#include "kudu/common/partition.h"
#include "kudu/common/row.h"
#include "kudu/common/schema.h"
#include "kudu/gutil/strings/escaping.h"
#include "kudu/gutil/strings/substitute.h"
#include "kudu/util/hash_util.h"
#include "kudu/util/test_util.h"

using boost::optional;
using std::pair;
using std::string;
using std::vector;

namespace kudu {

namespace {
void AddHashBucketComponent(PartitionSchemaPB* partition_schema_pb,
                            const vector<string>& columns,
                            uint32_t num_buckets, int32_t seed) {
  PartitionSchemaPB::HashBucketSchemaPB* hash_bucket_schema =
      partition_schema_pb->add_hash_bucket_schemas();
  for (const string& column : columns) {
    hash_bucket_schema->add_columns()->set_name(column);
  }
  hash_bucket_schema->set_num_buckets(num_buckets);
  hash_bucket_schema->set_seed(seed);
}

void SetRangePartitionComponent(PartitionSchemaPB* partition_schema_pb,
                                const vector<string>& columns) {
  PartitionSchemaPB::RangeSchemaPB* range_schema = partition_schema_pb->mutable_range_schema();
  range_schema->Clear();
  for (const string& column : columns) {
    range_schema->add_columns()->set_name(column);
  }
}

void CheckCreateRangePartitions(const vector<pair<optional<string>, optional<string>>>& raw_bounds,
                                const vector<string>& raw_splits,
                                const vector<pair<string, string>>& expected_partition_ranges) {
  CHECK_EQ(std::max(raw_bounds.size(), 1UL) + raw_splits.size(), expected_partition_ranges.size());

  // CREATE TABLE t (col STRING PRIMARY KEY),
  // PARITITION BY RANGE (col);
  Schema schema({ ColumnSchema("col", STRING) }, { ColumnId(0) }, 1);

  PartitionSchema partition_schema;
  ASSERT_OK(PartitionSchema::FromPB(PartitionSchemaPB(), schema, &partition_schema));

  ASSERT_EQ("range columns: [col]", partition_schema.DebugString(schema));

  vector<pair<KuduPartialRow, KuduPartialRow>> bounds;
  for (const auto& bound : raw_bounds) {
    const optional<string>& lower = bound.first;
    const optional<string>& upper = bound.second;
    KuduPartialRow lower_bound(&schema);
    KuduPartialRow upper_bound(&schema);
    if (lower) {
      ASSERT_OK(lower_bound.SetStringCopy("col", *lower));
    }
    if (upper) {
      ASSERT_OK(upper_bound.SetStringCopy("col", *upper));
    }
    bounds.emplace_back(std::move(lower_bound), std::move(upper_bound));
  }

  vector<KuduPartialRow> splits;
  for (const string& split_value : raw_splits) {
    KuduPartialRow split(&schema);
    ASSERT_OK(split.SetStringCopy("col", split_value));
    splits.emplace_back(split);
  }

  vector<Partition> partitions;
  ASSERT_OK(partition_schema.CreatePartitions(splits, bounds, schema, &partitions));
  ASSERT_EQ(expected_partition_ranges.size(), partitions.size());

  for (int i = 0; i < partitions.size(); i++) {
    const string& lower = expected_partition_ranges[i].first;
    const string& upper = expected_partition_ranges[i].second;

    EXPECT_TRUE(partitions[i].hash_buckets().empty());
    EXPECT_EQ(lower, partitions[i].range_key_start());
    EXPECT_EQ(upper, partitions[i].range_key_end());
    EXPECT_EQ(lower, partitions[i].partition_key_start());
    EXPECT_EQ(upper, partitions[i].partition_key_end());

    EXPECT_EQ(strings::Substitute("range: [($0$1), ($2$3))",
                                  lower.empty() ? "" : "string col=",
                                  lower.empty() ? "<start>" : strings::Utf8SafeCEscape(lower),
                                  upper.empty() ? "" : "string col=",
                                  upper.empty() ? "<end>" : strings::Utf8SafeCEscape(upper)),
              partition_schema.PartitionDebugString(partitions[i], schema));
  }
}

} // namespace

TEST(PartitionTest, TestPartitionKeyEncoding) {
  // CREATE TABLE t (a INT32, b VARCHAR, c VARCHAR, PRIMARY KEY (a, b, c))
  // PARITITION BY [HASH BUCKET (a, b), HASH BUCKET (c), RANGE (a, b, c)];
  Schema schema({ ColumnSchema("a", INT32),
                  ColumnSchema("b", STRING),
                  ColumnSchema("c", STRING) },
                { ColumnId(0), ColumnId(1), ColumnId(2) }, 3);

  PartitionSchemaPB schema_builder;
  AddHashBucketComponent(&schema_builder, { "a", "b" }, 32, 0);
  AddHashBucketComponent(&schema_builder, { "c" }, 32, 42);
  PartitionSchema partition_schema;
  ASSERT_OK(PartitionSchema::FromPB(schema_builder, schema, &partition_schema));

  ASSERT_EQ("hash bucket components: [(bucket count: 32, columns: [a, b]), "
            "(bucket count: 32, seed: 42, columns: [c])], range columns: [a, b, c]",
            partition_schema.DebugString(schema));

  {
    string key;
    KuduPartialRow row(&schema);
    ASSERT_OK(row.SetInt32("a", 0));
    ASSERT_OK(partition_schema.EncodeKey(row, &key));

    EXPECT_EQ(string("\0\0\0\0"   // hash(0, "")
                     "\0\0\0\x14" // hash("")
                     "\x80\0\0\0" // a = 0
                     "\0\0",      // b = ""; c is elided
                     14), key);
    string debug = "bucket=0, bucket=20, int32 a=0, string b=, string c=";
    EXPECT_EQ(debug, partition_schema.RowDebugString(row));
    EXPECT_EQ(debug, partition_schema.PartitionKeyDebugString(key, schema));
  }

  {
    string key;
    KuduPartialRow row(&schema);
    ASSERT_OK(row.SetInt32("a", 1));
    ASSERT_OK(partition_schema.EncodeKey(row, &key));

    EXPECT_EQ(string("\0\0\0\x5"    // hash(1, "")
                     "\0\0\0\x14"   // hash("")
                     "\x80\0\0\x01" // a = 1
                     "\0\0",        // b = ""; c is elided
                     14), key);

    string debug_b = "bucket=5, bucket=20, int32 a=1, string b=, string c=";
    EXPECT_EQ(debug_b, partition_schema.RowDebugString(row));
    EXPECT_EQ(debug_b, partition_schema.PartitionKeyDebugString(key, schema));
  }

  {
    string key;
    KuduPartialRow row(&schema);
    ASSERT_OK(row.SetInt32("a", 0));
    ASSERT_OK(row.SetStringCopy("b", "b"));
    ASSERT_OK(row.SetStringCopy("c", "c"));
    ASSERT_OK(partition_schema.EncodeKey(row, &key));

    EXPECT_EQ(string("\0\0\0\x1A" // hash(0, "b")
                     "\0\0\0\x1D" // hash("c")
                     "\x80\0\0\0" // a = 0
                     "b\0\0"      // b = "b"
                     "c",         // c = "c"
                     16), key);

    string debug = "bucket=26, bucket=29, int32 a=0, string b=b, string c=c";
    EXPECT_EQ(debug, partition_schema.RowDebugString(row));
    EXPECT_EQ(debug, partition_schema.PartitionKeyDebugString(key, schema));
  }

  {
    string key;
    KuduPartialRow row(&schema);
    ASSERT_OK(row.SetInt32("a", 1));
    ASSERT_OK(row.SetStringCopy("b", "b"));
    ASSERT_OK(row.SetStringCopy("c", "c"));
    ASSERT_OK(partition_schema.EncodeKey(row, &key));

    EXPECT_EQ(string("\0\0\0\x0"   // hash(1, "b")
                     "\0\0\0\x1D"  // hash("c")
                     "\x80\0\0\x1" // a = 1
                     "b\0\0"       // b = "b"
                     "c",          // c = "c"
                     16), key);

    string debug = "bucket=0, bucket=29, int32 a=1, string b=b, string c=c";
    EXPECT_EQ(debug, partition_schema.RowDebugString(row));
    EXPECT_EQ(debug, partition_schema.PartitionKeyDebugString(key, schema));
  }
}

TEST(PartitionTest, TestCreateRangePartitions) {
  {
    // Splits:
    // { a: "1" }
    // { a: "2" }
    // { a: "2\0" }
    //
    // Encoded Partition Keys:
    // [ ( ""), ("1") )
    // [ ("1"), ("2") )
    // [ ("2"), ("2\0") )
    // [ ("2\0"), ("") )

    vector<string> splits { "2", "1", string("2\0", 2) };
    vector<pair<string, string>> partitions {
      { "", "1" },
      { "1", "2" },
      { "2", string("2\0", 2) },
      { string("2\0", 2), "" },
    };
    CheckCreateRangePartitions({}, splits, partitions);
  }
  {
    // Bounds
    //
    // ({ a: "a" }, { a: "m" })
    // ({ a: "x" }, { a: "z" })
    //
    // Encoded Partition Keys:
    //
    // [ ("a"), ("m") )
    // [ ("x"), ("z") )

    vector<pair<optional<string>, optional<string>>> bounds {
      { string("a"), string("m") },
      { string("x"), string("z") },
    };
    vector<pair<string, string>> partitions {
      { "a", "m" },
      { "x", "z" },
    };
    CheckCreateRangePartitions(bounds, {}, partitions);
  }
  {
    // Bounds:
    // ({          }, { col: "b" })
    // ({ col: "c" }, { col: "f" })
    // ({ col: "f" }, { col: "z" })
    //
    // Splits:
    // { col: "a"   }
    // { col: "d"   }
    // { col: "e"   }
    // { col: "h"   }
    // { col: "h\0" }
    //
    // Encoded Partition Keys:
    // [ (""),    ("a")   )
    // [ ("a"),   ("b")   )
    // [ ("c"),   ("d")   )
    // [ ("d"),   ("e")   )
    // [ ("e"),   ("f")   )
    // [ ("f"),   ("h")   )
    // [ ("h"),   ("h\0") )
    // [ ("h\0"), ("z")   )

    vector<pair<optional<string>, optional<string>>> bounds {
      { boost::none, string("b") },
      { string("c"), string("f") },
      { string("f"), string("z") },
    };
    vector<string> splits { "d", "a", "h", "e", string("h\0", 2) };
    vector<pair<string, string>> partitions {
      { "", "a" },
      { "a", "b" },
      { "c", "d" },
      { "d", "e" },
      { "e", "f" },
      { "f", "h" },
      { "h", string("h\0", 2) },
      { string("h\0", 2), "z" },
    };
    CheckCreateRangePartitions(bounds, splits, partitions);
  }
  {
    // Bounds:
    // ({ }, { })
    //
    // Splits:
    // { col: "m"   }
    //
    // Encoded Partition Keys:
    // [ (""),    ("m")   )
    // [ ("m"),   ("")   )

    vector<pair<optional<string>, optional<string>>> bounds {
      { boost::none, boost::none },
    };
    vector<string> splits { "m" };
    vector<pair<string, string>> partitions {
      { "", "m" },
      { "m", "" },
    };
    CheckCreateRangePartitions(bounds, splits, partitions);
  }
}

TEST(PartitionTest, TestCreateHashBucketPartitions) {
  // CREATE TABLE t (a VARCHAR PRIMARY KEY),
  // PARITITION BY [HASH BUCKET (a)];
  Schema schema({ ColumnSchema("a", STRING) }, { ColumnId(0) }, 1);

  PartitionSchemaPB schema_builder;
  SetRangePartitionComponent(&schema_builder, vector<string>());
  AddHashBucketComponent(&schema_builder, { "a" }, 3, 42);
  PartitionSchema partition_schema;
  ASSERT_OK(PartitionSchema::FromPB(schema_builder, schema, &partition_schema));

  ASSERT_EQ("hash bucket components: [(bucket count: 3, seed: 42, columns: [a])]",
            partition_schema.DebugString(schema));

  // Encoded Partition Keys:
  //
  // [ (_), (1) )
  // [ (1), (2) )
  // [ (3), (_) )

  vector<Partition> partitions;
  ASSERT_OK(partition_schema.CreatePartitions(vector<KuduPartialRow>(), {}, schema, &partitions));
  ASSERT_EQ(3, partitions.size());

  EXPECT_EQ(0, partitions[0].hash_buckets()[0]);
  EXPECT_EQ("", partitions[0].range_key_start());
  EXPECT_EQ("", partitions[0].range_key_end());
  EXPECT_EQ(string("", 0), partitions[0].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1", 4), partitions[0].partition_key_end());
  EXPECT_EQ("hash buckets: (0)",
            partition_schema.PartitionDebugString(partitions[0], schema));

  EXPECT_EQ(1, partitions[1].hash_buckets()[0]);
  EXPECT_EQ("", partitions[1].range_key_start());
  EXPECT_EQ("", partitions[1].range_key_end());
  EXPECT_EQ(string("\0\0\0\1", 4), partitions[1].partition_key_start());
  EXPECT_EQ(string("\0\0\0\2", 4), partitions[1].partition_key_end());
  EXPECT_EQ("hash buckets: (1)",
            partition_schema.PartitionDebugString(partitions[1], schema));

  EXPECT_EQ(2, partitions[2].hash_buckets()[0]);
  EXPECT_EQ("", partitions[2].range_key_start());
  EXPECT_EQ("", partitions[2].range_key_end());
  EXPECT_EQ(string("\0\0\0\2", 4), partitions[2].partition_key_start());
  EXPECT_EQ(string("", 0), partitions[2].partition_key_end());
  EXPECT_EQ("hash buckets: (2)",
            partition_schema.PartitionDebugString(partitions[2], schema));
}

TEST(PartitionTest, TestCreatePartitions) {
  // CREATE TABLE t (a VARCHAR, b VARCHAR, c VARCHAR, PRIMARY KEY (a, b, c))
  // PARITITION BY [HASH BUCKET (a), HASH BUCKET (b), RANGE (a, b, c)];
  Schema schema({ ColumnSchema("a", STRING),
                  ColumnSchema("b", STRING),
                  ColumnSchema("c", STRING) },
                { ColumnId(0), ColumnId(1), ColumnId(2) }, 3);

  PartitionSchemaPB schema_builder;
  AddHashBucketComponent(&schema_builder, { "a" }, 2, 0);
  AddHashBucketComponent(&schema_builder, { "b" }, 2, 0);
  PartitionSchema partition_schema;
  ASSERT_OK(PartitionSchema::FromPB(schema_builder, schema, &partition_schema));

  ASSERT_EQ("hash bucket components: [(bucket count: 2, columns: [a]), "
            "(bucket count: 2, columns: [b])], range columns: [a, b, c]",
            partition_schema.DebugString(schema));

  // Split Rows:
  //
  // { a: "a1", b: "b1", c: "c1" }
  // { b: "a2", b: "b2" }
  //
  // non-specified column values default to the logical minimum value ("").
  //
  // Encoded Partition Keys:
  //
  // [ (_, _,        _), (0, 0, "a1b1c1") )
  // [ (0, 0, "a1b1c1"), (0, 0,   "a2b2") )
  // [ (0, 0,   "a2b2"), (0, 1,        _) )
  //
  // [ (0, 1,        _), (0, 1, "a1b1c1") )
  // [ (0, 1, "a1b1c1"), (0, 1,   "a2b2") )
  // [ (0, 1,   "a2b2"), (1, _,        _) )
  //
  // [ (1, _,        _), (1, 0, "a1b1c1") )
  // [ (1, 0, "a1b1c1"), (1, 0,   "a2b2") )
  // [ (1, 0,   "a2b2"), (1, 1,        _) )
  //
  // [ (1, 1,        _), (1, 1, "a1b1c1") )
  // [ (1, 1, "a1b1c1"), (1, 1,   "a2b2") )
  // [ (1, 1,   "a2b2"), (_, _,        _) )
  //
  // _ signifies that the value is omitted from the encoded partition key.

  KuduPartialRow split_a(&schema);
  ASSERT_OK(split_a.SetStringCopy("a", "a1"));
  ASSERT_OK(split_a.SetStringCopy("b", "b1"));
  ASSERT_OK(split_a.SetStringCopy("c", "c1"));
  string partition_key_a;
  ASSERT_OK(partition_schema.EncodeKey(split_a, &partition_key_a));

  KuduPartialRow split_b(&schema);
  ASSERT_OK(split_b.SetStringCopy("a", "a2"));
  ASSERT_OK(split_b.SetStringCopy("b", "b2"));
  string partition_key_b;
  ASSERT_OK(partition_schema.EncodeKey(split_b, &partition_key_b));

  // Split keys need not be passed in sorted order.
  vector<KuduPartialRow> split_rows = { split_b, split_a };
  vector<Partition> partitions;
  ASSERT_OK(partition_schema.CreatePartitions(split_rows, {}, schema, &partitions));
  ASSERT_EQ(12, partitions.size());

  EXPECT_EQ(0, partitions[0].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[0].hash_buckets()[1]);
  EXPECT_EQ(string("", 0), partitions[0].range_key_start());
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[0].range_key_end());
  EXPECT_EQ(string("", 0), partitions[0].partition_key_start());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\0" "a1\0\0b1\0\0c1", 18), partitions[0].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 0), "
            "range: [(<start>), (string a=a1, string b=b1, string c=c1))",
            partition_schema.PartitionDebugString(partitions[0], schema));

  EXPECT_EQ(0, partitions[1].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[1].hash_buckets()[1]);
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[1].range_key_start());
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[1].range_key_end());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\0" "a1\0\0b1\0\0c1", 18),
            partitions[1].partition_key_start());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\0" "a2\0\0b2\0\0", 16), partitions[1].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 0), "
            "range: [(string a=a1, string b=b1, string c=c1), (string a=a2, string b=b2, <end>))",
            partition_schema.PartitionDebugString(partitions[1], schema));

  EXPECT_EQ(0, partitions[2].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[2].hash_buckets()[1]);
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[2].range_key_start());
  EXPECT_EQ(string("", 0), partitions[2].range_key_end());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\0" "a2\0\0b2\0\0", 16), partitions[2].partition_key_start());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1", 8), partitions[2].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 0), "
            "range: [(string a=a2, string b=b2, <start>), (<end>))",
            partition_schema.PartitionDebugString(partitions[2], schema));

  EXPECT_EQ(0, partitions[3].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[3].hash_buckets()[1]);
  EXPECT_EQ(string("", 0), partitions[3].range_key_start());
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[3].range_key_end());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1", 8), partitions[3].partition_key_start());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1" "a1\0\0b1\0\0c1", 18), partitions[3].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 1), "
            "range: [(<start>), (string a=a1, string b=b1, string c=c1))",
            partition_schema.PartitionDebugString(partitions[3], schema));

  EXPECT_EQ(0, partitions[4].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[4].hash_buckets()[1]);
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[4].range_key_start());
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[4].range_key_end());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1" "a1\0\0b1\0\0c1", 18),
            partitions[4].partition_key_start());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1" "a2\0\0b2\0\0", 16), partitions[4].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 1), "
            "range: [(string a=a1, string b=b1, string c=c1), (string a=a2, string b=b2, <end>))",
            partition_schema.PartitionDebugString(partitions[4], schema));

  EXPECT_EQ(0, partitions[5].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[5].hash_buckets()[1]);
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[5].range_key_start());
  EXPECT_EQ(string("", 0), partitions[5].range_key_end());
  EXPECT_EQ(string("\0\0\0\0" "\0\0\0\1" "a2\0\0b2\0\0", 16), partitions[5].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1", 4), partitions[5].partition_key_end());
  EXPECT_EQ("hash buckets: (0, 1), "
            "range: [(string a=a2, string b=b2, <start>), (<end>))",
            partition_schema.PartitionDebugString(partitions[5], schema));

  EXPECT_EQ(1, partitions[6].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[6].hash_buckets()[1]);
  EXPECT_EQ(string("", 0), partitions[6].range_key_start());
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[6].range_key_end());
  EXPECT_EQ(string("\0\0\0\1", 4), partitions[6].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\0" "a1\0\0b1\0\0c1", 18), partitions[6].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 0), "
            "range: [(<start>), (string a=a1, string b=b1, string c=c1))",
            partition_schema.PartitionDebugString(partitions[6], schema));

  EXPECT_EQ(1, partitions[7].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[7].hash_buckets()[1]);
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[7].range_key_start());
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[7].range_key_end());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\0" "a1\0\0b1\0\0c1", 18),
            partitions[7].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\0" "a2\0\0b2\0\0", 16), partitions[7].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 0), "
            "range: [(string a=a1, string b=b1, string c=c1), (string a=a2, string b=b2, <end>))",
            partition_schema.PartitionDebugString(partitions[7], schema));

  EXPECT_EQ(1, partitions[8].hash_buckets()[0]);
  EXPECT_EQ(0, partitions[8].hash_buckets()[1]);
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[8].range_key_start());
  EXPECT_EQ(string("", 0), partitions[8].range_key_end());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\0" "a2\0\0b2\0\0", 16), partitions[8].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1", 8), partitions[8].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 0), "
            "range: [(string a=a2, string b=b2, <start>), (<end>))",
            partition_schema.PartitionDebugString(partitions[8], schema));

  EXPECT_EQ(1, partitions[9].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[9].hash_buckets()[1]);
  EXPECT_EQ(string("", 0), partitions[9].range_key_start());
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[9].range_key_end());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1", 8), partitions[9].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1" "a1\0\0b1\0\0c1", 18), partitions[9].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 1), "
            "range: [(<start>), (string a=a1, string b=b1, string c=c1))",
            partition_schema.PartitionDebugString(partitions[9], schema));

  EXPECT_EQ(1, partitions[10].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[10].hash_buckets()[1]);
  EXPECT_EQ(string("a1\0\0b1\0\0c1", 10), partitions[10].range_key_start());
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[10].range_key_end());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1" "a1\0\0b1\0\0c1", 18),
            partitions[10].partition_key_start());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1" "a2\0\0b2\0\0", 16), partitions[10].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 1), "
            "range: [(string a=a1, string b=b1, string c=c1), (string a=a2, string b=b2, <end>))",
            partition_schema.PartitionDebugString(partitions[10], schema));

  EXPECT_EQ(1, partitions[11].hash_buckets()[0]);
  EXPECT_EQ(1, partitions[11].hash_buckets()[1]);
  EXPECT_EQ(string("a2\0\0b2\0\0", 8), partitions[11].range_key_start());
  EXPECT_EQ(string("", 0), partitions[11].range_key_end());
  EXPECT_EQ(string("\0\0\0\1" "\0\0\0\1" "a2\0\0b2\0\0", 16), partitions[11].partition_key_start());
  EXPECT_EQ(string("", 0), partitions[11].partition_key_end());
  EXPECT_EQ("hash buckets: (1, 1), "
            "range: [(string a=a2, string b=b2, <start>), (<end>))",
            partition_schema.PartitionDebugString(partitions[11], schema));
}

} // namespace kudu

ext: cc


text:
/*
 * Copyright 2013 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

/*
 * Many thanks to the original author: Kevin Knoedler
 */

// Stubbed library AtlasRobotInterface that talks to Gazebo

// System includes
#include <stdio.h>
#include <math.h>
#include <boost/thread/mutex.hpp>

// From the AtlasRobotInterface SDK
#include <AtlasInterface.h>

// ROS stuff
#include <ros/ros.h>

#include <rosgraph_msgs/Clock.h>
#include <sensor_msgs/JointState.h>
#include <osrf_msgs/JointCommands.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>

#include <atlas_msgs/ControllerStatistics.h>
#include <atlas_msgs/ForceTorqueSensors.h>
#include <atlas_msgs/AtlasState.h>
#include <atlas_msgs/AtlasCommand.h>
#include <atlas_msgs/AtlasSimInterfaceCommand.h>
#include <atlas_msgs/AtlasSimInterfaceState.h>

// KDL, installed via ROS
#include <kdl/frames_io.hpp>

static boost::mutex G_atlasstate_lock;
static atlas_msgs::AtlasState G_atlasstate_msg;
static boost::mutex G_atlassiminterface_lock;
static atlas_msgs::AtlasSimInterfaceState G_atlassiminterface_msg;
static int G_dataAvailable = 0;
static boost::mutex G_dataavailable_lock;

static ros::Publisher pub_atlas_command_;
static ros::Publisher pub_bdi1;
static int G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::STAND;
static AtlasRobotBehavior G_atlas_behavior;

static void atlasstateCallback(const atlas_msgs::AtlasState::ConstPtr &msg)
{
  boost::mutex::scoped_lock lock(G_atlasstate_lock);
  G_atlasstate_msg = *msg;
  G_dataAvailable++;
}

static void atlasSimInterfaceStateCallback(const atlas_msgs::AtlasSimInterfaceState::ConstPtr &msg)
{
  boost::mutex::scoped_lock lock(G_atlassiminterface_lock);
  G_atlassiminterface_msg = *msg;
}

AtlasInterface::AtlasInterface()
{
  // Here I'm assuming that the user never creates more than one of these
  // objects in the same process.  That may be a bad assumption.
  const char* argv = "atlas_interface";
  int argc = 1;
  ros::init(argc, (char**)&argv, "atlas_interface");
  ros::NodeHandle* rosnode = new ros::NodeHandle();

  ros::Time last_ros_time_;
  bool wait = true;
  while (wait)
  {
    last_ros_time_ = ros::Time::now();
    if (last_ros_time_.toSec() > 0)
    {
      wait = false;
    }
  }

  // ros topic subscribtions

  // /atlas/atlas_state
  ros::SubscribeOptions atlasstateSo =
    ros::SubscribeOptions::create<atlas_msgs::AtlasState>(
     "/atlas/atlas_state", 1, atlasstateCallback,
     ros::VoidPtr(), rosnode->getCallbackQueue());
  atlasstateSo.transport_hints = ros::TransportHints().unreliable();
  static ros::Subscriber subAtlasState = rosnode->subscribe(atlasstateSo);

  // AtlasSimInterfaceState
  ros::SubscribeOptions simstateSo =
    ros::SubscribeOptions::create<atlas_msgs::AtlasSimInterfaceState>(
     "/atlas/atlas_sim_interface_state", 1, atlasSimInterfaceStateCallback,
     ros::VoidPtr(), rosnode->getCallbackQueue());
  simstateSo.transport_hints = ros::TransportHints().unreliable();
  static ros::Subscriber subSimState = rosnode->subscribe(simstateSo);

  pub_atlas_command_ =
    rosnode->advertise<atlas_msgs::AtlasCommand>("/atlas/atlas_command", 1);
  pub_bdi1 =
  rosnode->advertise<atlas_msgs::AtlasSimInterfaceCommand>("/atlas/atlas_sim_interface_command", 1);
}

AtlasInterface::~AtlasInterface()
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
}

AtlasErrorCode AtlasInterface::open_net_connection_to_robot(std::string robot_hostname,
              int send_port,
              int recv_port)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::close_net_connection_to_robot()
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

bool AtlasInterface::net_connection_open()
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return true;
}

#if ATLAS_VERSION_MAJOR >= 2 && ATLAS_VERSION_MINOR >= 4
AtlasErrorCode AtlasInterface::get_robot_ip_address(std::string& robot_ip_address)
{
  robot_ip_address = "10.66.171.30";
  return NO_ERRORS;
}
#endif

AtlasErrorCode AtlasInterface::start(AtlasHydraulicPressureSetting desired_pressure,
         int64_t* packet_seq_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::stop(int64_t* packet_seq_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

/*
AtlasErrorCode AtlasInterface::suspend(int64_t* packet_seq_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}
*/
bool AtlasInterface::command_in_progress()
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return false;
}

AtlasErrorCode AtlasInterface::set_desired_behavior(AtlasRobotBehavior behavior,
            int64_t* packet_seq_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;

  G_atlas_behavior = behavior;
  if (behavior == BEHAVIOR_NONE)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::FREEZE; // FREEZE
  }
  else if (behavior == BEHAVIOR_FREEZE)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::FREEZE;
  }
  else if (behavior == BEHAVIOR_STAND_PREP)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::STAND_PREP;
  }
  else if (behavior == BEHAVIOR_STAND)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::STAND;
  }
  else if (behavior == BEHAVIOR_WALK)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::WALK;
  }
  else if (behavior == BEHAVIOR_STEP)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::STEP;
  }
  else if (behavior == BEHAVIOR_MANIPULATE)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::MANIPULATE;
  }
  else if (behavior == BEHAVIOR_USER)
  {
    G_sim_behavior = atlas_msgs::AtlasSimInterfaceCommand::USER;
  }

  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::get_desired_behavior(AtlasRobotBehavior& desired_behavior)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

static void initSimInterfaceCommand(atlas_msgs::AtlasSimInterfaceCommand *msg)
{
  msg->k_effort.resize(Atlas::NUM_JOINTS);
  for (int i = 0; i < Atlas::NUM_JOINTS ; i++)
  {
    msg->k_effort[i] = 0;
  }
}

// Correctly sizes the arrays in an AtlasCommand
static void resizeAtlasCommand(atlas_msgs::AtlasCommand *ac)
{
  const static unsigned int n = Atlas::NUM_JOINTS;
  ac->position.resize(n);
  ac->velocity.resize(n);
  ac->effort.resize(n);
  ac->kp_position.resize(n);
  ac->ki_position.resize(n);
  ac->kd_position.resize(n);
  ac->kp_velocity.resize(n);
  ac->i_effort_min.resize(n);
  ac->i_effort_max.resize(n);
  ac->k_effort.resize(n);
}

static void zeroAtlasCommand(atlas_msgs::AtlasCommand *ac)
{
  for (int i = 0; i<Atlas::NUM_JOINTS; i++)
  {
    ac->position[i] = 0.0;
    ac->velocity[i] = 0.0;
    ac->effort[i] = 0.0;
    ac->kp_position[i] = 0.0;
    ac->ki_position[i] = 0.0;
    ac->kd_position[i] = 0.0;
    ac->kp_velocity[i] = 0.0;
    ac->i_effort_min[i] = 0.0;
    ac->i_effort_max[i] = 0.0;
    ac->k_effort[i] = 0;
  }
}

AtlasErrorCode AtlasInterface::send_control_data_to_robot(AtlasControlDataToRobot& data_to_robot,
            int64_t* packet_seq_id)
{
  //std::cout << __PRETTY_FUNCTION__ << std::endl;

  atlas_msgs::AtlasSimInterfaceCommand asic;
  atlas_msgs::AtlasCommand ac;
  initSimInterfaceCommand(&asic);
  resizeAtlasCommand(&ac);
  zeroAtlasCommand(&ac);

  for (int i = 0; i < Atlas::NUM_JOINTS; i++)
  {
    ac.position[i] = data_to_robot.j[i].q_d;
    ac.velocity[i] = data_to_robot.j[i].qd_d;
    ac.effort[i] = data_to_robot.j[i].f_d;

    ac.kp_position[i] = data_to_robot.jparams[i].k_q_p;
    ac.ki_position[i] = data_to_robot.jparams[i].k_q_i;
    ac.kd_position[i] = data_to_robot.jparams[i].k_qd_p;
    ac.kp_velocity[i] = 0;
    ac.k_effort[i] = 0;
    ac.i_effort_min[i] =  0.0;
    ac.i_effort_max[i] =  0.0;
    if (fabs(ac.ki_position[i] > 0.01))
    {
      ac.i_effort_min[i] = -10000;
      ac.i_effort_max[i] =  10000;
    }
    asic.k_effort[i] = 0;
  }

  // k_effort is gone from the real robot
  // set it to match the real robot behavior
  if (G_atlas_behavior == BEHAVIOR_MANIPULATE)
  {
    for (int i = 0; i < 4; i++)
    {
      ac.k_effort[i] = 255;
      asic.k_effort[i] = 255;
    }
    for (int i = 16; i < Atlas::NUM_JOINTS; i++)
    {
      ac.k_effort[i] = 255;
      asic.k_effort[i] = 255;
    }
  }
  else if (G_atlas_behavior == BEHAVIOR_USER)
  {
    for (int i = 0; i < Atlas::NUM_JOINTS; i++)
    {
      ac.k_effort[i] = 255;
      asic.k_effort[i] = 255;
    }
  }

  double roll, pitch, yaw, x, y, z, w;
  KDL::Rotation r1;
  asic.behavior = G_sim_behavior;

  asic.walk_params.use_demo_walk = data_to_robot.walk_params.use_demo_walk;

  for (int i = 0; i < 4; i++)
  {
    asic.walk_params.step_queue[i].step_index = data_to_robot.walk_params.step_queue[i].step_index;
    asic.walk_params.step_queue[i].foot_index = data_to_robot.walk_params.step_queue[i].foot_index;
    asic.walk_params.step_queue[i].duration   = data_to_robot.walk_params.step_queue[i].duration;
    asic.walk_params.step_queue[i].swing_height = data_to_robot.walk_params.step_queue[i].swing_height;
    asic.walk_params.step_queue[i].pose.position.x = data_to_robot.walk_params.step_queue[i].position.x();
    asic.walk_params.step_queue[i].pose.position.y = data_to_robot.walk_params.step_queue[i].position.y();
    asic.walk_params.step_queue[i].pose.position.z = data_to_robot.walk_params.step_queue[i].position.z();
    roll = asin(data_to_robot.walk_params.step_queue[i].normal.y());
    pitch = asin(data_to_robot.walk_params.step_queue[i].normal.x());
    yaw = data_to_robot.walk_params.step_queue[i].yaw;
    r1 = r1.RPY(roll, pitch, yaw);
    r1.GetQuaternion(x,y,z,w);
    asic.walk_params.step_queue[i].pose.orientation.x = x;
    asic.walk_params.step_queue[i].pose.orientation.y = y;
    asic.walk_params.step_queue[i].pose.orientation.z = z;
    asic.walk_params.step_queue[i].pose.orientation.w = w;
  }

  asic.step_params.use_demo_walk = data_to_robot.step_params.use_demo_walk;
  asic.step_params.desired_step.duration = data_to_robot.step_params.desired_step.duration;
  asic.step_params.desired_step.foot_index = data_to_robot.step_params.desired_step.foot_index;
  asic.step_params.desired_step.step_index = 1;
  asic.step_params.desired_step.swing_height = 0.3;
  asic.step_params.desired_step.pose.position.x = data_to_robot.step_params.desired_step.position.x();
  asic.step_params.desired_step.pose.position.y = data_to_robot.step_params.desired_step.position.y();
  asic.step_params.desired_step.pose.position.z = data_to_robot.step_params.desired_step.position.z();

  // data_to_robot.step_params.desired_step.normal.z(); // z is redundant
  roll = asin(data_to_robot.step_params.desired_step.normal.y());
  pitch = asin(data_to_robot.step_params.desired_step.normal.x());
  yaw = data_to_robot.step_params.desired_step.yaw;
  r1 = r1.RPY(roll, pitch, yaw);
  r1.GetQuaternion(x,y,z,w);
  asic.step_params.desired_step.pose.orientation.x = x;
  asic.step_params.desired_step.pose.orientation.y = y;
  asic.step_params.desired_step.pose.orientation.z = z;
  asic.step_params.desired_step.pose.orientation.w = w;

  //asic.stand_params.

  asic.manipulate_params.use_demo_mode = data_to_robot.manipulate_params.use_demo_mode;
  asic.manipulate_params.use_desired   = data_to_robot.manipulate_params.use_desired;
  asic.manipulate_params.desired.pelvis_height = data_to_robot.manipulate_params.desired.pelvis_height;
  asic.manipulate_params.desired.pelvis_lat    = data_to_robot.manipulate_params.desired.pelvis_lat;
  asic.manipulate_params.desired.pelvis_yaw    = data_to_robot.manipulate_params.desired.pelvis_yaw;

  pub_atlas_command_.publish(ac);
  pub_bdi1.publish(asic);

  ros::spinOnce();

  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::new_control_data_from_robot_available(double timeout,
                 bool* new_data_available)
{
  ros::spinOnce();

  *new_data_available = false;
  // sim sends data every 1mS, Atlas interface is every 3mS
  if (G_dataAvailable > 2)
  {
    *new_data_available = true;
  }

  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::get_control_data_from_robot(AtlasControlDataFromRobot* data_from_robot)
{
  // Waits until data is available
  while (G_dataAvailable < 3)
  {
    ros::spinOnce();
    //printf("waiting %d\n", G_dataAvailable);
    usleep(1000);
  }

  {
    boost::mutex::scoped_lock lock(G_atlasstate_lock);
    G_dataAvailable = 0;

    data_from_robot->air_sump_pressure = 77.7;

    data_from_robot->current_behavior = G_atlas_behavior;
    data_from_robot->filtered_imu.orientation_estimate.m_qx = G_atlasstate_msg.orientation.x;
    data_from_robot->filtered_imu.orientation_estimate.m_qy = G_atlasstate_msg.orientation.y;
    data_from_robot->filtered_imu.orientation_estimate.m_qz = G_atlasstate_msg.orientation.z;
    data_from_robot->filtered_imu.orientation_estimate.m_qw = G_atlasstate_msg.orientation.w;
    data_from_robot->filtered_imu.angular_velocity.set_x(G_atlasstate_msg.angular_velocity.x);
    data_from_robot->filtered_imu.angular_velocity.set_y(G_atlasstate_msg.angular_velocity.y);
    data_from_robot->filtered_imu.angular_velocity.set_z(G_atlasstate_msg.angular_velocity.z);
    data_from_robot->filtered_imu.linear_acceleration.set_x(G_atlasstate_msg.linear_acceleration.x);
    data_from_robot->filtered_imu.linear_acceleration.set_y(G_atlasstate_msg.linear_acceleration.y);
    data_from_robot->filtered_imu.linear_acceleration.set_z(G_atlasstate_msg.linear_acceleration.z);
    //data_from_robot->filtered_imu.imu_timestamp = ;

    data_from_robot->foot_sensors[0].fz = G_atlasstate_msg.l_foot.force.z;
    data_from_robot->foot_sensors[0].mx = G_atlasstate_msg.l_foot.torque.x;
    data_from_robot->foot_sensors[0].my = G_atlasstate_msg.l_foot.torque.y;
    data_from_robot->foot_sensors[1].fz = G_atlasstate_msg.r_foot.force.z;
    data_from_robot->foot_sensors[1].mx = G_atlasstate_msg.r_foot.torque.x;
    data_from_robot->foot_sensors[1].my = G_atlasstate_msg.r_foot.torque.y;

    for (int i = 0; i < Atlas::NUM_JOINTS; i++)
    {
      data_from_robot->j[i].f = G_atlasstate_msg.effort[i];
      data_from_robot->j[i].q = G_atlasstate_msg.position[i];
      data_from_robot->j[i].qd = G_atlasstate_msg.velocity[i];
    }

    //data_from_robot->processed_to_robot_packet_seq_id
    //data_from_robot->pump_inlet_pressure
    //data_from_robot->pump_pressure
    //data_from_robot->pump_return_pressure;
    //data_from_robot->pump_supply_pressure
    //data_from_robot->raw_imu
    //data_from_robot->robot_status_flags
    //data_from_robot->run_state;
    //data_from_robot->sensor_head_pps_timestamp;
    //data_from_robot->seq_id;
    //data_from_robot->stand_feedback;
    //data_from_robot->timestamp;
    data_from_robot->wrist_sensors[0].f.set_x(G_atlasstate_msg.l_hand.force.x);
    data_from_robot->wrist_sensors[0].m.set_x(G_atlasstate_msg.l_hand.torque.x);
    data_from_robot->wrist_sensors[0].f.set_y(G_atlasstate_msg.l_hand.force.y);
    data_from_robot->wrist_sensors[0].m.set_y(G_atlasstate_msg.l_hand.torque.y);
    data_from_robot->wrist_sensors[0].f.set_z(G_atlasstate_msg.l_hand.force.z);
    data_from_robot->wrist_sensors[0].m.set_z(G_atlasstate_msg.l_hand.torque.z);
    data_from_robot->wrist_sensors[1].f.set_x(G_atlasstate_msg.r_hand.force.x);
    data_from_robot->wrist_sensors[1].m.set_x(G_atlasstate_msg.r_hand.torque.x);
    data_from_robot->wrist_sensors[1].f.set_y(G_atlasstate_msg.r_hand.force.y);
    data_from_robot->wrist_sensors[1].m.set_y(G_atlasstate_msg.r_hand.torque.y);
    data_from_robot->wrist_sensors[1].f.set_z(G_atlasstate_msg.r_hand.force.z);
    data_from_robot->wrist_sensors[1].m.set_z(G_atlasstate_msg.r_hand.torque.z);
  }

  {
    boost::mutex::scoped_lock lock(G_atlassiminterface_lock);

    data_from_robot->behavior_feedback.status_flags = G_atlassiminterface_msg.behavior_feedback.status_flags;
    data_from_robot->behavior_feedback.trans_from_behavior_index = G_atlassiminterface_msg.behavior_feedback.trans_from_behavior_index;
    data_from_robot->behavior_feedback.trans_to_behavior_index = G_atlassiminterface_msg.behavior_feedback.trans_to_behavior_index;

    // Note - in the simulator a quaternion is provided for the foot pose, not just x/y/z
    data_from_robot->foot_pos_est[0].set_x(G_atlassiminterface_msg.foot_pos_est[0].position.x);
    data_from_robot->foot_pos_est[0].set_y(G_atlassiminterface_msg.foot_pos_est[0].position.y);
    data_from_robot->foot_pos_est[0].set_z(G_atlassiminterface_msg.foot_pos_est[0].position.z);
    data_from_robot->foot_pos_est[1].set_x(G_atlassiminterface_msg.foot_pos_est[1].position.x);
    data_from_robot->foot_pos_est[1].set_y(G_atlassiminterface_msg.foot_pos_est[1].position.y);
    data_from_robot->foot_pos_est[1].set_z(G_atlassiminterface_msg.foot_pos_est[1].position.z);

    data_from_robot->step_feedback.current_step_index     = G_atlassiminterface_msg.step_feedback.current_step_index;
    data_from_robot->step_feedback.next_step_index_needed = G_atlassiminterface_msg.step_feedback.next_step_index_needed;
    data_from_robot->step_feedback.status_flags           = G_atlassiminterface_msg.step_feedback.status_flags;
    data_from_robot->step_feedback.t_step_rem             = G_atlassiminterface_msg.step_feedback.t_step_rem;
    data_from_robot->step_feedback.desired_step_saturated.duration = G_atlassiminterface_msg.step_feedback.desired_step_saturated.duration;
    data_from_robot->step_feedback.desired_step_saturated.foot_index = G_atlassiminterface_msg.step_feedback.desired_step_saturated.foot_index;
    data_from_robot->step_feedback.desired_step_saturated.position.set_x(G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.position.x);
    data_from_robot->step_feedback.desired_step_saturated.position.set_y(G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.position.y);
    data_from_robot->step_feedback.desired_step_saturated.position.set_z(G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.position.z);
    data_from_robot->step_feedback.desired_step_saturated.step_index = G_atlassiminterface_msg.step_feedback.desired_step_saturated.step_index;
    data_from_robot->step_feedback.desired_step_saturated.swing_height = G_atlassiminterface_msg.step_feedback.desired_step_saturated.swing_height;

    KDL::Rotation r2;
    double yaw,pitch,roll, normx, normy, normz;
    r2 = r2.Quaternion(G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.orientation.x,
             G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.orientation.y,
             G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.orientation.z,
             G_atlassiminterface_msg.step_feedback.desired_step_saturated.pose.orientation.w);
    r2.GetRPY(roll, pitch, yaw);
    // fixme - test that these are correct
    normx = sin(pitch);
    normy = sin(roll);
    normz = sqrt(1.0 - (normx*normx + normy*normy)) ;
    data_from_robot->step_feedback.desired_step_saturated.normal.set_x(normx);
    data_from_robot->step_feedback.desired_step_saturated.normal.set_y(normy);
    data_from_robot->step_feedback.desired_step_saturated.normal.set_z(normz);
    data_from_robot->step_feedback.desired_step_saturated.yaw = yaw;

    data_from_robot->pos_est.position.set_x(G_atlassiminterface_msg.pos_est.position.x);
    data_from_robot->pos_est.velocity.set_x(G_atlassiminterface_msg.pos_est.velocity.x);
    data_from_robot->pos_est.position.set_y(G_atlassiminterface_msg.pos_est.position.y);
    data_from_robot->pos_est.velocity.set_y(G_atlassiminterface_msg.pos_est.velocity.y);
    data_from_robot->pos_est.position.set_z(G_atlassiminterface_msg.pos_est.position.z);
    data_from_robot->pos_est.velocity.set_z(G_atlassiminterface_msg.pos_est.velocity.z);

    data_from_robot->walk_feedback.current_step_index = G_atlassiminterface_msg.walk_feedback.current_step_index;
    data_from_robot->walk_feedback.next_step_index_needed = G_atlassiminterface_msg.walk_feedback.next_step_index_needed;
    data_from_robot->walk_feedback.status_flags = G_atlassiminterface_msg.walk_feedback.status_flags;
    data_from_robot->walk_feedback.t_step_rem = G_atlassiminterface_msg.walk_feedback.t_step_rem;
    for (int i = 0; i < 4 ; i++)
    {
      data_from_robot->walk_feedback.step_queue_saturated[i].duration = G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].duration;
      data_from_robot->walk_feedback.step_queue_saturated[i].foot_index = G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].foot_index;
      data_from_robot->walk_feedback.step_queue_saturated[i].position.set_x(G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.position.x);
      data_from_robot->walk_feedback.step_queue_saturated[i].position.set_y(G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.position.y);
      data_from_robot->walk_feedback.step_queue_saturated[i].position.set_z(G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.position.z);
      data_from_robot->walk_feedback.step_queue_saturated[i].step_index = G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].step_index;
      data_from_robot->walk_feedback.step_queue_saturated[i].swing_height = G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].swing_height;

      r2 = r2.Quaternion(G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.orientation.x,
           G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.orientation.y,
           G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.orientation.z,
           G_atlassiminterface_msg.walk_feedback.step_queue_saturated[i].pose.orientation.w);
      r2.GetRPY(roll, pitch, yaw);
      // fixme - test that these are correct
      normx = sin(pitch);
      normy = sin(roll);
      normz = sqrt(1.0 - (normx*normx + normy*normy)) ;
      data_from_robot->walk_feedback.step_queue_saturated[i].normal.set_x(normx);
      data_from_robot->walk_feedback.step_queue_saturated[i].normal.set_y(normy);
      data_from_robot->walk_feedback.step_queue_saturated[i].normal.set_z(normz);
      data_from_robot->walk_feedback.step_queue_saturated[i].yaw = yaw;
    }

    data_from_robot->manipulate_feedback.clamped.pelvis_height = G_atlassiminterface_msg.manipulate_feedback.clamped.pelvis_height;
    data_from_robot->manipulate_feedback.clamped.pelvis_lat =  G_atlassiminterface_msg.manipulate_feedback.clamped.pelvis_lat;
    data_from_robot->manipulate_feedback.clamped.pelvis_yaw =  G_atlassiminterface_msg.manipulate_feedback.clamped.pelvis_yaw;
    data_from_robot->manipulate_feedback.status_flags =  G_atlassiminterface_msg.manipulate_feedback.status_flags;
  }

  return NO_ERRORS;
}

AtlasErrorCode AtlasInterface::clear_faults(int64_t* packet_seq_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return NO_ERRORS;
}

#if ATLAS_VERSION_MAJOR >= 2 && ATLAS_VERSION_MINOR >= 4
AtlasErrorCode AtlasInterface::download_robot_log_file(std::string dest_directory, float duration)
{
  return NO_ERRORS;
}
#endif

std::string AtlasInterface::get_error_code_text(AtlasErrorCode ec)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

std::string AtlasInterface::get_run_state_as_string(AtlasRobotRunState run_state)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

std::string AtlasInterface::get_status_flag_as_string(AtlasRobotStatus robot_status_flag)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

std::string AtlasInterface::link_name_from_link_id(AtlasLinkId link_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

AtlasLinkId AtlasInterface::link_id_from_link_name(std::string link_name)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return LINK_PELVIS;
}

std::string AtlasInterface::joint_name_from_joint_id(AtlasJointId joint_id)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

AtlasJointId AtlasInterface::joint_id_from_joint_name(std::string joint_name)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return JOINT_BACK_BKZ;
}

std::string AtlasInterface::behavior_name_from_behavior(AtlasRobotBehavior behavior)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return("hello");
}

AtlasRobotBehavior AtlasInterface::behavior_from_behavior_name(std::string behavior_name)
{
  std::cout << __PRETTY_FUNCTION__ << std::endl;
  return BEHAVIOR_FREEZE;
}


text:
#ifdef SHASTA_HTTP_SERVER

#ifndef SHASTA_LOCAL_MARKER_GRAPH_HPP
#define SHASTA_LOCAL_MARKER_GRAPH_HPP

/*******************************************************************************

The local marker graph created by class LocalMarkerGraph is a subgraph
of the global marker graph, created by starting at a given vertex,
and extending out to a specified distance in both directions.
Distance is number of edges on the global marker graph.

Like in the global marker graph, each vertex corresponds to
a group of aligned markers.

*******************************************************************************/

// Shasta.
#include "AssemblyGraph.hpp"
#include "Kmer.hpp"
#include "MarkerGraph.hpp"

// Boost libraries.
#include <boost/graph/adjacency_list.hpp>

namespace shasta {

    class LocalMarkerGraphVertex;
    class LocalMarkerGraphEdge;
    class LocalMarkerGraph;
    using LocalMarkerGraphBaseClass = boost::adjacency_list<
        boost::setS,
        boost::listS,
        boost::bidirectionalS,
        LocalMarkerGraphVertex,
        LocalMarkerGraphEdge
        >;

    class CompressedMarker;
    class ConsensusCaller;
    class LongBaseSequences;
}

class shasta::LocalMarkerGraphVertex {
public:

    // The global vertex id of the vertex of the global marker
    // graph that corresponds to this vertex.
    MarkerGraph::VertexId vertexId;

    // The distance from the start vertex.
    int distance;

    // The markers of this vertex.
    class MarkerInfo {
    public:
        MarkerId markerId;
        OrientedReadId orientedReadId;
        uint32_t ordinal;
    };
    vector<MarkerInfo> markerInfos;

    LocalMarkerGraphVertex(
        MarkerGraph::VertexId vertexId,
        int distance) :
        vertexId(vertexId),
        distance(distance)
        {}

    // Look for the ordinal for a given oriented read id.
    // If found, returns pair(true, ordinal).
    // Otherwise, returns pair(false, don't care).
    // If more than an ordinal is found, the first one is returned.
    pair<bool, uint32_t> getOrdinal(OrientedReadId) const;

    // Coverage information at each of the k positions.
    // All reads agree on the bases, which are the marker bases,
    // but the repeat counts can be different.
    vector<Coverage> coverages;

    vector<uint8_t> storedConsensusRepeatCounts;

    // Fields used by approximateTopologicalSort.
    uint32_t color = 0;
    size_t rank = 0;
};

class shasta::LocalMarkerGraphEdge {
public:

    // Class to describe the intervening sequence between
    // the two markers that define the edge.
    class Sequence {
    public:
        // The number of overlapping bases between the
        // marker of the source vertex and the
        // marker of the target vertex.
        uint8_t overlappingBaseCount;

        // The intervening sequence between the two markers.
        vector<Base> sequence;

        // There are three possible cases:
        // overlappingBaseCount>0 && sequence.empty():
        //     The sequence of the two markers overlap by
        //     overlappingBaseCount bases.
        // overlappingBaseCount==0 && !sequence.empty():
        //     Sequence stores the base sequence between the two markers.
        // overlappingBaseCount==0 && sequence.empty():
        //     The two markers immediately follow each other,
        //     without any intervening sequence.

        bool operator<(const Sequence& that) const
        {
            return tie(overlappingBaseCount, sequence) <
                tie(that.overlappingBaseCount, that.sequence);
        }
    };

    // The oriented vertices of this edge, grouped by sequence.
    // Sorted by decreasing number of supporting reads.
    vector< pair<Sequence, vector<MarkerIntervalWithRepeatCounts> > > infos;

    // Coverage is the total number of reads supporting this edge.
    size_t coverage() const
    {
        size_t c = 0;
        for(const auto& p: infos) {
            c += p.second.size();
        }
        return c;
    }

    // Look for the ordinals for a given oriented read id.
    // If found, returns true.
    // If more than an ordinal pairs is found, the first one is returned.
    bool getOrdinals(OrientedReadId, array<uint32_t, 2>& ordinals) const;

    // Id of the global edge corresponding to this edge.
    // Only filled in when the graph is created using stored connectivity.
    MarkerGraph::EdgeId edgeId = MarkerGraph::invalidEdgeId;

    // The locations of this edge in the assembly graph.
    // Each location is a pair(AssemblyGraph::EdgeId, position),
    // where position is the index of this marker graph edge in the assembly graph edge.
    // Note that, before detangling, or if detangling is not used,
    // each marker graph edge corresponds to at most one location
    // in the assembly graph. However, after detangling a marker
    // graph edge can correspond to multiple locations in the
    // assembly graph.
    vector< pair<AssemblyGraph::EdgeId, uint32_t> > assemblyGraphLocations;

    // Flag that is set if the edge was removed during
    // approximate transitive reduction by flagWeakMarkerGraphEdges.
    uint8_t wasRemovedByTransitiveReduction = 0;

    // Set if this edge was removed during pruning.
    uint8_t wasPruned = 0;

    // Set if this edge belongs to a bubble/superbubble that was removed.
    uint8_t isSuperBubbleEdge = 0;

    // Flag that is set if this edge corresponds to a global marker graph
    // edge that was assembled.
    uint8_t wasAssembled = 0;

    // Field used by approximateTopologicalSort.
    bool isDagEdge = true;

    // Consensus sequence for this edge.
    vector<Base> consensusSequence;
    vector<uint32_t> consensusRepeatCounts;
    uint8_t consensusOverlappingBaseCount = 0;

};

class shasta::LocalMarkerGraph :
    public LocalMarkerGraphBaseClass {
public:

    LocalMarkerGraph(
        uint32_t k,
        LongBaseSequences& reads,
        const MemoryMapped::VectorOfVectors<uint8_t, uint64_t>& readRepeatCounts,
        const MemoryMapped::VectorOfVectors<CompressedMarker, uint64_t>& markers,
        const MemoryMapped::Vector<MarkerGraph::CompressedVertexId>& globalMarkerGraphVertex,
        const ConsensusCaller&
        );

    // Override base class Base defined in Boost Graph library.
    // Use shasta::Base instead.
    using Base = shasta::Base;

    // Find out if a vertex with the given MarkerGraph::VertexId exists.
    // If it exists, return make_pair(true, v).
    // Otherwise, return make_pair(false, null_vertex());
    pair<bool, vertex_descriptor> findVertex(MarkerGraph::VertexId) const;

    // Add a vertex with the given MarkerGraph::VertexId
    // and return its vertex descriptor.
    // A vertex with this MarkerGraph::VertexId must not exist.
    vertex_descriptor addVertex(
        MarkerGraph::VertexId,
        int distance,
        span<MarkerId> markers);

    // Get the KmerId for a vertex.
    KmerId getKmerId(vertex_descriptor) const;

    // Get the repeat counts for a MarkerInfo of a vertex.
    vector<uint8_t> getRepeatCounts(const LocalMarkerGraphVertex::MarkerInfo&) const;

    // Fill in the ConsensusInfo's for each vertex.
    void computeVertexConsensusInfo();
    void computeVertexConsensusInfo(vertex_descriptor);

    // Store sequence information in the edge.
    // Takes as input a vector of the
    // LocalMarkerGraphEdge::Info that caused the edge to be created.
    void storeEdgeInfo(edge_descriptor, const vector<MarkerInterval>&);

    // Write in Graphviz format.
    void write(
        ostream&,
        int maxDistance,
        bool addLabels,
        bool useDotLayout,
        double vertexScalingFactor,
        double arrowScalingFactor) const;
    void write(
        const string& fileName,
        int maxDistance,
        bool addLabels,
        bool useDotLayout,
        double vertexScalingFactor,
        double arrowScalingFactor) const;

    // Approximate topological sort, adding edges
    // in order of decreasing coverage. The topological sort rank
    // of each vertex is stored in LocalMarkerGrapg2Vertex::rank.
    // In addition, the vertices are stored in topological sort order
    // in vector topologicallySortedVertices.
    void approximateTopologicalSort();
    vector<vertex_descriptor> topologicallySortedVertices;

private:

    // Map a global vertex id to a vertex descriptor for the local graph.
    std::map<MarkerGraph::VertexId, vertex_descriptor> vertexMap;

    // The length of k-mers used as markers.
    uint32_t k;

    // Reference to the global data structure containing all reads and markers
    // (not just those in this local marker graph).
    LongBaseSequences& reads;
    const MemoryMapped::VectorOfVectors<uint8_t, uint64_t>& readRepeatCounts;
    const MemoryMapped::VectorOfVectors<CompressedMarker, uint64_t>& markers;

    // A reference to the vector containing the global marker graph vertex id
    // corresponding to each marker.
    // Indexed by MarkerId.
    const MemoryMapped::Vector<MarkerGraph::CompressedVertexId>& globalMarkerGraphVertex;

    // Object used to compute consensus bases and repeat counts.
    // This is owned by the caller (the Assembler object).
    const ConsensusCaller& consensusCaller;

    // Class used for graphviz output.
    class Writer {
    public:
        Writer(
            const LocalMarkerGraph&,
            int maxDistance,
            bool addLabels,
            bool useDotLayout,
            double vertexScalingFactor,
            double arrowScalingFactor);
            void operator()(ostream&) const;
        void operator()(ostream&, vertex_descriptor) const;
        void operator()(ostream&, edge_descriptor) const;
        const LocalMarkerGraph& graph;
        int maxDistance;
        bool addLabels;
        bool useDotLayout;
        double vertexScalingFactor;
        double arrowScalingFactor;

        // Vertex and edge colors.
        static const string vertexColorZeroDistance;
        static const string vertexColorIntermediateDistance;
        static const string vertexColorMaxDistance;
        static const string edgeArrowColorRemovedDuringTransitiveReduction;
        static const string edgeArrowColorRemovedDuringPruning;
        static const string edgeArrowColorRemovedDuringSuperBubbleRemoval;
        static const string edgeArrowColorNotRemovedNotAssembled;
        static const string edgeArrowColorNotRemovedAssembled;
        static const string edgeLabelColorRemovedDuringTransitiveReduction;
        static const string edgeLabelColorRemovedDuringPruning;
        static const string edgeLabelColorRemovedDuringSuperBubbleRemoval;
        static const string edgeLabelColorNotRemovedNotAssembled;
        static const string edgeLabelColorNotRemovedAssembled;
        const string& vertexColor(const LocalMarkerGraphVertex&) const;
        const string& edgeArrowColor(const LocalMarkerGraphEdge&) const;
        const string& edgeLabelColor(const LocalMarkerGraphEdge&) const;
    };
    friend class Writer;

public:
    static void writeColorLegend(ostream&);
};

#endif

#endif

ext: hpp

max_stars_repo_name: yatisht/shasta

max_issues_repo_name: yatisht/shasta

max_forks_repo_name: yatisht/shasta


text:
/*
 * visualizer.h
 *
 *  Created on: 20 may. 2017
 *      Author: Adrian Arroyo adr.arroyo.perez@gmail.com
 */

#ifndef INCLUDE_VISUA_H_
#define INCLUDE_VISUA_H_

#include <iostream>

#include </home/adrian/pcl/io/include/pcl/io/pcd_io.h>
#include </home/adrian/pcl/io/include/pcl/io/ply_io.h>
#include </home/adrian/pcl/common/include/pcl/point_cloud.h>
#include </home/adrian/pcl/common/include/pcl/console/parse.h>
#include </home/adrian/pcl/common/include/pcl/common/transforms.h>
#include </home/adrian/pcl/visualization/include/pcl/visualization/pcl_visualizer.h>
#include </home/adrian/pcl/visualization/include/pcl/visualization/point_cloud_color_handlers.h>
#include <boost/thread/thread.hpp>
#include </home/adrian/pcl/common/include/pcl/common/common_headers.h>
#include </home/adrian/pcl/features/include/pcl/features/normal_3d.h>
#include </home/adrian/pcl/common/include/pcl/range_image/range_image.h>
#include </home/adrian/pcl/features/include/pcl/features/range_image_border_extractor.h>
#include </home/adrian/pcl/keypoints/include/pcl/keypoints/narf_keypoint.h>

namespace pcl {
namespace PointCloudComparator {
}
}

#endif /* INCLUDE_VISUA_H_ */

max_stars_repo_name: adr-arroyo/PointCloudComparator

max_issues_repo_name: adr-arroyo/PointCloudComparator

max_forks_repo_name: adr-arroyo/PointCloudComparator


text:
#ifndef RAI_CHOLINV_HPP
#define RAI_CHOLINV_HPP

#include <Eigen/Core>
#include <Eigen/Dense>

namespace rai {

template<typename Derived, int n_>
static inline void cholInv(Eigen::Matrix<Derived, n_, n_> &A, Eigen::Matrix<Derived, n_, n_> &AInv) {
  unsigned short int i, j, k, n = (unsigned short int) (n_);
  Derived sum;
  Derived p[n];
  memcpy(AInv.data(), A.data(), n * n * sizeof(Derived));

  p[0] = std::sqrt(AInv(0, 0));

  for (j = 1; j < n; j++)
    AInv(j, 0) = AInv(0, j) / p[0];

  for (i = 1; i < n; i++) {
    sum = AInv(i, i);
    for (k = i - 1; k >= 1; k--)
      sum -= AInv(i, k) * AInv(i, k);
    sum -= AInv(i, 0) * AInv(i, 0);
    p[i] = std::sqrt(sum);
    for (j = i + 1; j < n; j++) {
      sum = AInv(i, j);
      for (k = i - 1; k >= 1; k--)
        sum -= AInv(i, k) * AInv(j, k);
      sum -= AInv(i, 0) * AInv(j, 0);
      AInv(j, i) = sum / p[i];
    }
  }

  for (i = 0; i < n; i++) {
    AInv(i, i) = 1.0 / p[i];
    for (j = i + 1; j < n; j++) {
      sum = 0.0;
      for (k = i; k < j; k++) {
        sum -= AInv(j, k) * AInv(k, i);
      }
      AInv(j, i) = sum / p[j];
    }
  }

  for (i = 0; i < n; i++) {
    AInv(i, i) *= AInv(i, i);
    for (k = i + 1; k < n; k++)
      AInv(i, i) += AInv(k, i) * AInv(k, i);

    for (j = i + 1; j < n; j++) {
      AInv(i, j) = AInv(j, i) * AInv(j, j);
      for (k = j + 1; k < n; k++)
        AInv(i, j) += AInv(k, i) * AInv(k, j);
    }
  }

  for (i = 1; i < n; i++)
    for (j = 0; j < i; j++)
      AInv(i, j) = AInv(j, i);

}
}

#endif //RAI_CHOLINV_HPP

max_stars_repo_name: Wistral/raicommon

max_issues_repo_name: Wistral/raicommon

max_forks_repo_name: Wistral/raicommon


text:
#pragma once

#include <chrono>
#include <mutex>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index_container.hpp>
#include <rai/common/numbers.hpp>
#include <rai/common/blocks.hpp>

namespace rai
{
class BlockWaitingEntry
{
public:
    BlockWaitingEntry(const rai::Account&, uint64_t,
                      const std::shared_ptr<rai::Block>&, bool);

    rai::AccountHeight for_;
    rai::BlockHash hash_;
    std::chrono::steady_clock::time_point arrival_;
    std::shared_ptr<rai::Block> block_;
    bool confirmed_;
};

class BlockWaitingByArrival
{
};

class BlockWaitingByHash
{
};

class BlockWaiting
{
public:
    void Insert(const rai::Account&, uint64_t,
                const std::shared_ptr<rai::Block>&, bool);
    bool Exists(const rai::Account&, uint64_t) const;
    void Age(uint64_t);
    std::vector<rai::BlockWaitingEntry> Remove(const rai::Account&, uint64_t);
    std::vector<rai::BlockWaitingEntry> List() const;
    size_t Size() const;

private:
    mutable std::mutex mutex_;
    boost::multi_index_container<
        BlockWaitingEntry,
        boost::multi_index::indexed_by<
            boost::multi_index::ordered_non_unique<boost::multi_index::member<
                BlockWaitingEntry, rai::AccountHeight,
                &BlockWaitingEntry::for_>>,
            boost::multi_index::hashed_unique<
                boost::multi_index::tag<BlockWaitingByHash>,
                boost::multi_index::member<BlockWaitingEntry, rai::BlockHash,
                                           &BlockWaitingEntry::hash_>>,
            boost::multi_index::ordered_non_unique<
                boost::multi_index::tag<BlockWaitingByArrival>,
                boost::multi_index::member<
                    BlockWaitingEntry, std::chrono::steady_clock::time_point,
                    &BlockWaitingEntry::arrival_>>>>
        entries_;
};

}

max_stars_repo_name: raicoincommunity/Raicoin

max_issues_repo_name: Cryptonoz/Raicoin

max_forks_repo_name: Cryptonoz/Raicoin


text:
/**
 * @file core/common/parsers/ConditionParser.cpp
 * @author Ian Yang
 * @date Created <2010-06-10 19:32:52>
 */
#include "ConditionParser.h"

#include "ConditionOperatorTable.h"

#include <common/Keys.h>

#include <boost/algorithm/string/case_conv.hpp>

#include <iostream>

namespace sf1r {

using driver::Keys;

ConditionParser::ConditionParser()
: array_(),
  property_(),
  op_()
{}

const Value& ConditionParser::operator()(std::size_t index) const
{
    if (index < array_.size())
    {
        return array_[index];
    }
    return def<Value>();
}

bool ConditionParser::parse(const Value& condition)
{
    clearMessages();

    if (condition.type() != Value::kObjectType)
    {
        error() = "Condition must be an object.";
        return false;
    }

    property_ = asString(condition[Keys::property]);
    if (property_.empty())
    {
        error() = "Require property in condition.";
        return false;
    }

    const Value::ArrayType* arrayPtr = condition[Keys::value].getArrayPtr();
    if (arrayPtr)
    {
        array_ = *arrayPtr;
    }
    else
    {
        if (nullValue(condition[Keys::value]))
        {
            array_.resize(0);
        }
        else
        {
            array_.resize(1);
            array_.front() = condition[Keys::value];
        }
    }

    // Keys::operator_
    op_ = asString(condition["operator"]);
    boost::to_lower(op_);

    ConditionOperatorTable::table_type::const_iterator operatorInfo =
        ConditionOperatorTable::get().find(op_);

    if (operatorInfo == ConditionOperatorTable::get().end())
    {
        error() = "Unknown operator " + op_ + ".";
        return false;
    }

    if (array_.size() < operatorInfo->second.minArgCount)
    {
        error() = "Too less arguments for operator " + op_ + ".";
        return false;
    }
    if (array_.size() > operatorInfo->second.maxArgCount)
    {
        error() = "Too many arguments for operator " + op_ + ".";
        return false;
    }

    // use formal name

    op_ = operatorInfo->second.name;

    id_type_ = asString(condition["id_type"]);
    //std::cout<<"id_type"<<id_type<<std::endl;
    return true;
}

}


text:
#ifndef INCLUDE_EXPRTK_SIGPACK_HPP
#define INCLUDE_EXPRTK_SIGPACK_HPP

#include <exprtk.hpp>
#include <armadillo>
#include <sigpack.h>

#ifdef exprtk_enable_debugging
#define exprtk_debug(params) printf params
#else
#define exprtk_debug(params) (void)0
#endif

namespace exprtk
{
   namespace sigpack
   {
      using namespace arma;
      using namespace sp;

      struct fir1 : public exprtk::igeneric_function<double>
      {
         typedef typename exprtk::igeneric_function<double> igfun_t;
         typedef typename igfun_t::parameter_list_t    parameter_list_t;
         typedef typename igfun_t::generic_type        generic_type;
         typedef typename generic_type::scalar_view    scalar_t;
         typedef typename generic_type::vector_view    vector_t;

         fir1() : exprtk::igeneric_function<double>("TTV") {}

         inline double operator() (parameter_list_t parameters)
         {
            scalar_t order(parameters[0]);
            scalar_t cutOffFrequency(parameters[1]);
            vector_t coefficients(parameters[2]);

            vec b = sp::fir1(static_cast<int>(order()), cutOffFrequency());

            memcpy(coefficients.begin(), b.memptr(), std::min((size_t)b.size(), coefficients.size()) * sizeof(double));
            return 1;
         }
      };

      struct package
      {
         fir1 fir1_f;
         bool register_package(exprtk::symbol_table<double>& symtab)
         {
            #define exprtk_register_function(FunctionName,FunctionType)                  \
            if (!symtab.add_function(FunctionName,FunctionType))                         \
            {                                                                            \
               exprtk_debug((                                                            \
               "sigpack::register_package - Failed to add function: %s\n",               \
               FunctionName));                                                           \
               return false;                                                             \
            }                                                                            \

            exprtk_register_function("fir1", fir1_f)
            #undef exprtk_register_function

            return true;
         }
      };
   } // namespace exprtk::sigpack
} // namespace exprtk

#ifdef exprtk_debug
#undef exprtk_debug
#endif

#endif

max_stars_repo_name: rajsite/exorbitant

max_issues_repo_name: rajsite/exorbitant

max_forks_repo_name: rajsite/exorbitant


text:
#include "application.hpp"

#include <boost/foreach.hpp>
#include <Wt/WBootstrapTheme>
#include <Wt/WNavigationBar>
#include <Wt/WPopupMenu>
#include <Wt/WStackedWidget>
#include <Wt/WText>
#include <Wt/Auth/AbstractPasswordService>
#include <Wt/WBorderLayout>

#include "shop/BasketListWidget.hpp"
#include "shop/InputItemWidget.hpp"
#include "shop/ItemGroupCheckbox.hpp"
#include "UserSettings.hpp"
#include "user/UserDisplay.hpp"
#include "version.hpp"

ShareBuy::ShareBuy(const Wt::WEnvironment &env, shared_ptr<std::map<string, shared_ptr<Shop> > > shops, string databaseFile)
:Wt::WApplication(env),
dbSession(databaseFile),
shops(shops),
authWidget(NULL)
{
	// app metadata
	enableUpdates(true); // necessary for updates from the backend parsing web pages
	Wt::WBootstrapTheme *theme=new Wt::WBootstrapTheme();
	theme->setVersion(Wt::WBootstrapTheme::Version::Version3);
	setTheme(theme);

	// navbar
	Wt::WNavigationBar *nav=new Wt::WNavigationBar(this->root());
	nav->setTitle("Sharebuy");
	nav->setResponsive(true);

	Wt::WMenu *leftMenu=new Wt::WMenu();
	nav->addMenu(leftMenu);

	// my items
	Wt::WMenuItem *myItems = leftMenu->addItem("My wishlist");
	myItems->setLink(Wt::WLink(Wt::WLink::InternalPath,"/user/wishlist"));
	myItems->setSelectable(false);

	// shops
	Wt::WPopupMenu *shopMenu=new Wt::WPopupMenu();
	BOOST_FOREACH(auto tuple,*shops)
	{
		shopMenu->addItem(tuple.first)->setLink(Wt::WLink(Wt::WLink::InternalPath,"/shop/"+tuple.first));
	}
	/*Wt::WMenuItem *shopItem=new Wt::WMenuItem("Shops");
	shopItem->setMenu(shopMenu);*/
	leftMenu->addMenu("All user's wishlists",shopMenu);

	// my orders
	Wt::WMenuItem *myOrders = leftMenu->addItem("Stuff I ordered");
	myOrders->setLink(Wt::WLink(Wt::WLink::InternalPath,"/user/orders"));
	myOrders->setSelectable(false);

	// RIGHT MENU
	Wt::WMenu *rightMenu = new Wt::WMenu();
	nav->addMenu(rightMenu, Wt::AlignRight);

	// settings
	Wt::WMenuItem *settings = rightMenu->addItem("Settings");
	settings->setLink(Wt::WLink(Wt::WLink::InternalPath,"/user/settings"));
	settings->setSelectable(false);

	// about
	Wt::WMenuItem *about = rightMenu->addItem("About Sharebuy");
	about->setLink(Wt::WLink(Wt::WLink::InternalPath,"/about"));
	about->setSelectable(false);

	// MAIN WINDOW

	// main window
	Wt::WContainerWidget *main = new Wt::WContainerWidget(this->root());
	Wt::WBorderLayout *layout = new Wt::WBorderLayout();
	main->setLayout(layout);

	// content & auth
	content = new Wt::WContainerWidget();
	layout->addWidget(content, Wt::WBorderLayout::Center);
	createAuthWidget();
	layout->addWidget(authWidget, Wt::WBorderLayout::East);

	// session state
	dbSession.login().changed().connect(this,&ShareBuy::onInternalPathChange);
	internalPathChanged().connect(this,&ShareBuy::onInternalPathChange);
	onInternalPathChange();
}

void ShareBuy::showUserSettings()
{
	PUser user = dbSession.user();
	new UserSettings(content);
}

void ShareBuy::showAbout()
{
	auto text = boost::format("This server is running ShareBuy version %1%. Visit %2% for more information.") % VERSION_STRING % "https://github.com/hephaisto/sharebuy";
	new Wt::WText(text.str(), content);
}

void ShareBuy::showUserItems()
{
	new Wt::WText("
Your personal wishlist
", content);
	new Wt::WText("
Paste the URL for the item and the number of items below.
", content);
	InputItemWidget *input = new InputItemWidget(shops,content);

	PUser user = dbSession.user();

	WishlistForWisher *p = new WishlistForWisher(shops, user, content);
	p->update();
	input->itemEntered().connect(p, &WishlistForWisher::update);

	// find orders with items that belong to userId
	dbo::Transaction transaction(dbSession);
	dbo::collection<dbo::ptr<Order> > myOrders = dbSession.query<dbo::ptr<Order> >(
		"select ordering from ordering,item where item.user_id=? and item.ordering_id=ordering.id group by ordering.id order by ordering.id desc"
		).bind(user.id());

	BOOST_FOREACH(auto order, myOrders)
	{
		OrderOverviewForWisher *ordered=new OrderOverviewForWisher(shops, order, user, content);
		ordered->update();
	}
}

void ShareBuy::showUserOrders()
{
	new Wt::WText("
Items you ordered for other users
", content);

	dbo::Transaction transaction(dbSession);
	dbo::collection<dbo::ptr<Order> > myOrders = dbSession.find<Order >().where("user_id = ?").bind(dbSession.user().id()).orderBy("id desc");

	BOOST_FOREACH(auto order, myOrders)
	{
		OrderOverviewForOrderer *list=new OrderOverviewForOrderer(shops, order, content);
		list->update();
	}

	if(myOrders.size()==0)
		new Wt::WText("You haven't ordered any items yet!", content);
}

void ShareBuy::showShop(string shopName)
{
	new Wt::WText("
Items other users want from "+shopName+"
", content);
	if(shops->find(shopName)!=shops->end())
	{
		ItemGroupCheckbox *groupCb = new ItemGroupCheckbox(shopName, content);
		groupCb->setTitle("Which users to order for?");

		WishlistForOrderer *add=new WishlistForOrderer(shops, shopName, content);
		add->update();
	}
	else
		new Wt::WText("Shop "+shopName+" is not configured",content);
}

void ShareBuy::showUserProfile(string userId)
{
	dbo::Transaction transaction(dbSession);

	PUser user = dbSession.find<User>().where("id = ?").bind(userId);

	new UserDisplay(user, dbSession.user()->isAdmin, content);
}

void ShareBuy::onInternalPathChange()
{
	content->clear();

	if(dbSession.login().loggedIn())
	{

		/*
		shared_ptr<Shop> shop = nullptr;
		BOOST_FOREACH(auto s, *shops)
		{
			shop = s;
		}*/

		//dbo::Query<PItem> query = dbSession.find<Item>().where("shop_name = ?").bind(shop->name);
		if(internalPath()=="/user/wishlist")
			showUserItems();
		else if(internalPath()=="/user/orders")
			showUserOrders();
		else if(internalPath()=="/user/settings")
			showUserSettings();
		else if(internalPath()=="/about")
			showAbout();
		else if(internalPath().substr(0,14)=="/user/profile/")
		{
			string userId = internalPath().substr(14);
			showUserProfile(userId);
		}
		else if(internalPath().substr(0,6)=="/shop/")
		{
			string shopName = internalPath().substr(6);
			showShop(shopName);
		}
		else
			setInternalPath("/user/wishlist", true);

	}
	else
	{
		//new Wt::WText("you have to be logged in to access this page", content);
	}
}
void ShareBuy::createAuthWidget()
{
	if(authWidget)
		delete authWidget;

	authWidget=new Wt::Auth::AuthWidget(Session::auth(), dbSession.users(), dbSession.login());

	authWidget->model()->addPasswordAuth(/*dynamic_cast<Wt::Auth::AbstractPasswordService*>(*/&Session::passwordAuth());
	/*authWidget->model()->addOAuth(Session::oAuth());*/
	authWidget->setRegistrationEnabled(true);
	authWidget->processEnvironment();
}

Wt::Auth::AuthWidget* ShareBuy::getAuthWidget()
{
	return authWidget;
}

max_stars_repo_name: hephaisto/sharebuy

max_issues_repo_name: hephaisto/sharebuy

max_forks_repo_name: hephaisto/sharebuy


text:
#include <string>
#include <iostream>
#include <boost/python.hpp>
#include <thread>
#include <chrono>

using namespace std;
using namespace boost::python;
int main() {
try{
  Py_Initialize();
  char* argv[] = {"hotbox_dnn_transform"};
  PySys_SetArgv(1, argv);

  cout << 1 << endl;
  object main_module_ = import("__main__");
  cout << 2 << endl;
  object main_namespace_ = main_module_.attr("__dict__");
  cout << 3 << endl;
  string model_path = "../../../test/resource/dataset/model.json";
  string weight_path = "../../../test/resource/dataset/weight";
  exec("from keras.models import Sequential\n"
		"from keras.layers import Dense, Activation\n"
		"import numpy as np\n"
		"from keras.models import model_from_json\n"
		"from keras import backend\n",
  	  main_namespace_);
  exec("def get_activations(model, layer, X_batch):\n"
      "\tget_activations = backend.function([model.layers[0].input, backend.learning_phase()], [model.layers[layer].output,])\n"
      "\tactivations = get_activations([np.array([X_batch]),0])\n"
      "\treturn activations[0][0].tolist()\n",
	  main_namespace_);
  exec("def get_model(model_path, weight_path):\n"
	  "\tfile = open(model_path, 'r')\n"
	  "\tmodel_json = file.read()\n"
	  "\tmodel = model_from_json(model_json)\n"
	  "\tmodel.load_weights(weight_path)\n"
	  "\treturn model\n",
	  main_namespace_);
  exec(str("model = get_model('" + model_path + "','" + weight_path + "')\n"),
	  main_namespace_);
  object func = main_module_.attr("get_activations");
  object model = main_module_.attr("model");
  int num_layers = extract<int>(eval("len(model.layers)", main_namespace_));
  cout << "num layers: " << num_layers << endl;
  Py_Finalize();
}
catch (error_already_set const&){
  PyErr_Print();
}
  std::cout << "Done" << std::endl;
  return 0;
}