text:
/*
 * Copyright (C) 2019 HERE Europe B.V.
 *
 * 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.
 *
 * SPDX-License-Identifier: Apache-2.0
 * License-Filename: LICENSE
 */

#include <gtest/gtest.h>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

#ifdef DATASERVICE_WRITE_HAS_OPENSSL
#include <openssl/sha.h>
#endif

#include <olp/authentication/TokenProvider.h>
#include <olp/core/cache/DefaultCache.h>
#include <olp/core/client/ApiError.h>
#include <olp/core/client/HRN.h>
#include <olp/core/client/HttpResponse.h>
#include <olp/core/client/OlpClient.h>
#include <olp/core/client/OlpClientSettings.h>
#include <olp/core/client/OlpClientSettingsFactory.h>
#include <olp/dataservice/write/StreamLayerClient.h>
#include <olp/dataservice/write/model/PublishDataRequest.h>
#include <olp/dataservice/write/model/PublishSdiiRequest.h>
#include <testutils/CustomParameters.hpp>
#include "Utils.h"

const std::string kEndpoint = "endpoint";
const std::string kAppid = "dataservice_write_test_appid";
const std::string kSecret = "dataservice_write_test_secret";
const std::string kCatalog = "dataservice_write_test_catalog";
const std::string kLayer = "layer";

namespace {

namespace write = olp::dataservice::write;
namespace model = olp::dataservice::write::model;

const std::string kBillingTag = "OlpCppSdkTest";

#ifdef DATASERVICE_WRITE_HAS_OPENSSL
// https://stackoverflow.com/a/10632725/275524
std::string sha256(const std::string str) {
  unsigned char hash[SHA256_DIGEST_LENGTH];
  SHA256_CTX sha256;
  SHA256_Init(&sha256);
  SHA256_Update(&sha256, str.c_str(), str.size());
  SHA256_Final(hash, &sha256);
  std::stringstream ss;
  for (int i = 0; i < SHA256_DIGEST_LENGTH; i++) {
    ss << std::hex << std::setw(2) << std::setfill('0') << (int)hash[i];
  }
  return ss.str();
}
#endif

std::string GenerateRandomUUID() {
  static boost::uuids::random_generator gen;
  return boost::uuids::to_string(gen());
}

void PublishDataSuccessAssertions(
    const olp::client::ApiResponse<model::ResponseOkSingle,
                                   olp::client::ApiError>& result) {
  EXPECT_SUCCESS(result);
  EXPECT_FALSE(result.GetResult().GetTraceID().empty());
}

template <typename T>
void PublishFailureAssertions(
    const olp::client::ApiResponse<T, olp::client::ApiError>& result) {
  EXPECT_FALSE(result.IsSuccessful());
  EXPECT_NE(result.GetError().GetHttpStatusCode(),
            olp::http::HttpStatusCode::OK);
}

class DataserviceWriteStreamLayerClientCacheTest : public ::testing::Test {
 protected:
  void SetUp() override {
    ASSERT_NO_FATAL_FAILURE(client_ = CreateStreamLayerClient());
    data_ = GenerateData();
  }

  void TearDown() override {
    data_.reset();
    client_.reset();
    if (disk_cache_) {
      disk_cache_->Close();
    }

    // verify that no other thread contains network or task scheduler instance:
    auto network = network_;
    auto scheduler = task_scheduler_;
    network_.reset();
    task_scheduler_.reset();

    ASSERT_EQ(network.use_count(), 1);
    ASSERT_EQ(scheduler.use_count(), 1);
  }

  std::string GetTestCatalog() {
    return CustomParameters::getArgument(kCatalog);
  }

  std::string GetTestLayer() { return CustomParameters::getArgument(kLayer); }

  void QueueMultipleEvents(int num_events) {
    for (int i = 0; i < num_events; i++) {
      data_->push_back(' ');
      data_->push_back(i);
      auto error = client_->Queue(
          model::PublishDataRequest().WithData(data_).WithLayerId(
              GetTestLayer()));
      ASSERT_FALSE(error) << error.get();
    }
  }

  virtual std::shared_ptr<write::StreamLayerClient> CreateStreamLayerClient() {
    network_ = olp::client::OlpClientSettingsFactory::
        CreateDefaultNetworkRequestHandler();
    task_scheduler_ =
        olp::client::OlpClientSettingsFactory::CreateDefaultTaskScheduler(1u);

    const auto app_id = CustomParameters::getArgument(kAppid);
    const auto secret = CustomParameters::getArgument(kSecret);

    olp::authentication::Settings authentication_settings({app_id, secret});
    authentication_settings.token_endpoint_url =
        CustomParameters::getArgument(kEndpoint);
    authentication_settings.network_request_handler = network_;

    olp::authentication::TokenProviderDefault provider(authentication_settings);

    olp::client::AuthenticationSettings auth_client_settings;
    auth_client_settings.provider = provider;

    olp::client::OlpClientSettings settings;
    settings.authentication_settings = auth_client_settings;
    settings.network_request_handler = network_;
    settings.task_scheduler = task_scheduler_;

    disk_cache_ = std::make_shared<olp::cache::DefaultCache>();
    EXPECT_EQ(disk_cache_->Open(),
              olp::cache::DefaultCache::StorageOpenResult::Success);
    settings.cache = disk_cache_;

    return std::make_shared<write::StreamLayerClient>(
        olp::client::HRN{GetTestCatalog()}, stream_client_settings_, settings);
  }

 private:
  std::shared_ptr<std::vector<unsigned char>> GenerateData() {
    std::string test_suite_name(testing::UnitTest::GetInstance()
                                    ->current_test_info()
                                    ->test_suite_name());
    std::string test_name(
        testing::UnitTest::GetInstance()->current_test_info()->name());
    std::string data_string(test_suite_name + " " + test_name + " Payload");
    return std::make_shared<std::vector<unsigned char>>(data_string.begin(),
                                                        data_string.end());
  }

 protected:
  std::shared_ptr<write::StreamLayerClient> client_;
  std::shared_ptr<std::vector<unsigned char>> data_;
  std::shared_ptr<olp::http::Network> network_;
  std::shared_ptr<olp::thread::TaskScheduler> task_scheduler_;
  std::shared_ptr<olp::cache::DefaultCache> disk_cache_;
  write::StreamLayerClientSettings stream_client_settings_;
};

TEST_F(DataserviceWriteStreamLayerClientCacheTest, Queue) {
  auto error = client_->Queue(
      model::PublishDataRequest().WithData(data_).WithLayerId(GetTestLayer()));

  ASSERT_FALSE(error) << error.get();
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, QueueNullData) {
  auto error =
      client_->Queue(model::PublishDataRequest().WithData(nullptr).WithLayerId(
          GetTestLayer()));

  ASSERT_TRUE(error);
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, QueueExtraRequestParams) {
  const auto uuid = GenerateRandomUUID();

  auto error = client_->Queue(model::PublishDataRequest()
                                  .WithData(data_)
                                  .WithLayerId(GetTestLayer())
                                  .WithTraceId(uuid)
                                  .WithBillingTag(kBillingTag));

  ASSERT_FALSE(error) << error.get();
}

#ifdef DATASERVICE_WRITE_HAS_OPENSSL
TEST_F(DataserviceWriteStreamLayerClientCacheTest, QueueWithChecksum) {
  const std::string data_string(data_->begin(), data_->end());
  auto checksum = sha256(data_string);

  auto error = client_->Queue(model::PublishDataRequest()
                                  .WithData(data_)
                                  .WithLayerId(GetTestLayer())
                                  .WithChecksum(checksum));

  ASSERT_FALSE(error) << error.get();
}
#endif

TEST_F(DataserviceWriteStreamLayerClientCacheTest, FlushDataSingle) {
  auto error = client_->Queue(
      model::PublishDataRequest().WithData(data_).WithLayerId(GetTestLayer()));

  ASSERT_FALSE(error) << error.get();

  auto response = client_->Flush(model::FlushRequest()).GetFuture().get();

  ASSERT_FALSE(response.empty());
  ASSERT_NO_FATAL_FAILURE(PublishDataSuccessAssertions(response[0]));
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, FlushDataMultiple) {
  ASSERT_NO_FATAL_FAILURE(QueueMultipleEvents(5));

  auto response = client_->Flush(model::FlushRequest()).GetFuture().get();

  ASSERT_EQ(5, response.size());
  for (auto& single_response : response) {
    ASSERT_NO_FATAL_FAILURE(PublishDataSuccessAssertions(single_response));
  }
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, FlushDataSingleAsync) {
  auto error = client_->Queue(
      model::PublishDataRequest().WithData(data_).WithLayerId(GetTestLayer()));

  ASSERT_FALSE(error) << error.get();

  std::promise<write::StreamLayerClient::FlushResponse> response_promise;
  bool call_is_async = true;
  auto cancel_token =
      client_->Flush(model::FlushRequest(),
                     [&](write::StreamLayerClient::FlushResponse response) {
                       call_is_async = false;
                       response_promise.set_value(response);
                     });

  EXPECT_TRUE(call_is_async);
  auto response_future = response_promise.get_future();
  auto status = response_future.wait_for(std::chrono::seconds(30));
  if (status != std::future_status::ready) {
    cancel_token.Cancel();
  }
  auto response = response_future.get();

  ASSERT_FALSE(response.empty());
  ASSERT_NO_FATAL_FAILURE(PublishDataSuccessAssertions(response[0]));
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, FlushDataMultipleAsync) {
  ASSERT_NO_FATAL_FAILURE(QueueMultipleEvents(5));

  std::promise<write::StreamLayerClient::FlushResponse> response_promise;
  bool call_is_async = true;
  auto cancel_token =
      client_->Flush(model::FlushRequest(),
                     [&](write::StreamLayerClient::FlushResponse response) {
                       call_is_async = false;
                       response_promise.set_value(response);
                     });

  EXPECT_TRUE(call_is_async);
  auto response_future = response_promise.get_future();
  auto status = response_future.wait_for(std::chrono::seconds(30));
  if (status != std::future_status::ready) {
    cancel_token.Cancel();
  }
  auto response = response_future.get();

  ASSERT_EQ(5, response.size());
  for (auto& single_response : response) {
    ASSERT_NO_FATAL_FAILURE(PublishDataSuccessAssertions(single_response));
  }
}

TEST_F(DataserviceWriteStreamLayerClientCacheTest, FlushDataCancel) {
  auto error = client_->Queue(
      model::PublishDataRequest().WithData(data_).WithLayerId(GetTestLayer()));

  ASSERT_FALSE(error) << error.get();

  auto cancel_future = client_->Flush(model::FlushRequest());

  std::thread([cancel_future]() {
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    cancel_future.GetCancellationToken().Cancel();
  })
      .detach();

  auto response = cancel_future.GetFuture().get();

  ASSERT_EQ(1, response.size());
  if (response[0].IsSuccessful()) {
    SUCCEED();
    return;
  }

  ASSERT_NO_FATAL_FAILURE(PublishFailureAssertions(response[0]));
}

}  // namespace

max_stars_repo_name: matthiaskunter/here-data-sdk-cpp

max_issues_repo_name: matthiaskunter/here-data-sdk-cpp

max_forks_repo_name: matthiaskunter/here-data-sdk-cpp


text:
/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <cmath>
#include <future>

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/opencv.hpp>
#include <unsupported/Eigen/CXX11/Tensor>

#include <boost/format.hpp>

#include "source/gpu/GlfwUtil.h"
#include "source/gpu/ReprojectionGpuUtil.h"
#include "source/util/Camera.h"
#include "source/util/CvUtil.h"
#include "source/util/ImageUtil.h"
#include "source/util/SystemUtil.h"

using namespace fb360_dep;

const std::string kUsageMessage = R"(
- Compute initial depth for every camera
- Repeat pass_count times:
  - Clean away depths that are implausible
  - Recompute depths using clean depths to estimate occlusions

- Example:
    GeometricConsistency \
    --color /path/to/color \
    --output /path/to/output \
    --rig /path/to/rigs/rig.json \
    --first 000000 \
    --last 000000
)";

DEFINE_double(agree_fraction, 0.75, "fraction considered in agreement");
DEFINE_string(color, "", "color directory (required)");
DEFINE_double(disparity_step, 0.5, "pixels per disparity step");
DEFINE_double(downscale, 4, "reduced resolution output");
DEFINE_string(first, "", "first frame to process (lexical)");
DEFINE_bool(keep_clean, false, "only recompute implausible depths");
DEFINE_string(last, "", "last frame to process (lexical)");
DEFINE_int32(median, 0, "radius of median filter applied to input");
DEFINE_string(output, "", "output subdirectory (required)");
DEFINE_int32(pass_count, 2, "how many times to refine depth");
DEFINE_string(rig, "", "path to rig .json file (required)");
DEFINE_string(single, "", "render a single destination camera");

// Image is 16-bit RGBA
using Pixel = cv::Vec4w;
using Image = cv::Mat_<Pixel>;

const int kSignedMax = INT16_MAX;
using SignedPixel = cv::Vec<int16_t, 4>;

void dump(const filesystem::path& path, const cv::Mat_<float>& mat) {
  fb360_dep::cv_util::writeCvMat32FC1ToPFM(path.string() + ".pfm", mat);
  // for convenience, also dump 1.0 m / mat as png
  cv::Mat_<float> disparity = 1.0 / mat;
  cv_util::imwriteExceptionOnFail(
      path.string() + "_disparity.png", cv_util::convertTo<uint16_t>(disparity));
}

// Depth is fp32
using DepthMat = cv::Mat_<float>;

template <typename T>
std::vector<GLuint> createTextures(
    const std::vector<T>& mats,
    const GLenum internalFormat, // e.g. GL_RGB8, GL_SRGB8_ALPHA8, GL_RGBA16F
    const GLenum format, // e.g. GL_RED, GL_RGB, GL_BGRA
    const GLenum type) { // e.g. GL_UNSIGNED_BYTE, GL_FLOAT
  std::vector<GLuint> result;
  for (const T& mat : mats) {
    result.push_back(createTexture(
        mat.cols,
        mat.rows,
        mat.ptr(),
        internalFormat,
        format,
        type,
        true)); // buildMipmaps = true
    setTextureAniso();
    setTextureWrap(GL_CLAMP_TO_BORDER);
  }
  return result;
}

static Camera::Real computeRigRadius(const Camera::Rig& rig) {
  Camera::Real sum = 0;
  for (const Camera& camera : rig) {
    sum += camera.position.norm();
  }
  return sum / rig.size();
}

template <typename T>
T sq(const T& t) {
  return t * t;
}

float sumNorm(const SignedPixel& rgba) {
  float sum = 0;
  for (int c = 0; c < 3; ++c) { // exclude alpha
    sum += float(rgba[c]);
  }
  return sum / kSignedMax;
}

float sumSqNorm(const SignedPixel& rgba) {
  float sum = 0;
  for (int c = 0; c < 3; ++c) { // exclude alpha
    sum += sq(float(rgba[c]));
  }
  return sum / (kSignedMax * kSignedMax);
}

Camera::Real sliceDisparity(const int slice, const int sliceCount) {
  return ReprojectionTable::unnormalizeDisparity((slice + 0.5) / sliceCount);
}

using Volume = Eigen::Tensor<float, 3, Eigen::RowMajor>;

DepthMat winnerTakesAll(const Volume& costs) {
  // find cheapest depth for each x, y in costs (NAN if everything is NAN)
  DepthMat depth(costs.dimension(1), costs.dimension(2), NAN);
  Eigen::MatrixXf best(depth.rows, depth.cols);
  best.setConstant(FLT_MAX);
  for (int d = 0; d < costs.dimension(0); ++d) {
    for (int y = 0; y < costs.dimension(1); ++y) {
      for (int x = 0; x < costs.dimension(2); ++x) {
        if (best(y, x) > costs(d, y, x)) { // note: (x > NAN) is alwyas false
          best(y, x) = costs(d, y, x);
          depth(y, x) = 1 / sliceDisparity(d, costs.dimension(0));
        }
      }
    }
  }

  // NAN out the edges
  for (int y = 0; y < costs.dimension(1); ++y) {
    depth(y, 0) = depth(y, costs.dimension(2) - 1) = NAN;
  }
  for (int x = 0; x < costs.dimension(2); ++x) {
    depth(0, x) = depth(costs.dimension(1) - 1, x) = NAN;
  }
  return depth;
}

cv::Mat_<SignedPixel> computeReference(const Image& image, int w, int h) {
  Image resized = cv_util::resizeImage(image, {w, h});
  cv::Mat_<SignedPixel> reference;
  resized.convertTo(reference, CV_16S, INT16_MAX / double(UINT16_MAX));
  return reference;
}

DepthMat computeDepth(
    const Camera::Rig& rig, // rig determines the resolution of the result
    const int d,
    const std::vector<Image>& images, // full resolution images
    const std::vector<GLuint>& imageTextures,
    const std::vector<DepthMat>& depths = {}, // could be same rez as rig, or not
    const std::vector<GLuint>& depthTextures = {}) {
  const Camera& dst = rig[d];
  LOG(INFO) << boost::format("compute depth for %1%") % dst.id;

  // compute reprojection textures
  std::vector<ReprojectionTexture> reprojections;
  for (const Camera& src : rig) {
    reprojections.emplace_back(dst, src);
  }

  // downsample destination image
  const int w = dst.resolution.x();
  const int h = dst.resolution.y();
  cv::Mat_<SignedPixel> reference = computeReference(images[d], w, h);

  // compute how many slices we need
  const Camera::Real radius = computeRigRadius(rig);
  const Camera::Real minDistance = 1 / ReprojectionTable::maxDisparity();
  const Camera::Real angle = asin(radius / minDistance);
  const Camera::Real focal = dst.focal.norm() * sqrt(0.5);
  const Camera::Real pixels = focal * angle;
  const int sliceCount = std::round(pixels / FLAGS_disparity_step);

  // compute each slice of the cost volume
  Volume costs(sliceCount, h, w);
  for (int slice = 0; slice < sliceCount; ++slice) {
    Camera::Real disparity = sliceDisparity(slice, sliceCount);
    LOG(INFO) << boost::format("slice %1%/%2% (%3%)") % slice % sliceCount % disparity;

    // accumulate each source cost into accum
    cv::Mat_<cv::Vec2f> accum(h, w, cv::Vec2f(0, 0));
    for (int s = 0; s < int(rig.size()); ++s) {
      if (s == d) {
        continue; // don't compare destination to itself
      }

      // compute src color at disparity by reprojection
      cv::Mat_<SignedPixel> image = reproject<SignedPixel>(
          w, h, GL_RGBA16, GL_RGBA, GL_SHORT, reprojections[s], imageTextures[s], disparity);

      // alpha away occluded areas if we have depth information
      if (!depths.empty()) {
        DepthMat depth = reproject<float>(
            w, h, GL_R32F, GL_RED, GL_FLOAT, reprojections[s], depthTextures[s], disparity);
        for (int y = 0; y < h; ++y) {
          for (int x = 0; x < w; ++x) {
            if (!std::isnan(depth(y, x))) {
              const Camera::Vector3 world = dst.rig({x, y}, 1 / disparity);
              const Camera::Real distance = (world - rig[s].position).norm();
              if (depth(y, x) < distance * FLAGS_agree_fraction) {
                image(y, x)[3] = 0; // src is occluded
              }
            }
          }
        }
      }

      // compute average of difference
      cv::Mat_<SignedPixel> diff = image - reference;
      const cv::Size box(3, 3);
      cv::Mat_<SignedPixel> average;
      cv::blur(diff, average, box);

      // compute average of diff^2
      cv::Mat_<SignedPixel> averageOfSq;
      cv::blur(diff.mul(diff, 1.0 / kSignedMax), averageOfSq, box);

      // cost += variance of diff = average of diff^2 - (average of diff)^2
      for (int y = 0; y < h; ++y) {
        for (int x = 0; x < w; ++x) {
          if (image(y, x)[3] == kSignedMax && average(y, x)[3] == 0) {
            accum(y, x) += cv::Vec2f(sumNorm(averageOfSq(y, x)) - sumSqNorm(average(y, x)), 1);
          }
        }
      }
    }

    // transfer accumulated fraction to cost
    for (int y = 0; y < h; ++y) {
      for (int x = 0; x < w; ++x) {
        costs(slice, y, x) = accum(y, x)[0] / accum(y, x)[1];
      }
    }
  }

  // winner takes all
  return winnerTakesAll(costs);
}

bool isPointBad(
    const Camera::Vector3& world,
    const Camera::Rig& rig,
    const int d,
    const std::vector<DepthMat>& depths) {
  // go through source cameras and see whether any disagree
  for (int s = 0; s < int(rig.size()); ++s) {
    if (s == d) {
      continue; // don't check dst against itself
    }

    // calculate pixel in src camera that sees world point
    const Camera& src = rig[s];
    Camera::Vector2 pixel;
    if (!src.sees(world, pixel)) {
      continue;
    }

    // calculate depth for that pixel in src camera
    const DepthMat& depth = depths[s];
    CHECK_EQ(src.resolution.x(), depth.cols);
    CHECK_EQ(src.resolution.y(), depth.rows);
    const float srcDepth = depth(pixel.y(), pixel.x()); // nearest

    // calculate distance from src camera to world point
    const float proposal = (world - src.position).norm();
    if (proposal < srcDepth * FLAGS_agree_fraction) {
      return true; // proposal is closer than src, reject
    }
  }
  return false;
}

DepthMat cleanDepth(const Camera::Rig& rig, const int d, const std::vector<DepthMat>& depths) {
  const Camera& dst = rig[d];
  LOG(INFO) << boost::format("cleaning %1%") % dst.id;

  // NaN out depths that other cameras disagree with
  DepthMat depth = depths[d].clone();
  CHECK_EQ(dst.resolution.x(), depth.cols);
  CHECK_EQ(dst.resolution.y(), depth.rows);
  for (int y = 0; y < depth.rows; ++y) {
    for (int x = 0; x < depth.cols; ++x) {
      Camera::Vector3 world = dst.rig({x + 0.5, y + 0.5}, depth(y, x));
      if (isPointBad(world, rig, d, depths)) {
        depth(y, x) = NAN;
      }
    }
  }
  return depth;
}

// copy every non-nan depth from cleanDepth into depth
void restoreCleanDepth(DepthMat& depth, const DepthMat& cleanDepth) {
  for (int y = 0; y < depth.rows; ++y) {
    for (int x = 0; x < depth.cols; ++x) {
      float value = cleanDepth(y, x);
      if (!std::isnan(value)) {
        depth(y, x) = value;
      }
    }
  }
}

Camera::Rig downscale(const Camera::Rig& rig) {
  Camera::Rig result;
  for (const Camera& camera : rig) {
    Camera::Vector2 resolution = camera.resolution / FLAGS_downscale;
    result.push_back(camera.rescale(resolution.array().round()));
  }
  return result;
}

void processFrame(const std::string& frameName, const Camera::Rig& rig) {
  const filesystem::path path = filesystem::path(FLAGS_output) / frameName;
  filesystem::create_directory(path);

  // load images
  const std::vector<Image> images = image_util::loadImages<Pixel>(FLAGS_color, rig, frameName);
  const std::vector<GLuint> imageTextures =
      createTextures(images, GL_RGBA16, GL_RGBA, GL_UNSIGNED_SHORT);

  // compute initial depth estimate
  Camera::Rig small = downscale(rig);
  std::vector<DepthMat> depths(small.size());
  for (int d = 0; d < int(small.size()); ++d) {
    depths[d] = computeDepth(small, d, images, imageTextures);
    dump(path / (boost::format("%1%_iffy") % small[d].id).str(), depths[d]);
  }

  // refine depth estimate
  for (int pass = 0; pass < FLAGS_pass_count; ++pass) {
    // compute clean depths by getting rid of improbable ones
    std::vector<DepthMat> cleanDepths;
    for (int d = 0; d < int(small.size()); ++d) {
      cleanDepths.push_back(cleanDepth(small, d, depths));
      dump(path / (boost::format("%1%_%2%_clean") % small[d].id % pass).str(), cleanDepths[d]);
    }
    const std::vector<GLuint> cleanDepthTextures =
        createTextures(cleanDepths, GL_R32F, GL_RED, GL_FLOAT);

    // recompute depth using cleaned depths
    for (int d = 0; d < int(small.size()); ++d) {
      depths[d] = computeDepth(small, d, images, imageTextures, cleanDepths, cleanDepthTextures);
      dump(path / (boost::format("%1%_%2%") % small[d].id % pass).str(), depths[d]);
    }

    // restore clean depths
    if (FLAGS_keep_clean) {
      for (int d = 0; d < int(small.size()); ++d) {
        restoreCleanDepth(depths[d], cleanDepths[d]);
      }
    }

    // free clean depth textures
    for (const GLuint& texture : cleanDepthTextures) {
      glDeleteTextures(1, &texture);
    }
  }

  // free image textures
  for (const GLuint& texture : imageTextures) {
    glDeleteTextures(1, &texture);
  }
}

class OffscreenWindow : public GlWindow {
 protected:
  Camera::Rig& rig;

 public:
  OffscreenWindow(Camera::Rig& rig) : GlWindow::GlWindow(), rig(rig) {
    filesystem::create_directory(FLAGS_output);
  }

  void display() override {
    const int numFrames = std::stoi(FLAGS_last) - std::stoi(FLAGS_first) + 1;
    for (int iFrame = 0; iFrame < numFrames; ++iFrame) {
      const std::string frameName =
          image_util::intToStringZeroPad(iFrame + std::stoi(FLAGS_first), 6);
      LOG(INFO) << boost::format("Processing frame %1%") % frameName;

      processFrame(frameName, rig);
    }
  }
};

int main(int argc, char* argv[]) {
  system_util::initDep(argc, argv, kUsageMessage);

  CHECK_NE(FLAGS_rig, "");
  CHECK_NE(FLAGS_color, "");
  CHECK_NE(FLAGS_output, "");

  // prepare for offscreen rendering
  Camera::Rig rig = Camera::loadRig(FLAGS_rig);
  OffscreenWindow offscreenWindow(rig);

  // Render frames
  offscreenWindow.display();

  return EXIT_SUCCESS;
}

ext: cpp


text:
//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#ifndef __Thea_Algorithms_LinearLeastSquares3_hpp__
#define __Thea_Algorithms_LinearLeastSquares3_hpp__

#include "../Common.hpp"
#include "../Array.hpp"
#include "../Line3.hpp"
#include "../MatVec.hpp"
#include "../Plane3.hpp"
#include "CentroidN.hpp"
#include "Iterators.hpp"
#include "PointTraitsN.hpp"
#include <Eigen/Eigenvalues>

namespace Thea {
namespace Algorithms {

/** Fitting linear models to 3D data by minimizing sum-of-squared-errors. */
template <typename T, typename Enable = void>
class /* THEA_API */ LinearLeastSquares3
{
  public:
    /**
     * Linear least-squares fitting of a line to a set of 3D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param line Used to return the best-fit line.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed in the process of finding the
     *   best-fit line.
     *
     * @return The sum of squared fitting errors.
     */
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line3 & line, Vector3 * centroid = nullptr);

    /**
     * Linear least-squares fitting of a plane to a set of 3D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param plane Used to return the best-fit plane.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed in the process of finding
     *   the best-fit plane.
     *
     * @return The fitting quality: 0 (worst) to 1 (perfect).
     */
    template <typename InputIterator>
    static double fitPlane(InputIterator begin, InputIterator end, Plane3 & plane, Vector3 * centroid = nullptr);

}; // class LinearLeastSquares3

// Fitting linear models to sets of objects that map to single points in 3-space.
template <typename T>
class LinearLeastSquares3<T, typename std::enable_if< IsNonReferencedPointN<T, 3>::value >::type>
{
  public:
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line3 & line, Vector3 * centroid = nullptr)
    {
      Vector3d center;
      Matrix3d cov = covMatrix(begin, end, center);

      double sum = 0;
      for (auto iter = makeRefIterator(begin); iter != makeRefIterator(end); ++iter)
      {
        Vector3d diff = PointTraitsN<T, 3>::getPosition(*iter).template cast<double>() - center;
        sum += (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]);
      }

      Matrix3d m = sum * Matrix3d::Identity() - cov;
      Eigen::SelfAdjointEigenSolver<Matrix3d> eigensolver;
      if (eigensolver.computeDirect(m).info() != Eigen::Success)
        throw Error("LinearLeastSquares3: Could not eigensolve matrix");

      // Eigenvalues are in increasing order
      line = Line3::fromPointAndDirection(center.cast<Real>(), eigensolver.eigenvectors().col(0).cast<Real>());
      if (centroid) *centroid = center.cast<Real>();
      return eigensolver.eigenvalues()[0];
    }

    template <typename InputIterator>
    static double fitPlane(InputIterator begin, InputIterator end, Plane3 & plane, Vector3 * centroid = nullptr)
    {
      Vector3d center;
      Matrix3d cov = covMatrix(begin, end, center);

      Eigen::SelfAdjointEigenSolver<Matrix3d> eigensolver;
      if (eigensolver.computeDirect(cov).info() != Eigen::Success)
        throw Error("LinearLeastSquares3: Could not eigensolve covariance matrix");

      // Eigenvalues are non-negative (covariance matrix is non-negative definite) and in increasing order
      plane = Plane3::fromPointAndNormal(center.cast<Real>(), eigensolver.eigenvectors().col(0).cast<Real>());
      if (centroid) *centroid = center.cast<Real>();
      return eigensolver.eigenvalues()[0];
    }

  private:
    /** Compute the covariance matrix between the coordinates of a set of 3D points. */
    template <typename InputIterator>
    static Matrix3d covMatrix(InputIterator begin, InputIterator end, Vector3d & centroid)
    {
      centroid = CentroidN<T, 3>::compute(begin, end).template cast<double>();

      Matrix3d m = Matrix3d::Zero();
      for (auto iter = makeRefIterator(begin); iter != makeRefIterator(end); ++iter)
      {
        Vector3d diff = PointTraitsN<T, 3>::getPosition(*iter).template cast<double>() - centroid;
        m += diff * diff.transpose();  // outer product
      }

      return m;
    }

}; // class LinearLeastSquares3<Point3>

} // namespace Algorithms
} // namespace Thea

#endif

ext: hpp

max_stars_repo_name: sidch/Thea

max_issues_repo_name: sidch/Thea

max_forks_repo_name: sidch/Thea


text:
//--------------------------------C-Code

#include "./include/System.h"
#include <boost/python.hpp>
#include <numpy/ndarrayobject.h>
#include <opencv2/core/core.hpp>

#include <string>
#include "lib/conversion.h"

namespace bpy = boost::python;
using namespace std;

//  need to initialize numpy
#if PY_MAJOR_VERSION >= 3
int
#else
void
#endif
init_numpy()
{
    bpy::numeric::array::set_module_and_type("numpy", "ndarray");
    import_array();
}

class SLAMClass{
    public:
        SLAMClass();

        NDArrayConverter cvt;

        void init(const std::string &path_to_vocabulary, const std::string &path_to_settings, const string camera_type, const bool flag_visualize){
            ORB_SLAM2::Config::SetParameterFile(path_to_settings);
            this->slam_obj = new ORB_SLAM2::System(path_to_vocabulary, path_to_settings, ORB_SLAM2::System::RGBD, flag_visualize);
            std::cout << path_to_vocabulary << std::endl;
        }

        PyObject* track_rgbd(PyObject* rgb_img, PyObject* d_img, const double timestamp){
            cv::Mat rgb_img_mat = cvt.toMat(rgb_img);
            cv::Mat d_img_mat = cvt.toMat(d_img);
//            d_img_mat.convertTo(d_img_mat, CV_32F);
            auto camera_matrix = this->slam_obj->TrackRGBD(rgb_img_mat, d_img_mat, timestamp);
            PyObject* ret = cvt.toNDArray(camera_matrix);
            return ret;
        }

        void prepare_dump(){
            slam_obj->PrepareDump();
        }

            // stuff for export
//    cv::Mat kf_ids_from_mps;
//    cv::Mat kf_ids;
//    cv::Mat plane_ids;
//    cv::Mat mp_3dpts;
//    cv::Mat kf_3dpts;
//    cv::Mat plane_ids_from_boundary_pts;
//    cv::Mat plane_params;
//    cv::Mat plane_boundary_pts;

        PyObject* get_kf_ids_from_mps(){
            return cvt.toNDArray(slam_obj->dump_kf_ids_from_mps());
        }
        PyObject* get_kf_ids(){
            return cvt.toNDArray(slam_obj->dump_kf_ids());
        }
        PyObject* get_mp_3dpts(){
            return cvt.toNDArray(slam_obj->dump_mp_3dpts());
        }
        PyObject* get_kf_3dpts(){
            return cvt.toNDArray(slam_obj->dump_kf_3dpts());
        }
        PyObject* get_plane_params(){
            return cvt.toNDArray(slam_obj->dump_plane_params());
        }
        PyObject* get_plane_ids(){
            return cvt.toNDArray(slam_obj->dump_plane_ids());
        }
        PyObject* get_plane_ids_from_boundary_pts(){
            return cvt.toNDArray(slam_obj->dump_plane_ids_from_boundary_pts());
        }
        PyObject* get_boundary_pts(){
            return cvt.toNDArray(slam_obj->dump_boundary_pts());
        }

        PyObject* get_boundary_update_sizes(){
            return cvt.toNDArray(slam_obj->dump_cntBoundaryUpdateSizes());
        }

        PyObject* get_grid_map(){
            return cvt.toNDArray(slam_obj->get_gridmaps());
        }

        ~SLAMClass();

    private:
        ORB_SLAM2::System *slam_obj;
};

    SLAMClass::SLAMClass() {
        // nothing
    }

    SLAMClass::~SLAMClass() {
        delete this->slam_obj; // Otherwise may end up with a SegFault!!
    }

//--------------------------------Boost-Python-Code

BOOST_PYTHON_MODULE(ORBSLAM2)
{
    using namespace boost::python;
    Py_Initialize(); // required by NumPy
    init_numpy(); // required by NumPy

    class_<SLAMClass>("SLAM")
        .def("init", &SLAMClass::init)
        .def("track_rgbd", &SLAMClass::track_rgbd)
.def("prepare_dump", &SLAMClass::prepare_dump)
.def("get_kf_ids_from_mps", &SLAMClass::get_kf_ids_from_mps)
.def("get_kf_ids", &SLAMClass::get_kf_ids)
.def("get_plane_ids", &SLAMClass::get_plane_ids)
.def("get_mp_3dpts", &SLAMClass::get_mp_3dpts)
.def("get_kf_3dpts", &SLAMClass::get_kf_3dpts)
.def("get_plane_ids_from_boundary_pts", &SLAMClass::get_plane_ids_from_boundary_pts)
.def("get_plane_params", &SLAMClass::get_plane_params)
.def("get_boundary_pts", &SLAMClass::get_boundary_pts)
.def("get_boundary_update_sizes", &SLAMClass::get_boundary_update_sizes)
.def("get_grid_map", &SLAMClass::get_grid_map);
//    cv::Mat kf_ids_from_mps;
//    cv::Mat kf_ids;
//    cv::Mat plane_ids;
//    cv::Mat mp_3dpts
//    cv::Mat kf_3dpts;
//    cv::Mat plane_ids_from_boundary_pts;
//    cv::Mat plane_params;
//    cv::Mat plane_boundary_pts;

}


text:
#include "ConditionVariable.hpp"

#include <boost/fiber/condition_variable.hpp>

namespace mo
{
    ConditionVariable::ConditionVariable()
    {
        m_cv.reset(new boost::fibers::condition_variable_any());
    }

    ConditionVariable::~ConditionVariable() = default;

    void ConditionVariable::notify_one() noexcept
    {
        m_cv->notify_one();
    }

    void ConditionVariable::notify_all() noexcept
    {
        m_cv->notify_all();
    }

    void ConditionVariable::wait(Mutex::Lock_t& lt)
    {
        m_cv->wait(lt);
    }

    void ConditionVariable::wait(RecursiveMutex::Lock_t& lt)
    {
        m_cv->wait(lt);
    }

    void ConditionVariable::wait(TimedMutex::Lock_t& lt)
    {
        m_cv->wait(lt);
    }

    ConditionVariable::STATUS ConditionVariable::wait_until(RecursiveMutex::Lock_t& lt,
                                                            const std::chrono::steady_clock::time_point& timeout_time_)
    {
        if (m_cv->wait_until(lt, timeout_time_) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }

    ConditionVariable::STATUS ConditionVariable::wait_until(TimedMutex::Lock_t& lt,
                                                            const std::chrono::steady_clock::time_point& timeout_time_)
    {
        if (m_cv->wait_until(lt, timeout_time_) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }

    ConditionVariable::STATUS ConditionVariable::wait_until(Mutex::Lock_t& lt,
                                                            const std::chrono::steady_clock::time_point& timeout_time_)
    {
        if (m_cv->wait_until(lt, timeout_time_) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }

    ConditionVariable::STATUS ConditionVariable::wait_for(RecursiveMutex::Lock_t& lt,
                                                          const Duration_t& timeout_duration)
    {
        if (m_cv->wait_for(lt, timeout_duration) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }

    ConditionVariable::STATUS ConditionVariable::wait_for(TimedMutex::Lock_t& lt, const Duration_t& timeout_duration)
    {
        if (m_cv->wait_for(lt, timeout_duration) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }

    ConditionVariable::STATUS ConditionVariable::wait_for(Mutex::Lock_t& lt, const Duration_t& timeout_duration)
    {
        if (m_cv->wait_for(lt, timeout_duration) == boost::fibers::cv_status::no_timeout)
        {
            return STATUS::kNO_TIMEOUT;
        }
        return STATUS::kTIMEOUT;
    }
} // namespace mo

max_stars_repo_name: dtmoodie/MetaObject

max_issues_repo_name: dtmoodie/MetaObject

max_forks_repo_name: dtmoodie/MetaObject


text:
// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <limits>
#include <cmath>
#include <boost/geometry.hpp>
#include "modules/world/opendrive/lane_section.hpp"
#include "modules/world/opendrive/lane_section.hpp"
#include "modules/geometry/commons.hpp"

namespace modules {
namespace world {
namespace opendrive {

void XodrLaneSection::AddLane(const XodrLanePtr& lane)  {
    lanes_[lane->GetId()] = lane;
}

XodrLanePtr XodrLaneSection::GetLaneByPosition(XodrLanePosition pos) {

  XodrLanePtr ret_lane_ptr = nullptr;

  for ( auto const& lane : lanes_ ) {
    if (pos == lane.second->GetLanePosition()) {
      ret_lane_ptr = lane.second;
    }
  }
  return ret_lane_ptr;
}

// TODO (@hart): replace dummy by real ray-check
XodrLanePtr XodrLaneSection::GetNearestLaneOnN(double x, double y, double vx, double vy) {
  float x_new = x + vx;
  float y_new = y + vy;
  modules::geometry::Point2d new_point(x_new, y_new);
  float min_dist = 100000.0;
  XodrLanePtr ret_lane_ptr = lanes_.begin()->second;

  for ( auto const& lane : lanes_ ) {
    float distance = modules::geometry::Distance(lane.second->GetLine(), new_point);
    if (distance < min_dist) {
      min_dist = distance;
      ret_lane_ptr = lane.second;
    }
  }

  return ret_lane_ptr;
}

XodrLanePtr XodrLaneSection::GetLaneWithOffset(const models::dynamic::State& state, double angle_offset) {
  double x = state[models::dynamic::StateDefinition::X_POSITION];
  double y = state[models::dynamic::StateDefinition::Y_POSITION];
  double theta = state[models::dynamic::StateDefinition::THETA_POSITION];
  double theta_new = theta + angle_offset;

  XodrLanePtr lane_ptr = GetNearestLaneOnN(x, y, cos(theta_new), sin(theta_new));
  return lane_ptr;
}

}  // namespace opendrive
}  // namespace world
}  // namespace modules


text:
//  equal_to.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef BOOST_RATIO_MPL_EQUAL_TO_HPP
#define BOOST_RATIO_MPL_EQUAL_TO_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost {
namespace mpl {

template<>
struct equal_to_impl< rational_c_tag,rational_c_tag >
{
    template< typename R1, typename R2 > struct apply
        : ratio_equal<R1, R2>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_EQUAL_TO_HPP

max_stars_repo_name: rudylee/expo

max_issues_repo_name: rudylee/expo

max_forks_repo_name: rudylee/expo


text:
//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <iostream>

int main()
{
	using namespace boost::numeric::ublas;

	using format_t  = column_major;
	using value_t   = float;
	using tensor_t = tensor<value_t,format_t>;
	using matrix_t = matrix<value_t,format_t>;
	using namespace boost::numeric::ublas::index;

	// Tensor-Vector-Multiplications - Including Transposition
	{

		auto n = shape{3,4,2};
		auto A = tensor_t(n,1);
		auto B1 = matrix_t(n[1],n[2],2);
		auto v1 = tensor_t(shape{n[0],1},2);
		auto v2 = tensor_t(shape{n[1],1},2);
//		auto v3 = tensor_t(shape{n[2],1},2);

		// C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);
		// tensor_t C1 = B1 + prod(A,vector_t(n[0],1),1);
//		tensor_t C1 = B1 + A(_i,_,_) * v1(_i,_);

		// C2(i,k) = A(i,j,k)*v2(j) + 4;
		//tensor_t C2 = prod(A,vector_t(n[1],1),2) + 4;
//		tensor_t C2 = A(_,_i,_) * v2(_i,_) + 4;

		// not yet implemented!
		// C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
		// tensor_t C3 = prod(prod(prod(A,v1,1),v2,1),v3,1);
		// tensor_t C3 = A(_i,_j,_k) * v1(_i,_) * v2(_j,_) * v3(_k,_);

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,k) = A(i,j,k)*v2(j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}

	// Tensor-Matrix-Multiplications - Including Transposition
	{
		auto n = shape{3,4,2};
		auto m = 5u;
		auto A = tensor_t(n,2);
		auto B  = tensor_t(shape{n[1],n[2],m},2);
		auto B1 = tensor_t(shape{m,n[0]},1);
		auto B2 = tensor_t(shape{m,n[1]},1);

		// C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);
		// tensor_t C1 = B + prod(A,B1,1);
//		tensor_t C1 = B + A(_i,_,_) * B1(_,_i);

		// C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;
		// tensor_t C2 = prod(A,B2) + 4;
//		tensor_t C2 =  A(_,_j,_) * B2(_,_j) + 4;

		// C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
		// not yet implemented.

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

//		// formatted output
//		std::cout << "% --------------------------- " << std::endl;
//		std::cout << "% --------------------------- " << std::endl << std::endl;
//		std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
//		std::cout << "C3=" << C3 << ";" << std::endl << std::endl;
	}

	// Tensor-Tensor-Multiplications Including Transposition
	{
		auto na = shape{3,4,5};
		auto nb = shape{4,6,3,2};
		auto A = tensor_t(na,2);
		auto B = tensor_t(nb,3);
		auto T1 = tensor_t(shape{na[2],na[2]},2);
		auto T2 = tensor_t(shape{na[2],nb[1],nb[3]},2);

		// C1(j,l) = T1(j,l) + A(i,j,k)*A(i,j,l) + 5;
		// tensor_t C1 = T1 + prod(A,A,perm_t{1,2}) + 5;
//		tensor_t C1 = T1 + A(_i,_j,_m)*A(_i,_j,_l) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(k,l) = T1(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
		//tensor_t C2 = T2 + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;
//		tensor_t C2 = T2 + A(_i,_j,_k)*B(_j,_l,_i,_m) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "%  C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}
}

max_stars_repo_name: cpp-pm/boost

max_issues_repo_name: cpp-pm/boost

max_forks_repo_name: cpp-pm/boost


text:
#include "ros/ros.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <math.h>
#include <sstream>
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <cmath>
#include <stdio.h>
#include <stdlib.h>
#include <tbb/atomic.h>

#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"

#include <dynamixel_msgs/MotorStateList.h>
#include <dynamixel_msgs/JointState.h>
#include <dynamixel_controllers/SetComplianceSlope.h>
#include <dynamixel_controllers/SetCompliancePunch.h>
#include <dynamixel_controllers/SetComplianceMargin.h>
#include <dynamixel_controllers/SetCompliancep.h>
#include <dynamixel_controllers/SetCompliancei.h>
#include <dynamixel_controllers/SetComplianced.h>
#include <dynamixel_controllers/SetSpeed.h>
#include <dynamixel_controllers/TorqueEnable.h>

#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <sepanta_msgs/upperbodymotors.h>
#include <sepanta_msgs/upperbodymotorsfeedback.h>
#include <sepanta_msgs/motorfeedback.h>
#include <sepanta_msgs/motor.h>
#include <sepanta_msgs/motorreset.h>
#include <sepanta_msgs/motortorque.h>
#include <sepanta_msgs/motorpid.h>
#include "upperbody_core/upperbodycorefunctions.h"

#include "opencv2/opencv.hpp"
#include <ros/package.h>

//version 1394/1/19 <=full check done
//version 1394/1/19 <=add waist motor
using namespace std;
using namespace boost;
using namespace cv;

bool robot_init = false;
int init_counter = 0;
bool motor_init = false;
bool enable_debug = true;
bool App_exit = false;

int ismotorthere_counter = 0;
bool pid_init = false;

ros::Publisher chatter_pub_motor_right[right_motor_count];
ros::Publisher chatter_pub_motor_left[left_motor_count];
ros::Publisher chatter_pub_motor_head[head_motor_count];

ros::Publisher chatter_pub_ack;
ros::Publisher chatter_pub_log;

ros::Publisher chatter_pub_motors;
ros::ServiceClient service_speed_motor_right[right_motor_count];
ros::ServiceClient service_speed_motor_left[left_motor_count];
ros::ServiceClient service_speed_motor_head[head_motor_count];

ros::ServiceClient service_torque_motor_right[right_motor_count];
ros::ServiceClient service_torque_motor_left[left_motor_count];
ros::ServiceClient service_torque_motor_head[head_motor_count];

ros::ServiceClient service_margin[total_motor_count];
ros::ServiceClient service_punch[total_motor_count];
ros::ServiceClient service_slope[total_motor_count];

ros::ServiceClient service_i[total_motor_count];
ros::ServiceClient service_d[total_motor_count];
ros::ServiceClient service_p[total_motor_count];

ros::ServiceServer service_reset_motor;
ros::ServiceServer service_torque_motor;
ros::ServiceServer service_pid_motor;

int system_state = 0;
bool motor_connected = false;

void send_ack()
{
    std_msgs::String msg_ack;
    msg_ack.data = "ok";
    chatter_pub_ack.publish(msg_ack);
}

void send_log(string msg)
{
    //std_msgs::String msg_log;
    //msg_log.data = msg;
    //chatter_pub_log.publish(msg_log);
}

int save_params_pid()
{
    std::string homedir = getenv("HOME");
    std::string path_points =  homedir + "catkin_ws/settings/pid.yml";
    FileStorage fs(path_points, FileStorage::WRITE);

    for ( int i = 0 ; i < motor_list.size() ; i++ )
    {
        string pp = motor_list.at(i).name + "P";
        string ii = motor_list.at(i).name + "I";
        string dd = motor_list.at(i).name + "D";

        fs << pp <<  motor_list.at(i).p;
        fs << ii <<  motor_list.at(i).i;
        fs << dd <<  motor_list.at(i).d;
    }

    fs.release();
}

void load_params_pid()
{
    std::string homedir = getenv("HOME");
    std::string path_points =  homedir + "/catkin_ws/settings/pid.yml";

    cout<<path_points<<endl;
    FileStorage fs2( path_points, FileStorage::READ);

    for ( int i = 0 ; i < motor_list.size() ; i++ )
    {
        string pp = motor_list.at(i).name + "P";
        string ii = motor_list.at(i).name + "I";
        string dd = motor_list.at(i).name + "D";

        int ppv = (int)fs2[pp];
        int iiv = (int)fs2[ii];
        int ddv = (int)fs2[dd];

        motor_list.at(i).p = ppv;
        motor_list.at(i).i = iiv;
        motor_list.at(i).d = ddv;

        //read
        //cout<<motor_list.at(i).name<<" - "<<motor_list.at(i).p<<" "<<motor_list.at(i).i<<" "<<motor_list.at(i).d<<endl;
    }

    fs2.release();
}

void reset_temp_values()
{
    for ( int i = 0 ; i < left_motor_count ; i++)
    {
        sp_Motortemp_left[i] = -1;
        g_Motortemp_left[i] = -1;
    }

    for ( int i = 0 ; i < right_motor_count ; i++)
    {
        sp_Motortemp_right[i] = -1;
        g_Motortemp_right[i] = -1;
    }

    for ( int i = 0 ; i < head_motor_count ; i++)
    {
        sp_Motortemp_head[i] = -1;
        g_Motortemp_head[i] = -1;
    }
}

void init_motors()
{
    //Init Motors

    reset_temp_values();

    for ( int i = 0 ; i < right_motor_count ; i++ )
    {
        g_Motor_right[i] =  motorconfig_list.at(i).init;
        sp_Motor_right[i] = motorconfig_list.at(i).speed;
    }

    for ( int i = 0 ; i < left_motor_count ; i++ )
    {
        g_Motor_left[i] =  motorconfig_list.at(i + right_motor_count).init;
        sp_Motor_left[i] = motorconfig_list.at(i + right_motor_count).speed;
    }

    for ( int i = 0 ; i < head_motor_count ; i++ )
    {
        g_Motor_head[i] =  motorconfig_list.at(i + right_motor_count + left_motor_count).init;
        sp_Motor_head[i] = motorconfig_list.at(i + right_motor_count + left_motor_count).speed;
    }

    //------------------------------------------------------------------------------------
    //Add motors list [22 motors] [empty field] [we should wait for real motors to fill this list]

    for ( int i = 0 ; i < motorconfig_list.size() ; i++ )
    {
        motor_data motor;
        motor.speed = -1;
        motor.position = -1;
        motor.load = -1;
        motor.voltage = -1;
        motor.id = motorconfig_list.at(i).id;
        motor.temp = -1;
        motor.name = motorconfig_list.at(i).name;
        motor.status = "not found";
        motor.min = motorconfig_list.at(i).min;
        motor.max = motorconfig_list.at(i).max;
        motor.init = motorconfig_list.at(i).init;
        motor.model = motorconfig_list.at(i).model;
        motor.p = 0;
        motor.i = 0;
        motor.d = 0;
        motor_list.push_back(motor);
    }

   // load_params_pid();

}

void init_config()
{
    ros::NodeHandle node;
    motor_config motorconfig;
    std::string arg;
    //Right Motors
    //1
    node.getParam("/right1_controller/motor/id",motorconfig.id);
    node.getParam("/right1_controller/joint_name",motorconfig.name);motorconfig.name = "right1";
    node.getParam("/right1_controller/motor/init",motorconfig.init);
    node.getParam("/right1_controller/motor/max",motorconfig.max);
    node.getParam("/right1_controller/motor/min",motorconfig.min);
    node.getParam("/right1_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //5
    node.getParam("/right2_controller/motor/id",motorconfig.id);
    node.getParam("/right2_controller/joint_name",motorconfig.name);motorconfig.name = "right2";
    node.getParam("/right2_controller/motor/init",motorconfig.init);
    node.getParam("/right2_controller/motor/max",motorconfig.max);
    node.getParam("/right2_controller/motor/min",motorconfig.min);
    node.getParam("/right2_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //6
    node.getParam("/right3_controller/motor/id",motorconfig.id);
    node.getParam("/right3_controller/joint_name",motorconfig.name);motorconfig.name = "right3";
    node.getParam("/right3_controller/motor/init",motorconfig.init);
    node.getParam("/right3_controller/motor/max",motorconfig.max);
    node.getParam("/right3_controller/motor/min",motorconfig.min);
    node.getParam("/right3_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //7
    node.getParam("/right4_controller/motor/id",motorconfig.id);
    node.getParam("/right4_controller/joint_name",motorconfig.name);motorconfig.name = "right4";
    node.getParam("/right4_controller/motor/init",motorconfig.init);
    node.getParam("/right4_controller/motor/max",motorconfig.max);
    node.getParam("right4_controller/motor/min",motorconfig.min);
    node.getParam("right4_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //8
    node.getParam("/right5_controller/motor/id",motorconfig.id);
    node.getParam("/right5_controller/joint_name",motorconfig.name);motorconfig.name = "right5";
    node.getParam("/right5_controller/motor/init",motorconfig.init);
    node.getParam("/right5_controller/motor/max",motorconfig.max);
    node.getParam("/right5_controller/motor/min",motorconfig.min);
    node.getParam("/right5_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";

    node.getParam("/right6_controller/motor/id",motorconfig.id);
    node.getParam("/right6_controller/joint_name",motorconfig.name);motorconfig.name = "right6";
    node.getParam("/right6_controller/motor/init",motorconfig.init);
    node.getParam("/right6_controller/motor/max",motorconfig.max);
    node.getParam("/right6_controller/motor/min",motorconfig.min);
    node.getParam("/right6_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";

    //-----------------------------------------------------------------------------
    //Left

    node.getParam("/left1_controller/motor/id",motorconfig.id);
    node.getParam("/left1_controller/joint_name",motorconfig.name);motorconfig.name = "left1";
    node.getParam("/left1_controller/motor/init",motorconfig.init);
    node.getParam("/left1_controller/motor/max",motorconfig.max);
    node.getParam("/left1_controller/motor/min",motorconfig.min);
    node.getParam("/left1_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //5
    node.getParam("/left2_controller/motor/id",motorconfig.id);
    node.getParam("/left2_controller/joint_name",motorconfig.name);motorconfig.name = "left2";
    node.getParam("/left2_controller/motor/init",motorconfig.init);
    node.getParam("/left2_controller/motor/max",motorconfig.max);
    node.getParam("/left2_controller/motor/min",motorconfig.min);
    node.getParam("/left2_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //6
    node.getParam("/left3_controller/motor/id",motorconfig.id);
    node.getParam("/left3_controller/joint_name",motorconfig.name);motorconfig.name = "left3";
    node.getParam("/left3_controller/motor/init",motorconfig.init);
    node.getParam("/left3_controller/motor/max",motorconfig.max);
    node.getParam("/left3_controller/motor/min",motorconfig.min);
    node.getParam("/left3_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //7
    node.getParam("/left4_controller/motor/id",motorconfig.id);
    node.getParam("/left4_controller/joint_name",motorconfig.name);motorconfig.name = "left4";
    node.getParam("/left4_controller/motor/init",motorconfig.init);
    node.getParam("/left4_controller/motor/max",motorconfig.max);
    node.getParam("left4_controller/motor/min",motorconfig.min);
    node.getParam("left4_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //8
    node.getParam("/left5_controller/motor/id",motorconfig.id);
    node.getParam("/left5_controller/joint_name",motorconfig.name);motorconfig.name = "left5";
    node.getParam("/left5_controller/motor/init",motorconfig.init);
    node.getParam("/left5_controller/motor/max",motorconfig.max);
    node.getParam("/left5_controller/motor/min",motorconfig.min);
    node.getParam("/left5_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";

    node.getParam("/left6_controller/motor/id",motorconfig.id);
    node.getParam("/left6_controller/joint_name",motorconfig.name);motorconfig.name = "left6";
    node.getParam("/left6_controller/motor/init",motorconfig.init);
    node.getParam("/left6_controller/motor/max",motorconfig.max);
    node.getParam("/left6_controller/motor/min",motorconfig.min);
    node.getParam("/left6_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";

    //-----------------------------------------------------------------------------
    //Head Motors
    //0
    node.getParam("/headYaw_controller/motor/id",motorconfig.id);
    node.getParam("/headYaw_controller/joint_name",motorconfig.name);motorconfig.name = "headYaw";
    node.getParam("/headYaw_controller/motor/init",motorconfig.init);
    node.getParam("/headYaw_controller/motor/max",motorconfig.max);
    node.getParam("/headYaw_controller/motor/min",motorconfig.min);
    node.getParam("/headYaw_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //1
    node.getParam("/headPitch_controller/motor/id",motorconfig.id);
    node.getParam("/headPitch_controller/joint_name",motorconfig.name);motorconfig.name = "headPitch";
    node.getParam("/headPitch_controller/motor/init",motorconfig.init);
    node.getParam("/headPitch_controller/motor/max",motorconfig.max);
    node.getParam("/headPitch_controller/motor/min",motorconfig.min);
    node.getParam("/headPitch_controller/joint_speed",motorconfig.speed);
    arg = "/dynamixel/dx_port/" + boost::lexical_cast<std::string>(motorconfig.id) + "/model_name";
    node.getParam(arg,motorconfig.model);
    if (motorconfig.model == "") motorconfig.model = "none";
    motorconfig.speed *= 10;
    motorconfig_list.push_back(motorconfig);
    motorconfig.name = "";
    //----------------------------------------------------------------------------------

    init_motors();

}

motor_config get_motorconfig(int id)
{
    motor_config empty;
    empty.name = "none";
    empty.init = -1;
    empty.max = -1;
    empty.min = -1;
    empty.id = -1;

    for ( int i = 0 ; i < motorconfig_list.size() ; i++ )
    {
        if ( motorconfig_list.at(i).id == id )
            return motorconfig_list.at(i);
    }

    return empty;
}

motor_config get_motorconfig(string name)
{
    motor_config empty;
    empty.name = "none";
    empty.init = -1;
    empty.max = -1;
    empty.min = -1;
    empty.id = -1;

    for ( int i = 0 ; i < motorconfig_list.size() ; i++ )
    {
        if ( motorconfig_list.at(i).name == name )
            return motorconfig_list.at(i);
    }

    return empty;
}

int getindex_byid(int id)
{
    for ( int i = 0 ; i < motor_list.size() ; i++ )
    {
        if ( motor_list.at(i).id == id)
            return i;
    }

    return -1;
}

void chatterCallbackw(const dynamixel_msgs::MotorStateList::ConstPtr &msg)
{

    //motor connection established
    motor_connected = true;
    ismotorthere_counter = 0;

    for ( int i = 0 ; i < motor_list.size() ; i++)
    {
        motor_list.at(i).status = "not found";
    }
    for ( int i = 0 ; i < msg->motor_states.size() ; i++ )
    {
        int get_id = msg->motor_states[i].id;
        int index = getindex_byid(get_id);
        if ( index == -1 ) continue;

        motor_list.at(index).speed = msg->motor_states[i].speed / 10;
        motor_list.at(index).position = msg->motor_states[i].position;
        motor_list.at(index).load = msg->motor_states[i].load * 1000;
        motor_list.at(index).voltage = msg->motor_states[i].voltage;
        motor_list.at(index).id = msg->motor_states[i].id;
        motor_list.at(index).temp = msg->motor_states[i].temperature;
        motor_list.at(index).status = "ready";
    }

    //============================ output with same Hz

}

void publish_feedbacks()
{

    if ( ismotorthere_counter < 20 )
    {
        ismotorthere_counter++;
    }
    else
    {
        cout<<"not found counter overflow"<<endl;

        for ( int i = 0 ; i < motor_list.size() ; i++)
        {
            motor_list.at(i).status = "not found";
        }

        //motor connection lost
        motor_connected = false;
        system_state = 0;
    }

    sepanta_msgs::upperbodymotorsfeedback feedback_list;

    //TODO1 check for name and index
    for ( int i = 0 ; i < motor_list.size() ; i++)
    {
        sepanta_msgs::motorfeedback feedback;
        feedback.id = motor_list.at(i).id;
        feedback.load = motor_list.at(i).load;
        feedback.speed = motor_list.at(i).speed;
        feedback.position = motor_list.at(i).position;
        feedback.voltage = motor_list.at(i).voltage;
        feedback.temp = motor_list.at(i).temp;
        feedback.name = motor_list.at(i).name;
        feedback.status = motor_list.at(i).status;
        feedback.min = motor_list.at(i).min;
        feedback.max = motor_list.at(i).max;
        feedback.init = motor_list.at(i).init;
        feedback.model = motor_list.at(i).model;
        feedback.p = motor_list.at(i).p;
        feedback.i = motor_list.at(i).i;
        feedback.d = motor_list.at(i).d;

        feedback_list.motorfeedbacks.push_back(feedback);
    }

    chatter_pub_motors.publish(feedback_list);
}

void chatterCallback_right_motors(const sepanta_msgs::upperbodymotors::ConstPtr &msg)
{
    g_Motor_right[0] = msg->position1;
    g_Motor_right[1] = msg->position2;
    g_Motor_right[2] = msg->position3;
    g_Motor_right[3] = msg->position4;
    g_Motor_right[4] = msg->position5;
    //==============================================
    sp_Motor_right[0] = msg->speed1;
    sp_Motor_right[1] = msg->speed2;
    sp_Motor_right[2] = msg->speed3;
    sp_Motor_right[3] = msg->speed4;
    sp_Motor_right[4] = msg->speed5;
}

void chatterCallback_left_motors(const sepanta_msgs::upperbodymotors::ConstPtr &msg)
{
    g_Motor_left[0] = msg->position1;
    g_Motor_left[1] = msg->position2;
    g_Motor_left[2] = msg->position3;
    g_Motor_left[3] = msg->position4;
    g_Motor_left[4] = msg->position5;
    //==============================================
    sp_Motor_left[0] = msg->speed1;
    sp_Motor_left[1] = msg->speed2;
    sp_Motor_left[2] = msg->speed3;
    sp_Motor_left[3] = msg->speed4;
    sp_Motor_left[4] = msg->speed5;
}

void chatterCallback_head_motors(const sepanta_msgs::upperbodymotors::ConstPtr &msg)
{
    g_Motor_head[0] = msg->head_yaw_position;     //left_shoulder_base1 ?
    g_Motor_head[1] = msg->head_pitch_position;     //left_shoulder_base2 ?
    sp_Motor_head[0] = msg->head_yaw_speed;     //left_shoulder_base1 ?
    sp_Motor_head[1] = msg->head_pitch_speed;     //left_shoulder_base2 ?
}

void set_pid(int index ,int margin ,  int slope ,int punch )
{
    dynamixel_controllers::SetComplianceMargin srv_margin;
    dynamixel_controllers::SetComplianceSlope srv_slope;
    dynamixel_controllers::SetCompliancePunch srv_punch;

    dynamixel_controllers::SetCompliancep srv_p;
    dynamixel_controllers::SetCompliancei srv_i;
    dynamixel_controllers::SetComplianced srv_d;

    string model = motor_list.at(index).model;
    if ( model.at(0) == 'A')
    {
        srv_margin.request.margin = margin;
        srv_slope.request.slope = slope;
        srv_punch.request.punch = punch;
        service_margin[index].call(srv_margin);
        service_slope[index].call(srv_slope);
        service_punch[index].call(srv_punch);
    }
    else
    {
        srv_i.request.margin = margin;
        srv_p.request.slope = slope;
        srv_d.request.punch = punch;
        service_i[index].call(srv_i);
        service_p[index].call(srv_p);
        service_d[index].call(srv_d);
    }

    motor_list.at(index).p = slope;
    motor_list.at(index).i = margin;
    motor_list.at(index).d = punch;

    //save_params_pid();
}

void Motor_Update()
{
     if ( robot_init == true && pid_init == false )
     {
         pid_init = true;
         for (int i = 0; i < motor_list.size() ; i++)
         {
             motor_data item = motor_list.at(i);

                 if ( item.status == "ready" )
                 {
                     set_pid(i,0,16,0);
                     cout<<"SET PID : ["<<i<<"] "<<item.p<<" "<<item.i<<" "<<item.d<<endl;
                     //boost::this_thread::sleep(boost::posix_time::milliseconds(100));
                 }

         }
     }
    //-----------------------------------------------------
    //right motors update
    for (int i = 0; i < right_motor_count; i++)
    {

            if ( g_Motortemp_right[i] != g_Motor_right[i] && robot_init == true)
            {
                g_Motortemp_right[i] = g_Motor_right[i];

                std_msgs::Int32 msg;
                msg.data = g_Motor_right[i];

                if ( motor_list.at(i).status != "not found")
                    chatter_pub_motor_right[i].publish(msg);

                //cout<<"send right "<<i<<" "<<g_Motor_right[i]<<endl;

            }

    }

    //left motors update
    for (int i = 0; i < left_motor_count; i++)
    {

            if ( g_Motortemp_left[i] != g_Motor_left[i] && robot_init == true)
            {
                g_Motortemp_left[i] = g_Motor_left[i];

                std_msgs::Int32 msg;
                msg.data = g_Motor_left[i];

                if ( motor_list.at(i + right_motor_count).status != "not found")
                    chatter_pub_motor_left[i].publish(msg);

                //cout<<"send left"<<i<<" "<<g_Motor_left[i]<<endl;

            }

    }

    //head motors update
    for (int i = 0; i < head_motor_count; i++)
    {
        if ( g_Motortemp_head[i] != g_Motor_head[i] && robot_init == true)
        {
            g_Motortemp_head[i] = g_Motor_head[i];

            std_msgs::Int32 msg;
            msg.data = g_Motor_head[i];

            if ( motor_list.at(i + right_motor_count + left_motor_count).status != "not found")
                chatter_pub_motor_head[i].publish(msg);

            //cout<<"send head"<<i<<" "<<g_Motor_head[i]<<endl;

        }
    }
}

void Motor_Update_speed()
{

    if ( motor_connected && system_state == 0) //state is 0 , wait for motors to connect...
    {
       system_state = 1;
       init_counter = 0;
       robot_init = false;
       pid_init = false;
       reset_temp_values();
    }

   if ( system_state == 0 ) return;

   if ( init_counter < 25)
       init_counter++;
   else
       robot_init = true;

    for (int i = 0; i < right_motor_count; i++)
    {

            float value = 0;
            if ( sp_Motortemp_right[i] != sp_Motor_right[i] && robot_init == true)
            {
                sp_Motortemp_right[i] = sp_Motor_right[i];

                value = ((float)sp_Motor_right[i]) / 1000;
                dynamixel_controllers::SetSpeed speed;
                speed.request.speed = value;

                if ( motor_list.at(i).status != "not found")
                    service_speed_motor_right[i].call(speed);

                // cout<<"speed right "<<i<<" "<<value<<endl;
            }

    }

    for (int i = 0; i < left_motor_count; i++)
    {
            float value = 0;
            if ( sp_Motortemp_left[i] != sp_Motor_left[i] && robot_init == true)
            {
                sp_Motortemp_left[i] = sp_Motor_left[i];

                value = ((float)sp_Motor_left[i]) / 1000;
                dynamixel_controllers::SetSpeed speed;
                speed.request.speed = value;

                if ( motor_list.at(i + right_motor_count).status != "not found")
                    service_speed_motor_left[i].call(speed);

                //cout<<"speed left "<<i<<" "<<value<<endl;
            }
    }

    for (int i = 0; i < head_motor_count; i++)
    {
        float value = 0;
        if ( sp_Motortemp_head[i] != sp_Motor_head[i] && robot_init == true)
        {
            sp_Motortemp_head[i] = sp_Motor_head[i];

            value = ((float)sp_Motor_head[i]) / 1000;
            dynamixel_controllers::SetSpeed speed;
            speed.request.speed = value;

            if ( motor_list.at(i + right_motor_count + left_motor_count).status != "not found")
            {
                service_speed_motor_head[i].call(speed);

            }

             //cout<<"speed head "<<i<<" "<<value<<endl;
        }

    }
}

void torque_toggle(bool value)
{
    for ( int i = 0 ; i < right_motor_count ; i++ )
    {
        dynamixel_controllers::TorqueEnable t_srv;
        t_srv.request.torque_enable = value;

        if ( motor_list.at(i).status != "not found")
            service_torque_motor_right[i].call(t_srv);
    }

    for ( int i = 0 ; i < left_motor_count ; i++ )
    {
        dynamixel_controllers::TorqueEnable t_srv;
        t_srv.request.torque_enable = value;

        if ( motor_list.at(i + right_motor_count).status != "not found")
            service_torque_motor_left[i].call(t_srv);
    }

    for ( int i = 0 ; i < head_motor_count ; i++ )
    {
        dynamixel_controllers::TorqueEnable t_srv;
        t_srv.request.torque_enable = value;

        if ( motor_list.at(i + right_motor_count + left_motor_count).status != "not found")
            service_torque_motor_head[i].call(t_srv);
    }
}

bool callback_resetmotor(sepanta_msgs::motorreset::Request& request, sepanta_msgs::motorreset::Response& response)
{

    send_log("get [reset] request : " + request.id );

    if ( request.id  == "right1" ) reset_right_1();
    if ( request.id  == "right2" ) reset_right_2();
    if ( request.id  == "right3") reset_right_3();
    if ( request.id  == "right4" ) reset_right_4();
    if ( request.id  == "right5") reset_right_5();
    if ( request.id  == "right6") reset_right_gripper();
    if ( request.id  == "rightArm") reset_right_arm();
    if ( request.id  == "left1" ) reset_left_1();
    if ( request.id  == "left2" ) reset_left_2();
    if ( request.id  == "left3") reset_left_3();
    if ( request.id  == "left4" ) reset_left_4();
    if ( request.id  == "left5") reset_left_5();
    if ( request.id  == "left6") reset_left_gripper();
    if ( request.id  == "leftArm") reset_left_arm();
    if ( request.id  == "headYaw" ) reset_head_yaw();
    if ( request.id  == "headPitch" ) reset_head_pitch();

    return true;
}

bool callback_pidmotor(sepanta_msgs::motorpid::Request& request, sepanta_msgs::motorpid::Response& response)
{
    send_log("get [pid] request : " + request.id );

    uint8_t margin = request.margin;
    uint8_t slope  = request.slope;
    uint8_t punch = request.punch;

    // if ( request.id  == "right1" ) set_pid(0,margin,slope,punch);
    // if ( request.id  == "right2" ) set_pid(1,margin,slope,punch);
    // if ( request.id  == "right3") set_pid(2,margin,slope,punch);
    // if ( request.id  == "right4" ) set_pid(3,margin,slope,punch);
    // if ( request.id  == "right5") set_pid(4,margin,slope,punch);
    // if ( request.id  == "right6") set_pid(5,margin,slope,punch);
    // if ( request.id  == "left1" ) set_pid(6,margin,slope,punch);
    // if ( request.id  == "left2" ) set_pid(7,margin,slope,punch);
    // if ( request.id  == "left3") set_pid(8,margin,slope,punch);
    // if ( request.id  == "left4" ) set_pid(9,margin,slope,punch);
    // if ( request.id  == "left5") set_pid(10,margin,slope,punch);
    // if ( request.id  == "left6") set_pid(11,margin,slope,punch);
    // if ( request.id  == "headYaw" ) set_pid(12,margin,slope,punch);
    // if ( request.id  == "headPitch" )set_pid(13,margin,slope,punch);

    return true;
}

bool callback_torquemotor(sepanta_msgs::motortorque::Request& request, sepanta_msgs::motortorque::Response& response)
{
    int a = 0;
    if ( request.status == true) a = 1;

    string v = "False";
    if ( a == 1 )
        v = "True";

    send_log("get [torque] request : " + v );

    torque_toggle(request.status);

    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "upperbody_core");
    cout << "upperBody core started done..." << endl;

    ros::NodeHandle node_handles[100];
    ros::Subscriber sub_handles[50];

    //-----------------------------------------------------------------------------------------
    sub_handles[0] = node_handles[10].subscribe("/motor_states/dx_port", 10, chatterCallbackw);
    sub_handles[1] = node_handles[10].subscribe("upperbodycorein_right_motors", 10, chatterCallback_right_motors); //all right motors
    sub_handles[2] = node_handles[10].subscribe("upperbodycorein_left_motors", 10, chatterCallback_left_motors); //all left motors
    sub_handles[3] = node_handles[10].subscribe("upperbodycorein_head_motors", 10, chatterCallback_head_motors); //all head motors
    sub_handles[4] = node_handles[10].subscribe("upperbodycorein_right_1", 10, chatterCallback_right_1);
    sub_handles[5] = node_handles[10].subscribe("upperbodycorein_right_2", 10, chatterCallback_right_2);
    sub_handles[6] = node_handles[10].subscribe("upperbodycorein_right_3", 10, chatterCallback_right_3);
    sub_handles[7] = node_handles[10].subscribe("upperbodycorein_right_4", 10, chatterCallback_right_4);
    sub_handles[8] = node_handles[10].subscribe("upperbodycorein_right_5", 10, chatterCallback_right_5);
    sub_handles[9] = node_handles[10].subscribe("upperbodycorein_right_6", 10, chatterCallback_right_gripper);
    sub_handles[10] = node_handles[10].subscribe("upperbodycorein_left_1", 10, chatterCallback_left_1);
    sub_handles[11] = node_handles[10].subscribe("upperbodycorein_left_2", 10, chatterCallback_left_2);
    sub_handles[12] = node_handles[10].subscribe("upperbodycorein_left_3", 10, chatterCallback_left_3);
    sub_handles[13] = node_handles[10].subscribe("upperbodycorein_left_4", 10, chatterCallback_left_4);
    sub_handles[14] = node_handles[10].subscribe("upperbodycorein_left_5", 10, chatterCallback_left_5);
    sub_handles[15] = node_handles[10].subscribe("upperbodycorein_left_6", 10, chatterCallback_left_gripper);
    sub_handles[16] = node_handles[10].subscribe("upperbodycorein_head_yaw", 10, chatterCallback_head_yaw);
    sub_handles[17] = node_handles[10].subscribe("upperbodycorein_head_pitch", 10, chatterCallback_head_pitch);

    //=================================================================================================================

    chatter_pub_motors = node_handles[5].advertise<sepanta_msgs::upperbodymotorsfeedback>("/upperbodycoreout_feedback", 10);

    chatter_pub_motor_right[0] = node_handles[5].advertise<std_msgs::Int32>("/right1_controller/command", 10);
    chatter_pub_motor_right[1] = node_handles[5].advertise<std_msgs::Int32>("/right2_controller/command", 10);
    chatter_pub_motor_right[2] = node_handles[5].advertise<std_msgs::Int32>("/right3_controller/command", 10);
    chatter_pub_motor_right[3] = node_handles[5].advertise<std_msgs::Int32>("/right4_controller/command", 10);
    chatter_pub_motor_right[4] = node_handles[5].advertise<std_msgs::Int32>("/right5_controller/command", 10);
    chatter_pub_motor_right[5] = node_handles[5].advertise<std_msgs::Int32>("/right6_controller/command", 10);
    chatter_pub_motor_left[0] = node_handles[5].advertise<std_msgs::Int32>("/left1_controller/command", 10);
    chatter_pub_motor_left[1] = node_handles[5].advertise<std_msgs::Int32>("/left2_controller/command", 10);
    chatter_pub_motor_left[2] = node_handles[5].advertise<std_msgs::Int32>("/left3_controller/command", 10);
    chatter_pub_motor_left[3] = node_handles[5].advertise<std_msgs::Int32>("/left4_controller/command", 10);
    chatter_pub_motor_left[4] = node_handles[5].advertise<std_msgs::Int32>("/left5_controller/command", 10);
    chatter_pub_motor_left[5] = node_handles[5].advertise<std_msgs::Int32>("/left6_controller/command", 10);
    chatter_pub_motor_head[0] = node_handles[5].advertise<std_msgs::Int32>("/headYaw_controller/command", 10);
    chatter_pub_motor_head[1] = node_handles[5].advertise<std_msgs::Int32>("/headPitch_controller/command", 10);

    chatter_pub_ack = node_handles[5].advertise<std_msgs::String>("/core_upperbody/ack",10);
    chatter_pub_log = node_handles[5].advertise<std_msgs::String>("/core_upperbody/log",10);
    //========================================================================================================
    //speed service clients

    service_speed_motor_right[0] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right1_controller/set_speed");
    service_speed_motor_right[1] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right2_controller/set_speed");
    service_speed_motor_right[2] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right3_controller/set_speed");
    service_speed_motor_right[3] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right4_controller/set_speed");
    service_speed_motor_right[4] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right5_controller/set_speed");
    service_speed_motor_right[5] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/right6_controller/set_speed");
    service_speed_motor_left[0] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left1_controller/set_speed");
    service_speed_motor_left[1] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left2_controller/set_speed");
    service_speed_motor_left[2] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left3_controller/set_speed");
    service_speed_motor_left[3] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left4_controller/set_speed");
    service_speed_motor_left[4] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left5_controller/set_speed");
    service_speed_motor_left[5] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/left6_controller/set_speed");
    service_speed_motor_head[0] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/headYaw_controller/set_speed");
    service_speed_motor_head[1] = node_handles[2].serviceClient<dynamixel_controllers::SetSpeed>("/headPitch_controller/set_speed");

    service_torque_motor_right[0] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right1_controller/torque_enable");
    service_torque_motor_right[1] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right2_controller/torque_enable");
    service_torque_motor_right[2] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right3_controller/torque_enable");
    service_torque_motor_right[3] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right4_controller/torque_enable");
    service_torque_motor_right[4] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right5_controller/torque_enable");
    service_torque_motor_right[5] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/right6_controller/torque_enable");
    service_torque_motor_left[0] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left1_controller/torque_enable");
    service_torque_motor_left[1] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left2_controller/torque_enable");
    service_torque_motor_left[2] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left3_controller/torque_enable");
    service_torque_motor_left[3] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left4_controller/torque_enable");
    service_torque_motor_left[4] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left5_controller/torque_enable");
    service_torque_motor_left[5] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/left6_controller/torque_enable");
    service_torque_motor_head[0] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/headYaw_controller/torque_enable");
    service_torque_motor_head[1] = node_handles[3].serviceClient<dynamixel_controllers::TorqueEnable>("/headPitch_controller/torque_enable");

    //========================================================================================================= PID

    service_margin[0] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right1_controller/set_compliance_margin");
    service_margin[1] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right2_controller/set_compliance_margin");
    service_margin[2] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right3_controller/set_compliance_margin");
    service_margin[3] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right4_controller/set_compliance_margin");
    service_margin[4] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right5_controller/set_compliance_margin");
    service_margin[5] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/right6_controller/set_compliance_margin");
    service_margin[6] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left1_controller/set_compliance_margin");
    service_margin[7] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left2_controller/set_compliance_margin");
    service_margin[8] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left3_controller/set_compliance_margin");
    service_margin[9] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left4_controller/set_compliance_margin");
    service_margin[10] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left5_controller/set_compliance_margin");
    service_margin[11] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/left6_controller/set_compliance_margin");
    service_margin[12] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/headYaw_controller/set_compliance_margin");
    service_margin[13] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceMargin>("/headPitch_controller/set_compliance_margin");

    service_punch[0] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right1_controller/set_compliance_punch");
    service_punch[1] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right2_controller/set_compliance_punch");
    service_punch[2] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right3_controller/set_compliance_punch");
    service_punch[3] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right4_controller/set_compliance_punch");
    service_punch[4] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right5_controller/set_compliance_punch");
    service_punch[5] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/right6_controller/set_compliance_punch");
    service_punch[6] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left1_controller/set_compliance_punch");
    service_punch[7] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left2_controller/set_compliance_punch");
    service_punch[8] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left3_controller/set_compliance_punch");
    service_punch[9] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left4_controller/set_compliance_punch");
    service_punch[10] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left5_controller/set_compliance_punch");
    service_punch[11] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/left6_controller/set_compliance_punch");
    service_punch[12] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/headYaw_controller/set_compliance_punch");
    service_punch[13] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancePunch>("/headPitch_controller/set_compliance_punch");

    service_slope[0] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right1_controller/set_compliance_slope");
    service_slope[1] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right2_controller/set_compliance_slope");
    service_slope[2] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right3_controller/set_compliance_slope");
    service_slope[3] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right4_controller/set_compliance_slope");
    service_slope[4] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right5_controller/set_compliance_slope");
    service_slope[5] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/right6_controller/set_compliance_slope");
    service_slope[6] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left1_controller/set_compliance_slope");
    service_slope[7] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left2_controller/set_compliance_slope");
    service_slope[8] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left3_controller/set_compliance_slope");
    service_slope[9] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left4_controller/set_compliance_slope");
    service_slope[10] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left5_controller/set_compliance_slope");
    service_slope[11] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/left6_controller/set_compliance_slope");
    service_slope[12] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/headYaw_controller/set_compliance_slope");
    service_slope[13] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianceSlope>("/headPitch_controller/set_compliance_slope");

    //==========================================================================================================

    service_p[0] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right1_controller/set_compliance_p");
    service_p[1] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right2_controller/set_compliance_p");
    service_p[2] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right3_controller/set_compliance_p");
    service_p[3] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right4_controller/set_compliance_p");
    service_p[4] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right5_controller/set_compliance_p");
    service_p[5] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/right6_controller/set_compliance_p");
    service_p[6] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left1_controller/set_compliance_p");
    service_p[7] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left2_controller/set_compliance_p");
    service_p[8] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left3_controller/set_compliance_p");
    service_p[9] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left4_controller/set_compliance_p");
    service_p[10] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left5_controller/set_compliance_p");
    service_p[11] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/left6_controller/set_compliance_p");
    service_p[12] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/headYaw_controller/set_compliance_p");
    service_p[13] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancep>("/headPitch_controller/set_compliance_p");

    service_i[0] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right1_controller/set_compliance_i");
    service_i[1] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right2_controller/set_compliance_i");
    service_i[2] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right3_controller/set_compliance_i");
    service_i[3] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right4_controller/set_compliance_i");
    service_i[4] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right5_controller/set_compliance_i");
    service_i[5] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/right6_controller/set_compliance_i");
    service_i[6] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left1_controller/set_compliance_i");
    service_i[7] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left2_controller/set_compliance_i");
    service_i[8] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left3_controller/set_compliance_i");
    service_i[9] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left4_controller/set_compliance_i");
    service_i[10] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left5_controller/set_compliance_i");
    service_i[11] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/left6_controller/set_compliance_i");
    service_i[12] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/headYaw_controller/set_compliance_i");
    service_i[13] = node_handles[4].serviceClient<dynamixel_controllers::SetCompliancei>("/headPitch_controller/set_compliance_i");

    service_d[0] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right1_controller/set_compliance_d");
    service_d[1] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right2_controller/set_compliance_d");
    service_d[2] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right3_controller/set_compliance_d");
    service_d[3] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right4_controller/set_compliance_d");
    service_d[4] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right5_controller/set_compliance_d");
    service_d[5] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/right6_controller/set_compliance_d");
    service_d[6] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left1_controller/set_compliance_d");
    service_d[7] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left2_controller/set_compliance_d");
    service_d[8] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left3_controller/set_compliance_d");
    service_d[9] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left4_controller/set_compliance_d");
    service_d[10] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left5_controller/set_compliance_d");
    service_d[11] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/left6_controller/set_compliance_d");
    service_d[12] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/headYaw_controller/set_compliance_d");
    service_d[13] = node_handles[4].serviceClient<dynamixel_controllers::SetComplianced>("/headPitch_controller/set_compliance_d");

    //==========================================================================================================

    service_reset_motor = node_handles[70].advertiseService("upperbodycorein_resetmotor", callback_resetmotor);
    service_torque_motor = node_handles[71].advertiseService("upperbodycorein_torquemotor", callback_torquemotor);
    service_pid_motor = node_handles[72].advertiseService("upperbodycorein_pidmotor",callback_pidmotor);

    //===========================================================================================================
    cout<<"wait finished 1"<<endl;

    ros::Rate loop_rate(20); //20 Hz
    ros::Rate loop_rate2(0.5); //20 Hz
    loop_rate2.sleep();

    init_config();

    cout<<"wait finished 2"<<endl;

    //send_log("upperBody core started done... [V1] [93/07/24]");
    while (ros::ok() && App_exit == false)
    {
        cout<<"loop"<<endl;
        publish_feedbacks();
        Motor_Update_speed();
        Motor_Update();
        ros::spinOnce();
        loop_rate.sleep();

        send_ack();
    }

    App_exit = true;
    return 0;
}


text:
#include <boost/stacktrace.hpp>

int main()
{
  boost::stacktrace::safe_dump_to("./backtrace.dump");
  return 0;
}

ext: cc