<commit_before>/*
Copyright 2016 Fixstars Corporation

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

#include <iostream>

#include <libsgm.h>

#include "internal.h"

namespace sgm {
	static bool is_cuda_input(EXECUTE_INOUT type) { return (int)type & 0x1; }
	static bool is_cuda_output(EXECUTE_INOUT type) { return (int)type & 0x2; }

	struct CudaStereoSGMResources {
		void* d_src_left;
		void* d_src_right;
		void* d_left;
		void* d_right;
		void* d_scost;
		void* d_matching_cost;
		void* d_left_disp;
		void* d_right_disp;

		void* d_tmp_left_disp;
		void* d_tmp_right_disp;

		cudaStream_t cuda_streams[8];

		void* d_output_16bit_buffer;
		uint16_t* h_output_16bit_buffer;

		CudaStereoSGMResources(int width_, int height_, int disparity_size_, int input_depth_bits_, int output_depth_bits_, EXECUTE_INOUT inout_type_) {

			if (is_cuda_input(inout_type_)) {
				this->d_src_left = NULL;
				this->d_src_right = NULL;
			}
			else {
				CudaSafeCall(cudaMalloc(&this->d_src_left, input_depth_bits_ / 8 * width_ * height_));
				CudaSafeCall(cudaMalloc(&this->d_src_right, input_depth_bits_ / 8 * width_ * height_));
			}

			CudaSafeCall(cudaMalloc(&this->d_left, sizeof(uint64_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_right, sizeof(uint64_t) * width_ * height_));

			CudaSafeCall(cudaMalloc(&this->d_matching_cost, sizeof(uint8_t) * width_ * height_ * disparity_size_));

			CudaSafeCall(cudaMalloc(&this->d_scost, sizeof(uint16_t) * width_ * height_ * disparity_size_));

			CudaSafeCall(cudaMalloc(&this->d_left_disp, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_right_disp, sizeof(uint16_t) * width_ * height_));

			CudaSafeCall(cudaMalloc(&this->d_tmp_left_disp, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_tmp_right_disp, sizeof(uint16_t) * width_ * height_));

			for (int i = 0; i < 8; i++) {
				CudaSafeCall(cudaStreamCreate(&this->cuda_streams[i]));
			}

			// create temporary buffer when dst type is 8bit host pointer
			if (!is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
				this->h_output_16bit_buffer = (uint16_t*)malloc(sizeof(uint16_t) * width_ * height_);
			}
			else {
				this->h_output_16bit_buffer = NULL;
			}
		}

		~CudaStereoSGMResources() {
			CudaSafeCall(cudaFree(this->d_src_left));
			CudaSafeCall(cudaFree(this->d_src_right));

			CudaSafeCall(cudaFree(this->d_left));
			CudaSafeCall(cudaFree(this->d_right));

			CudaSafeCall(cudaFree(this->d_matching_cost));

			CudaSafeCall(cudaFree(this->d_scost));

			CudaSafeCall(cudaFree(this->d_left_disp));
			CudaSafeCall(cudaFree(this->d_right_disp));

			CudaSafeCall(cudaFree(this->d_tmp_left_disp));
			CudaSafeCall(cudaFree(this->d_tmp_right_disp));

			for (int i = 0; i < 8; i++) {
				CudaSafeCall(cudaStreamDestroy(this->cuda_streams[i]));
			}

			free(h_output_16bit_buffer);
		}
	};

	StereoSGM::StereoSGM(int width, int height, int disparity_size, int input_depth_bits, int output_depth_bits, EXECUTE_INOUT inout_type) :
		width_(width),
		height_(height),
		disparity_size_(disparity_size),
		input_depth_bits_(input_depth_bits),
		output_depth_bits_(output_depth_bits),
		inout_type_(inout_type),
		cu_res_(NULL)
	{
		// check values
		if (width_ % 2 != 0 || height_ % 2 != 0) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("width and height must be even");
		}
		if (input_depth_bits_ != 8 && input_depth_bits_ != 16 && output_depth_bits_ != 8 && output_depth_bits_ != 16) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("depth bits must be 8 or 16");
		}
		if (disparity_size_ != 64 && disparity_size_ != 128) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("disparity size must be 64 or 128");
		}

		cu_res_ = new CudaStereoSGMResources(width_, height_, disparity_size_, input_depth_bits_, output_depth_bits_, inout_type_);
	}

	StereoSGM::~StereoSGM() {
		if (cu_res_) { delete cu_res_; }
	}

	void StereoSGM::execute(const void* left_pixels, const void* right_pixels, void** dst) {

		const void *d_input_left, *d_input_right;

		if (is_cuda_input(inout_type_)) {
			d_input_left = left_pixels;
			d_input_right = right_pixels;
		}
		else {
			CudaSafeCall(cudaMemcpy(cu_res_->d_src_left, left_pixels, input_depth_bits_ / 8 * width_ * height_, cudaMemcpyHostToDevice));
			CudaSafeCall(cudaMemcpy(cu_res_->d_src_right, right_pixels, input_depth_bits_ / 8 * width_ * height_, cudaMemcpyHostToDevice));
			d_input_left = cu_res_->d_src_left;
			d_input_right = cu_res_->d_src_right;
		}

		sgm::details::census(d_input_left, (uint64_t*)cu_res_->d_left, 9, 7, width_, height_, input_depth_bits_, cu_res_->cuda_streams[0]);
		sgm::details::census(d_input_right, (uint64_t*)cu_res_->d_right, 9, 7, width_, height_, input_depth_bits_, cu_res_->cuda_streams[1]);

		CudaSafeCall(cudaMemsetAsync(cu_res_->d_left_disp, 0, sizeof(uint16_t) * width_ * height_, cu_res_->cuda_streams[2]));
		CudaSafeCall(cudaMemsetAsync(cu_res_->d_right_disp, 0, sizeof(uint16_t) * width_ * height_, cu_res_->cuda_streams[3]));

		CudaSafeCall(cudaMemsetAsync(cu_res_->d_scost, 0, sizeof(uint16_t) * width_ * height_ * disparity_size_, cu_res_->cuda_streams[4]));

		sgm::details::matching_cost((const uint64_t*)cu_res_->d_left, (const uint64_t*)cu_res_->d_right, (uint8_t*)cu_res_->d_matching_cost, width_, height_, disparity_size_);

		sgm::details::scan_scost((const uint8_t*)cu_res_->d_matching_cost, (uint16_t*)cu_res_->d_scost, width_, height_, disparity_size_, cu_res_->cuda_streams);

		cudaStreamSynchronize(cu_res_->cuda_streams[2]);
		cudaStreamSynchronize(cu_res_->cuda_streams[3]);

		sgm::details::winner_takes_all((const uint16_t*)cu_res_->d_scost, (uint16_t*)cu_res_->d_left_disp, (uint16_t*)cu_res_->d_right_disp, width_, height_, disparity_size_);

		sgm::details::median_filter((uint16_t*)cu_res_->d_left_disp, (uint16_t*)cu_res_->d_tmp_left_disp, width_, height_);
		sgm::details::median_filter((uint16_t*)cu_res_->d_right_disp, (uint16_t*)cu_res_->d_tmp_right_disp, width_, height_);

		sgm::details::check_consistency((uint16_t*)cu_res_->d_tmp_left_disp, (uint16_t*)cu_res_->d_tmp_right_disp, d_input_left, width_, height_, input_depth_bits_);

		// output disparity image
		void* disparity_image = cu_res_->d_tmp_left_disp;

		if (!is_cuda_output(inout_type_) && output_depth_bits_ == 16) {
			CudaSafeCall(cudaMemcpy(*dst, disparity_image, sizeof(uint16_t) * width_ * height_, cudaMemcpyDeviceToHost));
		}
		else if (is_cuda_output(inout_type_) && output_depth_bits_ == 16) {
			*dst = disparity_image; // optimize! no-copy!
		}
		else if (!is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
			CudaSafeCall(cudaMemcpy(cu_res_->h_output_16bit_buffer, disparity_image, sizeof(uint16_t) * width_ * height_, cudaMemcpyDeviceToHost));
			for (int i = 0; i < width_ * height_; i++) { ((uint8_t*)*dst)[i] = (uint8_t)cu_res_->h_output_16bit_buffer[i]; }
		}
		else if (is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
			sgm::details::cast_16bit_8bit_array((const uint16_t*)disparity_image, (uint8_t*)*dst, width_ * height_);
		}
		else {
			std::cerr << "not impl" << std::endl;
		}
	}
}
<commit_msg>zero clear tmp_disp<commit_after>/*
Copyright 2016 Fixstars Corporation

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

#include <iostream>

#include <libsgm.h>

#include "internal.h"

namespace sgm {
	static bool is_cuda_input(EXECUTE_INOUT type) { return (int)type & 0x1; }
	static bool is_cuda_output(EXECUTE_INOUT type) { return (int)type & 0x2; }

	struct CudaStereoSGMResources {
		void* d_src_left;
		void* d_src_right;
		void* d_left;
		void* d_right;
		void* d_scost;
		void* d_matching_cost;
		void* d_left_disp;
		void* d_right_disp;

		void* d_tmp_left_disp;
		void* d_tmp_right_disp;

		cudaStream_t cuda_streams[8];

		void* d_output_16bit_buffer;
		uint16_t* h_output_16bit_buffer;

		CudaStereoSGMResources(int width_, int height_, int disparity_size_, int input_depth_bits_, int output_depth_bits_, EXECUTE_INOUT inout_type_) {

			if (is_cuda_input(inout_type_)) {
				this->d_src_left = NULL;
				this->d_src_right = NULL;
			}
			else {
				CudaSafeCall(cudaMalloc(&this->d_src_left, input_depth_bits_ / 8 * width_ * height_));
				CudaSafeCall(cudaMalloc(&this->d_src_right, input_depth_bits_ / 8 * width_ * height_));
			}

			CudaSafeCall(cudaMalloc(&this->d_left, sizeof(uint64_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_right, sizeof(uint64_t) * width_ * height_));

			CudaSafeCall(cudaMalloc(&this->d_matching_cost, sizeof(uint8_t) * width_ * height_ * disparity_size_));

			CudaSafeCall(cudaMalloc(&this->d_scost, sizeof(uint16_t) * width_ * height_ * disparity_size_));

			CudaSafeCall(cudaMalloc(&this->d_left_disp, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_right_disp, sizeof(uint16_t) * width_ * height_));

			CudaSafeCall(cudaMalloc(&this->d_tmp_left_disp, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMalloc(&this->d_tmp_right_disp, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMemset(&this->d_tmp_left_disp, 0, sizeof(uint16_t) * width_ * height_));
			CudaSafeCall(cudaMemset(&this->d_tmp_right_disp, 0, sizeof(uint16_t) * width_ * height_));

			for (int i = 0; i < 8; i++) {
				CudaSafeCall(cudaStreamCreate(&this->cuda_streams[i]));
			}

			// create temporary buffer when dst type is 8bit host pointer
			if (!is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
				this->h_output_16bit_buffer = (uint16_t*)malloc(sizeof(uint16_t) * width_ * height_);
			}
			else {
				this->h_output_16bit_buffer = NULL;
			}
		}

		~CudaStereoSGMResources() {
			CudaSafeCall(cudaFree(this->d_src_left));
			CudaSafeCall(cudaFree(this->d_src_right));

			CudaSafeCall(cudaFree(this->d_left));
			CudaSafeCall(cudaFree(this->d_right));

			CudaSafeCall(cudaFree(this->d_matching_cost));

			CudaSafeCall(cudaFree(this->d_scost));

			CudaSafeCall(cudaFree(this->d_left_disp));
			CudaSafeCall(cudaFree(this->d_right_disp));

			CudaSafeCall(cudaFree(this->d_tmp_left_disp));
			CudaSafeCall(cudaFree(this->d_tmp_right_disp));

			for (int i = 0; i < 8; i++) {
				CudaSafeCall(cudaStreamDestroy(this->cuda_streams[i]));
			}

			free(h_output_16bit_buffer);
		}
	};

	StereoSGM::StereoSGM(int width, int height, int disparity_size, int input_depth_bits, int output_depth_bits, EXECUTE_INOUT inout_type) :
		width_(width),
		height_(height),
		disparity_size_(disparity_size),
		input_depth_bits_(input_depth_bits),
		output_depth_bits_(output_depth_bits),
		inout_type_(inout_type),
		cu_res_(NULL)
	{
		// check values
		if (width_ % 2 != 0 || height_ % 2 != 0) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("width and height must be even");
		}
		if (input_depth_bits_ != 8 && input_depth_bits_ != 16 && output_depth_bits_ != 8 && output_depth_bits_ != 16) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("depth bits must be 8 or 16");
		}
		if (disparity_size_ != 64 && disparity_size_ != 128) {
			width_ = height_ = input_depth_bits_ = output_depth_bits_ = disparity_size_ = 0;
			throw std::runtime_error("disparity size must be 64 or 128");
		}

		cu_res_ = new CudaStereoSGMResources(width_, height_, disparity_size_, input_depth_bits_, output_depth_bits_, inout_type_);
	}

	StereoSGM::~StereoSGM() {
		if (cu_res_) { delete cu_res_; }
	}

	void StereoSGM::execute(const void* left_pixels, const void* right_pixels, void** dst) {

		const void *d_input_left, *d_input_right;

		if (is_cuda_input(inout_type_)) {
			d_input_left = left_pixels;
			d_input_right = right_pixels;
		}
		else {
			CudaSafeCall(cudaMemcpy(cu_res_->d_src_left, left_pixels, input_depth_bits_ / 8 * width_ * height_, cudaMemcpyHostToDevice));
			CudaSafeCall(cudaMemcpy(cu_res_->d_src_right, right_pixels, input_depth_bits_ / 8 * width_ * height_, cudaMemcpyHostToDevice));
			d_input_left = cu_res_->d_src_left;
			d_input_right = cu_res_->d_src_right;
		}

		sgm::details::census(d_input_left, (uint64_t*)cu_res_->d_left, 9, 7, width_, height_, input_depth_bits_, cu_res_->cuda_streams[0]);
		sgm::details::census(d_input_right, (uint64_t*)cu_res_->d_right, 9, 7, width_, height_, input_depth_bits_, cu_res_->cuda_streams[1]);

		CudaSafeCall(cudaMemsetAsync(cu_res_->d_left_disp, 0, sizeof(uint16_t) * width_ * height_, cu_res_->cuda_streams[2]));
		CudaSafeCall(cudaMemsetAsync(cu_res_->d_right_disp, 0, sizeof(uint16_t) * width_ * height_, cu_res_->cuda_streams[3]));

		CudaSafeCall(cudaMemsetAsync(cu_res_->d_scost, 0, sizeof(uint16_t) * width_ * height_ * disparity_size_, cu_res_->cuda_streams[4]));

		sgm::details::matching_cost((const uint64_t*)cu_res_->d_left, (const uint64_t*)cu_res_->d_right, (uint8_t*)cu_res_->d_matching_cost, width_, height_, disparity_size_);

		sgm::details::scan_scost((const uint8_t*)cu_res_->d_matching_cost, (uint16_t*)cu_res_->d_scost, width_, height_, disparity_size_, cu_res_->cuda_streams);

		cudaStreamSynchronize(cu_res_->cuda_streams[2]);
		cudaStreamSynchronize(cu_res_->cuda_streams[3]);

		sgm::details::winner_takes_all((const uint16_t*)cu_res_->d_scost, (uint16_t*)cu_res_->d_left_disp, (uint16_t*)cu_res_->d_right_disp, width_, height_, disparity_size_);

		sgm::details::median_filter((uint16_t*)cu_res_->d_left_disp, (uint16_t*)cu_res_->d_tmp_left_disp, width_, height_);
		sgm::details::median_filter((uint16_t*)cu_res_->d_right_disp, (uint16_t*)cu_res_->d_tmp_right_disp, width_, height_);

		sgm::details::check_consistency((uint16_t*)cu_res_->d_tmp_left_disp, (uint16_t*)cu_res_->d_tmp_right_disp, d_input_left, width_, height_, input_depth_bits_);

		// output disparity image
		void* disparity_image = cu_res_->d_tmp_left_disp;

		if (!is_cuda_output(inout_type_) && output_depth_bits_ == 16) {
			CudaSafeCall(cudaMemcpy(*dst, disparity_image, sizeof(uint16_t) * width_ * height_, cudaMemcpyDeviceToHost));
		}
		else if (is_cuda_output(inout_type_) && output_depth_bits_ == 16) {
			*dst = disparity_image; // optimize! no-copy!
		}
		else if (!is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
			CudaSafeCall(cudaMemcpy(cu_res_->h_output_16bit_buffer, disparity_image, sizeof(uint16_t) * width_ * height_, cudaMemcpyDeviceToHost));
			for (int i = 0; i < width_ * height_; i++) { ((uint8_t*)*dst)[i] = (uint8_t)cu_res_->h_output_16bit_buffer[i]; }
		}
		else if (is_cuda_output(inout_type_) && output_depth_bits_ == 8) {
			sgm::details::cast_16bit_8bit_array((const uint16_t*)disparity_image, (uint8_t*)*dst, width_ * height_);
		}
		else {
			std::cerr << "not impl" << std::endl;
		}
	}
}
<|endoftext|>


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

#include "telepathy-handler-application.h"

#include <QTimer>
#include <KCmdLineArgs>
#include <KDebug>

#include <TelepathyQt4/Types>
#include <TelepathyQt4/Debug>

extern bool kde_kdebug_enable_dbus_interface;

namespace KTelepathy {

namespace {
int s_tpqt4DebugArea;

static void tpDebugCallback(const QString &libraryName,
                            const QString &libraryVersion,
                            QtMsgType type,
                            const QString &msg)
{
    Q_UNUSED(libraryName)
    Q_UNUSED(libraryVersion)
    kDebugStream(type, s_tpqt4DebugArea, __FILE__, __LINE__, "Telepathy-Qt4") << qPrintable(msg);
}
}

class TelepathyHandlerApplication::Private
{
public:
    Private(TelepathyHandlerApplication *q);
    ~Private();

    void _k_onInitialTimeout();
    void _k_onTimeout();

    static KComponentData initHack();
    void init(int initialTimeout, int timeout);

    TelepathyHandlerApplication *q;

    static bool s_persist;
    static bool s_debug;

    int initialTimeout;
    int timeout;
    QTimer *timer;
    bool firstJobStarted;
    QAtomicInt jobCount;
};

TelepathyHandlerApplication::Private::Private(TelepathyHandlerApplication *q)
    : q(q),
      firstJobStarted(false),
      jobCount(0)
{
}

TelepathyHandlerApplication::Private::~Private()
{
}

void TelepathyHandlerApplication::Private::_k_onInitialTimeout()
{
    if (jobCount == 0 && jobCount.fetchAndAddOrdered(-1) == 0) {
        // m_jobCount is now -1
        kDebug() << "No job received. Exiting";
        QCoreApplication::quit();
    }
}

void TelepathyHandlerApplication::Private::_k_onTimeout()
{
    if (jobCount == 0 && jobCount.fetchAndAddOrdered(-1) == 0) {
        // m_jobCount is now -1
        kDebug() << "Timeout. Exiting";
        QCoreApplication::quit();
    }
}

// this gets called before even entering QApplication::QApplication()
KComponentData TelepathyHandlerApplication::Private::initHack()
{
    KComponentData cData(KCmdLineArgs::aboutData());
    KCmdLineOptions handler_options;
    handler_options.add("persist", ki18n("Persistent mode (do not exit on timeout)"));
    handler_options.add("debug", ki18n("Show Telepathy debugging information"));
    KCmdLineArgs::addCmdLineOptions(handler_options, ki18n("KDE Telepathy"), "kde-telepathy", "kde");
    KCmdLineArgs *args = KCmdLineArgs::parsedArgs("kde-telepathy");
    Private::s_persist = args->isSet("persist");
    Private::s_debug = args->isSet("debug");

    s_tpqt4DebugArea = KDebug::registerArea("Telepathy-Qt4");

    return cData;
}

void TelepathyHandlerApplication::Private::init(int initialTimeout, int timeout)
{
    this->initialTimeout = initialTimeout;
    this->timeout = timeout;

    // If timeout < 0 we let the application exit when the last window is closed,
    // Otherwise we handle it with the timeout
    if (timeout >= 0) {
        q->setQuitOnLastWindowClosed(false);
    }

    // Register TpQt4 types
    Tp::registerTypes();

    // Redirect Tp debug and warnings to KDebug output
    Tp::setDebugCallback(&tpDebugCallback);

    // Enable telepathy-Qt4 debug
    Tp::enableDebug(s_debug);
    Tp::enableWarnings(true);

    // Enable KDebug DBus interface
    // FIXME This must be enabled here because there is a bug in plasma
    //       it should be removed when this is fixed
    kde_kdebug_enable_dbus_interface = s_debug;

    if (!Private::s_persist) {
        timer = new QTimer(q);
        if (initialTimeout >= 0) {
            q->connect(timer, SIGNAL(timeout()), q, SLOT(_k_onInitialTimeout()));
            timer->start(initialTimeout);
        }
    }
}

bool TelepathyHandlerApplication::Private::s_persist = false;
bool TelepathyHandlerApplication::Private::s_debug = false;

TelepathyHandlerApplication::TelepathyHandlerApplication(bool GUIenabled,
                                                         int initialTimeout,
                                                         int timeout)
    : KApplication(GUIenabled, Private::initHack()),
      d(new Private(this))
{
    d->init(initialTimeout, timeout);
}

TelepathyHandlerApplication::TelepathyHandlerApplication(Display *display,
                                                         Qt::HANDLE visual,
                                                         Qt::HANDLE colormap,
                                                         int initialTimeout,
                                                         int timeout)
    : KApplication(display, visual, colormap, Private::initHack()),
      d(new Private(this))
{
    d->init(initialTimeout, timeout);
}

TelepathyHandlerApplication::~TelepathyHandlerApplication()
{
    delete d;
}

int TelepathyHandlerApplication::newJob()
{
    TelepathyHandlerApplication *app = qobject_cast<TelepathyHandlerApplication*>(KApplication::kApplication());
    TelepathyHandlerApplication::Private *d = app->d;

    int ret = d->jobCount.fetchAndAddOrdered(1);
    if (!Private::s_persist) {
        if (d->timer->isActive()) {
            d->timer->stop();
        }
        if (!d->firstJobStarted) {
            if (d->initialTimeout) {
                disconnect(d->timer, SIGNAL(timeout()), app, SLOT(_k_onInitialTimeout()));
            }
            if (d->timeout >= 0) {
                connect(d->timer, SIGNAL(timeout()), app, SLOT(_k_onTimeout()));
            }
            d->firstJobStarted = true;
        }
    }
    return ret;
}

void TelepathyHandlerApplication::jobFinished()
{
    TelepathyHandlerApplication *app = qobject_cast<TelepathyHandlerApplication*>(KApplication::kApplication());
    TelepathyHandlerApplication::Private *d = app->d;

    if (d->jobCount.fetchAndAddOrdered(-1) <= 1) {
        if (!Private::s_persist && d->timeout >= 0) {
            kDebug() << "No other jobs at the moment. Starting timer.";
            d->timer->start(d->timeout);
        }
    }
}

} // namespace KTelepathy

#include "telepathy-handler-application.moc"
<commit_msg>Fix crash in debug callback<commit_after>/*
* Copyright (C) 2011 Daniele E. Domenichelli <daniele.domenichelli@gmail.com>
* Copyright (C) 1999 Preston Brown <pbrown@kde.org>
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "telepathy-handler-application.h"

#include <QTimer>
#include <KCmdLineArgs>
#include <KDebug>

#include <TelepathyQt4/Types>
#include <TelepathyQt4/Debug>

extern bool kde_kdebug_enable_dbus_interface;

namespace KTelepathy {

namespace {
int s_tpqt4DebugArea;

static void tpDebugCallback(const QString &libraryName,
                            const QString &libraryVersion,
                            QtMsgType type,
                            const QString &msg)
{
    Q_UNUSED(libraryName)
    Q_UNUSED(libraryVersion)
    kDebugStream(type, s_tpqt4DebugArea, __FILE__, __LINE__, 0) << qPrintable(msg);
}
}

class TelepathyHandlerApplication::Private
{
public:
    Private(TelepathyHandlerApplication *q);
    ~Private();

    void _k_onInitialTimeout();
    void _k_onTimeout();

    static KComponentData initHack();
    void init(int initialTimeout, int timeout);

    TelepathyHandlerApplication *q;

    static bool s_persist;
    static bool s_debug;

    int initialTimeout;
    int timeout;
    QTimer *timer;
    bool firstJobStarted;
    QAtomicInt jobCount;
};

TelepathyHandlerApplication::Private::Private(TelepathyHandlerApplication *q)
    : q(q),
      firstJobStarted(false),
      jobCount(0)
{
}

TelepathyHandlerApplication::Private::~Private()
{
}

void TelepathyHandlerApplication::Private::_k_onInitialTimeout()
{
    if (jobCount == 0 && jobCount.fetchAndAddOrdered(-1) == 0) {
        // m_jobCount is now -1
        kDebug() << "No job received. Exiting";
        QCoreApplication::quit();
    }
}

void TelepathyHandlerApplication::Private::_k_onTimeout()
{
    if (jobCount == 0 && jobCount.fetchAndAddOrdered(-1) == 0) {
        // m_jobCount is now -1
        kDebug() << "Timeout. Exiting";
        QCoreApplication::quit();
    }
}

// this gets called before even entering QApplication::QApplication()
KComponentData TelepathyHandlerApplication::Private::initHack()
{
    KComponentData cData(KCmdLineArgs::aboutData());
    KCmdLineOptions handler_options;
    handler_options.add("persist", ki18n("Persistent mode (do not exit on timeout)"));
    handler_options.add("debug", ki18n("Show Telepathy debugging information"));
    KCmdLineArgs::addCmdLineOptions(handler_options, ki18n("KDE Telepathy"), "kde-telepathy", "kde");
    KCmdLineArgs *args = KCmdLineArgs::parsedArgs("kde-telepathy");
    Private::s_persist = args->isSet("persist");
    Private::s_debug = args->isSet("debug");

    s_tpqt4DebugArea = KDebug::registerArea("Telepathy-Qt4");

    return cData;
}

void TelepathyHandlerApplication::Private::init(int initialTimeout, int timeout)
{
    this->initialTimeout = initialTimeout;
    this->timeout = timeout;

    // If timeout < 0 we let the application exit when the last window is closed,
    // Otherwise we handle it with the timeout
    if (timeout >= 0) {
        q->setQuitOnLastWindowClosed(false);
    }

    // Register TpQt4 types
    Tp::registerTypes();

    // Redirect Tp debug and warnings to KDebug output
    Tp::setDebugCallback(&tpDebugCallback);

    // Enable telepathy-Qt4 debug
    Tp::enableDebug(s_debug);
    Tp::enableWarnings(true);

    // Enable KDebug DBus interface
    // FIXME This must be enabled here because there is a bug in plasma
    //       it should be removed when this is fixed
    kde_kdebug_enable_dbus_interface = s_debug;

    if (!Private::s_persist) {
        timer = new QTimer(q);
        if (initialTimeout >= 0) {
            q->connect(timer, SIGNAL(timeout()), q, SLOT(_k_onInitialTimeout()));
            timer->start(initialTimeout);
        }
    }
}

bool TelepathyHandlerApplication::Private::s_persist = false;
bool TelepathyHandlerApplication::Private::s_debug = false;

TelepathyHandlerApplication::TelepathyHandlerApplication(bool GUIenabled,
                                                         int initialTimeout,
                                                         int timeout)
    : KApplication(GUIenabled, Private::initHack()),
      d(new Private(this))
{
    d->init(initialTimeout, timeout);
}

TelepathyHandlerApplication::TelepathyHandlerApplication(Display *display,
                                                         Qt::HANDLE visual,
                                                         Qt::HANDLE colormap,
                                                         int initialTimeout,
                                                         int timeout)
    : KApplication(display, visual, colormap, Private::initHack()),
      d(new Private(this))
{
    d->init(initialTimeout, timeout);
}

TelepathyHandlerApplication::~TelepathyHandlerApplication()
{
    delete d;
}

int TelepathyHandlerApplication::newJob()
{
    TelepathyHandlerApplication *app = qobject_cast<TelepathyHandlerApplication*>(KApplication::kApplication());
    TelepathyHandlerApplication::Private *d = app->d;

    int ret = d->jobCount.fetchAndAddOrdered(1);
    if (!Private::s_persist) {
        if (d->timer->isActive()) {
            d->timer->stop();
        }
        if (!d->firstJobStarted) {
            if (d->initialTimeout) {
                disconnect(d->timer, SIGNAL(timeout()), app, SLOT(_k_onInitialTimeout()));
            }
            if (d->timeout >= 0) {
                connect(d->timer, SIGNAL(timeout()), app, SLOT(_k_onTimeout()));
            }
            d->firstJobStarted = true;
        }
    }
    return ret;
}

void TelepathyHandlerApplication::jobFinished()
{
    TelepathyHandlerApplication *app = qobject_cast<TelepathyHandlerApplication*>(KApplication::kApplication());
    TelepathyHandlerApplication::Private *d = app->d;

    if (d->jobCount.fetchAndAddOrdered(-1) <= 1) {
        if (!Private::s_persist && d->timeout >= 0) {
            kDebug() << "No other jobs at the moment. Starting timer.";
            d->timer->start(d->timeout);
        }
    }
}

} // namespace KTelepathy

#include "telepathy-handler-application.moc"
<|endoftext|>


<commit_before>
#pragma once

#include <proxc.hpp>

#include <stdlib.h> // posix_memalign

#include <new>
#include <type_traits>

PROXC_NAMESPACE_BEGIN

void* aligned_alloc(std::size_t size, std::size_t align)
{
#ifdef _WIN32
    void *ptr = _aligned_malloc(size, align);
    if (!ptr) {
        throw std::bad_alloc();
    }
    return ptr;
#else
    void *ptr;
    if (posix_memalign(&ptr, align, size)) {
        throw std::bad_alloc();
    }
    return ptr;
#endif
}

void aligned_free(void* addr) noexcept
{
#ifdef _WIN32
    _aligned_free(addr);
#else
    free(addr);
#endif
}

template<typename T, std::size_t Align = std::alignment_of<T>::value>
class AlignedArray
{
public:
    AlignedArray()
        : m_length(0), m_ptr(nullptr) {}

    AlignedArray(std::nullptr_t)
        : m_length(0), m_ptr(nullptr) {}

    explicit AlignedArray(std::size_t length)
        : m_length(length)
    {
        m_ptr = static_cast<T*>(aligned_alloc(length * sizeof(T), Align));
        std::size_t i;
        try {
            for (i = 0; i < m_length; ++i) {
                new(m_ptr + i) T;
            }
        } catch(...) {
            for (std::size_t j = 0; j < i; ++j) {
                m_ptr[i].~T();
                throw;
            }
        }
    }

    AlignedArray(AlignedArray&& other) noexcept
        : m_length(other.m_length), m_ptr(other.m_ptr)
    {

    }

    AlignedArray& operator=(std::nullptr_t)
    {
        return *this = AlignedArray();
    }

    ~AlignedArray()
    {
        for (std::size_t i = 0; i < m_length; ++i) {
            m_ptr[i].~T();
        }
        aligned_free(m_ptr);
    }

    T& operator[](std::size_t i) const
    {
        return m_ptr[i];
    }

    std::size_t size() const
    {
        return m_length;
    }

    T* get() const
    {
        return m_ptr;
    }

    explicit operator bool() const
    {
        return m_ptr != nullptr;
    }

private:
    std::size_t m_length;
    T* m_ptr;
};

PROXC_NAMESPACE_END

<commit_msg>Forgot impl of move constructor for AlignedArray<commit_after>
#pragma once

#include <proxc.hpp>

#include <stdlib.h> // posix_memalign

#include <new>
#include <type_traits>

PROXC_NAMESPACE_BEGIN

void* aligned_alloc(std::size_t size, std::size_t align)
{
#ifdef _WIN32
    void *ptr = _aligned_malloc(size, align);
    if (!ptr) {
        throw std::bad_alloc();
    }
    return ptr;
#else
    void *ptr;
    if (posix_memalign(&ptr, align, size)) {
        throw std::bad_alloc();
    }
    return ptr;
#endif
}

void aligned_free(void* addr) noexcept
{
#ifdef _WIN32
    _aligned_free(addr);
#else
    free(addr);
#endif
}

template<typename T, std::size_t Align = std::alignment_of<T>::value>
class AlignedArray
{
public:
    AlignedArray()
        : m_length(0), m_ptr(nullptr) {}

    AlignedArray(std::nullptr_t)
        : m_length(0), m_ptr(nullptr) {}

    explicit AlignedArray(std::size_t length)
        : m_length(length)
    {
        m_ptr = static_cast<T*>(aligned_alloc(length * sizeof(T), Align));
        std::size_t i;
        try {
            for (i = 0; i < m_length; ++i) {
                new(m_ptr + i) T;
            }
        } catch(...) {
            for (std::size_t j = 0; j < i; ++j) {
                m_ptr[i].~T();
                throw;
            }
        }
    }

    AlignedArray(AlignedArray&& other) noexcept
        : m_length(other.m_length), m_ptr(other.m_ptr)
    {
        other.m_ptr = nullptr;
        other.m_length = 0;
    }

    AlignedArray& operator=(std::nullptr_t)
    {
        return *this = AlignedArray();
    }

    ~AlignedArray()
    {
        for (std::size_t i = 0; i < m_length; ++i) {
            m_ptr[i].~T();
        }
        aligned_free(m_ptr);
    }

    T& operator[](std::size_t i) const
    {
        return m_ptr[i];
    }

    std::size_t size() const
    {
        return m_length;
    }

    T* get() const
    {
        return m_ptr;
    }

    explicit operator bool() const
    {
        return m_ptr != nullptr;
    }

private:
    std::size_t m_length;
    T* m_ptr;
};

PROXC_NAMESPACE_END

<|endoftext|>


<commit_before>#include "prune.hpp"

namespace vg {

constexpr size_t PRUNE_THREAD_BUFFER_SIZE = 128 * 1024;

pair_hash_set<edge_t> find_edges_to_prune(const HandleGraph& graph, size_t k, size_t edge_max) {

    // Each thread collects edges to be deleted into a separate buffer. When the buffer grows
    // large enough, flush it into a shared hash set.
    pair_hash_set<edge_t> result;
    auto flush_buffer = [&result](pair_hash_set<edge_t>& buffer) {
        #pragma omp critical (prune_flush)
        {
            for (const edge_t& edge : buffer) {
                result.insert(edge);
            }
        }
        buffer.clear();
    };

    // for each position on the forward and reverse of the graph
    std::vector<pair_hash_set<edge_t>> buffers(get_thread_count());
    graph.for_each_handle([&](const handle_t& h) {
        // for the forward and reverse of this handle
        // walk k bases from the end, so that any kmer starting on the node will be represented in the tree we build
        for (auto handle_is_rev : { false, true }) {
            //cerr << "###########################################" << endl;
            handle_t handle = handle_is_rev ? graph.flip(h) : h;
            list<walk_t> walks;
            // for each position in the node, set up a kmer with that start position and the node end or kmer length as the end position
            // determine next positions
            id_t handle_id = graph.get_id(handle);
            size_t handle_length = graph.get_length(handle);
            string handle_seq = graph.get_sequence(handle);
            for (size_t i = 0; i < handle_length;  ++i) {
                pos_t begin = make_pos_t(handle_id, handle_is_rev, handle_length);
                pos_t end = make_pos_t(handle_id, handle_is_rev, min(handle_length, i+k));
                walk_t walk = walk_t(offset(end)-offset(begin), begin, end, handle, 0);
                if (walk.length < k) {
                    // are we branching over more than one edge?
                    size_t next_count = 0;
                    graph.follow_edges(walk.curr, false, [&](const handle_t& next) { ++next_count; });
                    graph.follow_edges(walk.curr, false, [&](const handle_t& next) {
                            if (next_count > 1 && edge_max == walk.forks) { // our next step takes us over the max
                                int tid = omp_get_thread_num();
                                buffers[tid].insert(graph.edge_handle(walk.curr, next));
                                if (buffers[tid].size() >= PRUNE_THREAD_BUFFER_SIZE) {
                                    flush_buffer(buffers[tid]);
                                }
                            } else {
                                walks.push_back(walk);
                                auto& todo = walks.back();
                                todo.curr = next;
                                if (next_count > 1) {
                                    ++todo.forks;
                                }
                            }
                        });
                } else {
                    walks.push_back(walk);
                }
            }
            // now expand the kmers until they reach k
            while (!walks.empty()) {
                // first we check which ones have reached length k in the current handle; for each of these we run lambda and remove them from our list
                auto walks_end = walks.end();
                for (list<walk_t>::iterator q = walks.begin(); q != walks_end; ++q) {
                    auto& walk = *q;
                    // did we reach our target length?
                    if (walk.length >= k) {
                        q = walks.erase(q);
                    } else {
                        id_t curr_id = graph.get_id(walk.curr);
                        size_t curr_length = graph.get_length(walk.curr);
                        bool curr_is_rev = graph.get_is_reverse(walk.curr);
                        size_t take = min(curr_length, k-walk.length);
                        walk.end = make_pos_t(curr_id, curr_is_rev, take);
                        walk.length += take;
                        if (walk.length < k) {
                            // if not, we need to expand through the node then follow on
                            size_t next_count = 0;
                            graph.follow_edges(walk.curr, false, [&](const handle_t& next) { ++next_count; });
                            graph.follow_edges(walk.curr, false, [&](const handle_t& next) {
                                if (next_count > 1 && edge_max == walk.forks) { // our next step takes us over the max
                                    int tid = omp_get_thread_num();
                                    buffers[tid].insert(graph.edge_handle(walk.curr, next));
                                    if (buffers[tid].size() >= PRUNE_THREAD_BUFFER_SIZE) {
                                        flush_buffer(buffers[tid]);
                                    }
                                } else {
                                    walks.push_back(walk);
                                    auto& todo = walks.back();
                                    todo.curr = next;
                                    if (next_count > 1) {
                                        ++todo.forks;
                                    }
                                }
                            });
                            q = walks.erase(q);
                        } else {
                            // nothing, we'll remove it next time around
                        }
                    }
                }
            }
        }
    }, true);

    // Flush the buffers and return the result.
    for (pair_hash_set<edge_t>& buffer : buffers) {
        flush_buffer(buffer);
    }
    return result;
}

}
<commit_msg>Use DFS in prune<commit_after>#include "prune.hpp"

#include <stack>

namespace vg {

constexpr size_t PRUNE_THREAD_BUFFER_SIZE = 128 * 1024;

pair_hash_set<edge_t> find_edges_to_prune(const HandleGraph& graph, size_t k, size_t edge_max) {

    // Each thread collects edges to be deleted into a separate buffer. When the buffer grows
    // large enough, flush it into a shared hash set.
    pair_hash_set<edge_t> result;
    auto flush_buffer = [&result](pair_hash_set<edge_t>& buffer) {
        #pragma omp critical (prune_flush)
        {
            for (const edge_t& edge : buffer) {
                result.insert(edge);
            }
        }
        buffer.clear();
    };

    // for each position on the forward and reverse of the graph
    std::vector<pair_hash_set<edge_t>> buffers(get_thread_count());
    graph.for_each_handle([&](const handle_t& h) {
        // for the forward and reverse of this handle
        // walk k bases from the end, so that any kmer starting on the node will be represented in the tree we build
        for (auto handle_is_rev : { false, true }) {
            handle_t handle = handle_is_rev ? graph.flip(h) : h;
            std::stack<walk_t> walks;
            // for each position in the node, set up a kmer with that start position and the node end or kmer length as the end position
            // determine next positions
            id_t handle_id = graph.get_id(handle);
            size_t handle_length = graph.get_length(handle);
            for (size_t i = 0; i < handle_length; i++) {
                pos_t begin = make_pos_t(handle_id, handle_is_rev, handle_length);
                pos_t end = make_pos_t(handle_id, handle_is_rev, std::min(handle_length, i + k));
                // We are only interested in walks that did not reach length k in the initial node.
                if (offset(end) - offset(begin) < k) {
                    size_t outdegree = graph.get_degree(handle, false);
                    graph.follow_edges(handle, false, [&](const handle_t& next) {
                        if (outdegree > 1 && edge_max == 0) { // our next step takes us over the max
                            int tid = omp_get_thread_num();
                            buffers[tid].insert(graph.edge_handle(handle, next));
                            if (buffers[tid].size() >= PRUNE_THREAD_BUFFER_SIZE) {
                                flush_buffer(buffers[tid]);
                            }
                        } else {
                            walk_t walk(offset(end) - offset(begin), begin, end, next, 0);
                            if (outdegree > 1) {
                                walk.forks++;
                            }
                            walks.push(walk);
                        }
                    });
                }
            }

            // Now expand the kmers until they reach k.
            while (!walks.empty()) {
                walk_t walk = walks.top();
                walks.pop();
                // Did we reach our target length?
                if (walk.length >= k) {
                    continue;
                }
                id_t curr_id = graph.get_id(walk.curr);
                size_t curr_length = graph.get_length(walk.curr);
                bool curr_is_rev = graph.get_is_reverse(walk.curr);
                size_t take = min(curr_length, k - walk.length);
                walk.end = make_pos_t(curr_id, curr_is_rev, take);
                walk.length += take;
                // Do we need to continue to the successor nodes?
                if (walk.length < k) {
                    size_t outdegree = graph.get_degree(walk.curr, false);
                    graph.follow_edges(walk.curr, false, [&](const handle_t& next) {
                        if (outdegree > 1 && edge_max == walk.forks) { // our next step takes us over the max
                            int tid = omp_get_thread_num();
                            buffers[tid].insert(graph.edge_handle(walk.curr, next));
                            if (buffers[tid].size() >= PRUNE_THREAD_BUFFER_SIZE) {
                                flush_buffer(buffers[tid]);
                            }
                        } else {
                            walk_t next_walk = walk;
                            next_walk.curr = next;
                            if (outdegree > 1) {
                                next_walk.forks++;
                            }
                            walks.push(next_walk);
                        }
                    });
                }
            }
        }
    }, true);

    // Flush the buffers and return the result.
    for (pair_hash_set<edge_t>& buffer : buffers) {
        flush_buffer(buffer);
    }
    return result;
}

}
<|endoftext|>


<commit_before><commit_msg>coverity#1194898 Logically dead code<commit_after><|endoftext|>


<commit_before>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <vector>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if LLVM_VERSION_MAJOR >= 4 || (LLVM_VERSION_MAJOR == 3 && LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"

using namespace llvm;

class RemoveInfiniteLoops : public FunctionPass {
  public:
    static char ID;

    RemoveInfiniteLoops() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

static RegisterPass<RemoveInfiniteLoops> RIL("remove-infinite-loops",
                                             "delete patterns like LABEL: goto LABEL"
                                             "and replace them with exit(0)");
char RemoveInfiniteLoops::ID;

bool RemoveInfiniteLoops::runOnFunction(Function &F) {
  Module *M = F.getParent();

  std::vector<BasicBlock *> to_process;
  for (BasicBlock& block : F) {
    // if this is a block that jumps on itself (it has the only instruction
    // which is the jump)
    if (block.size() == 1 && block.getSingleSuccessor() == &block)
        to_process.push_back(&block);
  }

  if (to_process.empty())
    return false;

  CallInst* ext;
  LLVMContext& Ctx = M->getContext();
  Type *argTy = Type::getInt32Ty(Ctx);
  Function *extF
    = cast<Function>(M->getOrInsertFunction("exit",
                                            Type::getVoidTy(Ctx),
                                            argTy, nullptr));

  std::vector<Value *> args = { ConstantInt::get(argTy, 0) };

  for (BasicBlock *block : to_process) {
    Instruction *T = block->getTerminator();
    ext = CallInst::Create(extF, args);
    ext->insertBefore(T);

    // replace the jump with unreachable, since exit will terminate
    // the computation
    new UnreachableInst(Ctx, T);
    T->eraseFromParent();
  }

  llvm::errs() << "Removed infinite loop in " << F.getName().data() << "\n";
  return true;
}

<commit_msg>RemoveInfiniteLoops: clone metadata too<commit_after>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <vector>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if LLVM_VERSION_MAJOR >= 4 || (LLVM_VERSION_MAJOR == 3 && LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"

using namespace llvm;

class RemoveInfiniteLoops : public FunctionPass {
  public:
    static char ID;

    RemoveInfiniteLoops() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

static RegisterPass<RemoveInfiniteLoops> RIL("remove-infinite-loops",
                                             "delete patterns like LABEL: goto LABEL"
                                             "and replace them with exit(0)");
char RemoveInfiniteLoops::ID;

void CloneMetadata(const llvm::Instruction *i1, llvm::Instruction *i2);

bool RemoveInfiniteLoops::runOnFunction(Function &F) {
  Module *M = F.getParent();

  std::vector<BasicBlock *> to_process;
  for (BasicBlock& block : F) {
    // if this is a block that jumps on itself (it has the only instruction
    // which is the jump)
    if (block.size() == 1 && block.getSingleSuccessor() == &block)
        to_process.push_back(&block);
  }

  if (to_process.empty())
    return false;

  CallInst* ext;
  LLVMContext& Ctx = M->getContext();
  Type *argTy = Type::getInt32Ty(Ctx);
  Function *extF
    = cast<Function>(M->getOrInsertFunction("exit",
                                            Type::getVoidTy(Ctx),
                                            argTy, nullptr));

  std::vector<Value *> args = { ConstantInt::get(argTy, 0) };

  for (BasicBlock *block : to_process) {
    Instruction *T = block->getTerminator();
    ext = CallInst::Create(extF, args);
    CloneMetadata(&*(block->begin()), ext);
    ext->insertBefore(T);

    // replace the jump with unreachable, since exit will terminate
    // the computation
    new UnreachableInst(Ctx, T);
    T->eraseFromParent();
  }

  llvm::errs() << "Removed infinite loop in " << F.getName().data() << "\n";
  return true;
}

<|endoftext|>


<commit_before>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm_comm, ros::NodeHandle &nh) : arm(arm_comm)
{
	std::string joint_velocity_topic, joint_angles_topic, cartesian_velocity_topic,
		tool_position_topic, set_finger_position_topic, finger_position_topic, joint_state_topic,
		set_joint_angle_topic;

	nh.param<std::string>("joint_velocity_topic", joint_velocity_topic, "joint_velocity");
	nh.param<std::string>("joint_angles_topic", joint_angles_topic, "joint_angles");
	nh.param<std::string>("cartesian_velocity_topic", cartesian_velocity_topic, "cartesian_velocity");
	nh.param<std::string>("tool_position_topic", tool_position_topic, "tool_position");
	nh.param<std::string>("set_finger_position_topic", set_finger_position_topic, "set_finger_position");
	nh.param<std::string>("finger_position_topic", finger_position_topic, "finger_position");
	nh.param<std::string>("joint_state_topic", joint_state_topic, "joint_state");
	nh.param<std::string>("set_joint_angle_topic", set_joint_angle_topic, "set_joint_angle");

	//Print out received topics
	ROS_DEBUG("Got Joint Velocity Topic Name: <%s>", joint_velocity_topic.c_str());
	ROS_DEBUG("Got Joint Angles Topic Name: <%s>", joint_angles_topic.c_str());
	ROS_DEBUG("Got Cartesian Velocity Topic Name: <%s>", cartesian_velocity_topic.c_str());
	ROS_DEBUG("Got Tool Position Topic Name: <%s>", tool_position_topic.c_str());
	ROS_DEBUG("Got Set Finger Position Topic Name: <%s>", set_finger_position_topic.c_str());
	ROS_DEBUG("Got Finger Position Topic Name: <%s>", finger_position_topic.c_str());
	ROS_DEBUG("Got Joint State Topic Name: <%s>", joint_state_topic.c_str());
	ROS_DEBUG("Got Set Joint Angle Topic Name: <%s>", set_joint_angle_topic.c_str());

	ROS_INFO("Starting Up Jaco Arm Controller...");

	previous_state = 0;

	/* Set up Services */
	stop_service = nh.advertiseService("stop", &JacoArm::StopSRV, this);
	start_service = nh.advertiseService("start", &JacoArm::StartSRV, this);
	homing_service = nh.advertiseService("home_arm", &JacoArm::HomeArmSRV, this);

	/* Set Default Configuration */

	// API->RestoreFactoryDefault(); // uncomment comment ONLY if you want to lose your settings on each launch.

	ClientConfigurations configuration;
	arm.GetConfig(configuration);
	arm.PrintConfig(configuration);

	last_update_time = ros::Time::now();
	update_time = ros::Duration(5.0);

	/* Storing arm in home position */

	/* Set up Publishers */
	JointAngles_pub = nh.advertise<jaco_driver::JointAngles>(joint_angles_topic, 2);
	JointState_pub = nh.advertise<sensor_msgs::JointState>(joint_state_topic, 2);
	ToolPosition_pub = nh.advertise<geometry_msgs::PoseStamped>(tool_position_topic, 2);
	FingerPosition_pub = nh.advertise<jaco_driver::FingerPosition>(finger_position_topic, 2);

	/* Set up Subscribers*/
	JointVelocity_sub = nh.subscribe(joint_velocity_topic, 1, &JacoArm::VelocityMSG, this);
	CartesianVelocity_sub = nh.subscribe(cartesian_velocity_topic, 1, &JacoArm::CartesianVelocityMSG, this);
	SetFingerPosition_sub = nh.subscribe(set_finger_position_topic, 1, &JacoArm::SetFingerPositionMSG, this);
	SetJoint_sub = nh.subscribe(set_joint_angle_topic, 1, &JacoArm::SetJointAnglesMSG, this);

	status_timer = nh.createTimer(ros::Duration(0.05), &JacoArm::StatusTimer, this);

	joint_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::JointVelTimer, this);
	joint_vel_timer.stop();
	joint_vel_timer_flag = false;
	cartesian_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::CartesianVelTimer, this);
	cartesian_vel_timer.stop();
	cartesian_vel_timer_flag = false;

	ROS_INFO("The Arm is ready to use.");

	TrajectoryPoint Jaco_Velocity;

	memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

	arm.SetCartesianVelocities(Jaco_Velocity.Position.CartesianPosition);
}

JacoArm::~JacoArm()
{
}

bool JacoArm::HomeArmSRV(jaco_driver::HomeArm::Request &req, jaco_driver::HomeArm::Response &res)
{
	arm.HomeArm();
	res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";

	return true;
}

/*!
 * \brief Receives ROS command messages and relays them to SetFingers.
 */
void JacoArm::SetFingerPositionMSG(const jaco_driver::FingerPositionConstPtr& finger_pos)
{
	if (!arm.Stopped())
	{
		FingersPosition Finger_Position;
		memset(&Finger_Position, 0, sizeof(Finger_Position)); //zero structure

		Finger_Position.Finger1 = finger_pos->Finger_1;
		Finger_Position.Finger2 = finger_pos->Finger_2;
		Finger_Position.Finger3 = finger_pos->Finger_3;

		arm.SetFingers(Finger_Position);
	}
}

/*!
 * \brief Receives ROS command messages and relays them to SetAngles.
 */
void JacoArm::SetJointAnglesMSG(const jaco_driver::JointAnglesConstPtr& msg)
{
	if (!arm.Stopped())
	{
		jaco_driver::JointAngles angles(*msg);
		JacoAngles position(angles);
		arm.SetAngles(position);
	}
}

void JacoArm::VelocityMSG(const jaco_driver::JointVelocityConstPtr& joint_vel)
{
	if (!arm.Stopped())
	{
		joint_velocities.Actuator1 = joint_vel->Velocity_J1;
		joint_velocities.Actuator2 = joint_vel->Velocity_J2;
		joint_velocities.Actuator3 = joint_vel->Velocity_J3;
		joint_velocities.Actuator4 = joint_vel->Velocity_J4;
		joint_velocities.Actuator5 = joint_vel->Velocity_J5;
		joint_velocities.Actuator6 = joint_vel->Velocity_J6;
		last_joint_update = ros::Time().now();

		if (joint_vel_timer_flag == false)
		{
			joint_vel_timer.start();
			joint_vel_timer_flag = true;
		}
	}
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::StopSRV(jaco_driver::Stop::Request &req, jaco_driver::Stop::Response &res)
{
	arm.Stop();
	res.stop_result = "JACO ARM HAS BEEN STOPPED";
	ROS_DEBUG("JACO ARM STOP REQUEST");

	return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::StartSRV(jaco_driver::Start::Request &req, jaco_driver::Start::Response &res)
{
	arm.Start();
	res.start_result = "JACO ARM CONTROL HAS BEEN ENABLED";
	ROS_DEBUG("JACO ARM START REQUEST");

	return true;
}

void JacoArm::CartesianVelocityMSG(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
	if (!arm.Stopped())
	{
		cartesian_velocities.X = cartesian_vel->twist.linear.x;
		cartesian_velocities.Y = cartesian_vel->twist.linear.y;
		cartesian_velocities.Z = cartesian_vel->twist.linear.z;
		cartesian_velocities.ThetaX = cartesian_vel->twist.angular.x;
		cartesian_velocities.ThetaY = cartesian_vel->twist.angular.y;
		cartesian_velocities.ThetaZ = cartesian_vel->twist.angular.z;

		last_cartesian_update = ros::Time().now();

		if (cartesian_vel_timer_flag == false)
		{
			cartesian_vel_timer.start();
			cartesian_vel_timer_flag = true;
		}
	}
}

void JacoArm::CartesianVelTimer(const ros::TimerEvent&)
{
	arm.SetCartesianVelocities(cartesian_velocities);

	if ((ros::Time().now().toSec() - last_cartesian_update.toSec()) > 1)
	{
		cartesian_vel_timer.stop();
		cartesian_vel_timer_flag = false;
	}
}

void JacoArm::JointVelTimer(const ros::TimerEvent&)
{
	arm.SetVelocities(joint_velocities);

	if ((ros::Time().now().toSec() - last_joint_update.toSec()) > 1)
	{
		joint_vel_timer.stop();
		joint_vel_timer_flag = false;
	}
}

/*!
 * \brief Contains coordinates for an alternate "Home" position
 *
 * GoHome() function must be enabled in the initialization routine for this to
 * work.
 */
void JacoArm::GoHome(void)
{
/*
	AngularInfo joint_home;

	joint_home.Actuator1 = 176.0;
	joint_home.Actuator2 = 111.0;
	joint_home.Actuator3 = 107.0;
	joint_home.Actuator4 = 459.0;
	joint_home.Actuator5 = 102.0;
	joint_home.Actuator6 = 106.0;

	SetAngles(joint_home, 10); //send joints to home position

	API->SetCartesianControl();
*/
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * JointState will eventually also publish the velocity and effort for each
 * joint, when this data is made available by the C++ API.  Currenty velocity
 * and effort are reported as being zero (0.0) for all joints.
 */
void JacoArm::BroadCastAngles(void)
{
	// Populate an array of joint names.  arm_0_joint is the base, arm_5_joint is the wrist.
	const char* nameArgs[] = {"arm_0_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint"};
	std::vector<std::string> JointName(nameArgs, nameArgs+6);

	jaco_driver::JointAngles current_angles;

	sensor_msgs::JointState joint_state;
	joint_state.name = JointName;

	// Define array sizes for the joint_state topic
	joint_state.position.resize(6);
	joint_state.velocity.resize(6);
	joint_state.effort.resize(6);

	//Query arm for current joint angles
	JacoAngles arm_angles;
	arm.GetAngles(arm_angles);
	jaco_driver::JointAngles angles = arm_angles.Angles();

	// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
	joint_state.position[0] = angles.Angle_J1;
	joint_state.position[1] = angles.Angle_J2;
	joint_state.position[2] = angles.Angle_J3;
	joint_state.position[3] = angles.Angle_J4;
	joint_state.position[4] = angles.Angle_J5;
	joint_state.position[5] = angles.Angle_J6;

	//Publish the joint state messages

	JointAngles_pub.publish(angles); // Publishes the raw joint angles in a custom message.

	JointState_pub.publish(joint_state);     // Publishes the transformed angles in a standard sensor_msgs format.
}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::BroadCastPosition(void)
{
	JacoPose pose;
	geometry_msgs::PoseStamped current_position;

	arm.GetPosition(pose);
	current_position.pose = pose.Pose();

	ToolPosition_pub.publish(current_position);
}

void JacoArm::BroadCastFingerPosition(void)
{

/*
Publishes the current finger positions.
*/

	CartesianPosition Jaco_Position;
	jaco_driver::FingerPosition finger_position;

	memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
	arm.GetFingers(Jaco_Position.Fingers);

	finger_position.Finger_1 = Jaco_Position.Fingers.Finger1;
	finger_position.Finger_2 = Jaco_Position.Fingers.Finger2;
	finger_position.Finger_3 = Jaco_Position.Fingers.Finger3;

	FingerPosition_pub.publish(finger_position);
}

void JacoArm::StatusTimer(const ros::TimerEvent&)
{
	BroadCastAngles();
	BroadCastPosition();
	BroadCastFingerPosition();
}

}
<commit_msg>Send angles in degrees for tf_publisher.<commit_after>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm_comm, ros::NodeHandle &nh) : arm(arm_comm)
{
	std::string joint_velocity_topic, joint_angles_topic, cartesian_velocity_topic,
		tool_position_topic, set_finger_position_topic, finger_position_topic, joint_state_topic,
		set_joint_angle_topic;

	nh.param<std::string>("joint_velocity_topic", joint_velocity_topic, "joint_velocity");
	nh.param<std::string>("joint_angles_topic", joint_angles_topic, "joint_angles");
	nh.param<std::string>("cartesian_velocity_topic", cartesian_velocity_topic, "cartesian_velocity");
	nh.param<std::string>("tool_position_topic", tool_position_topic, "tool_position");
	nh.param<std::string>("set_finger_position_topic", set_finger_position_topic, "set_finger_position");
	nh.param<std::string>("finger_position_topic", finger_position_topic, "finger_position");
	nh.param<std::string>("joint_state_topic", joint_state_topic, "joint_state");
	nh.param<std::string>("set_joint_angle_topic", set_joint_angle_topic, "set_joint_angle");

	//Print out received topics
	ROS_DEBUG("Got Joint Velocity Topic Name: <%s>", joint_velocity_topic.c_str());
	ROS_DEBUG("Got Joint Angles Topic Name: <%s>", joint_angles_topic.c_str());
	ROS_DEBUG("Got Cartesian Velocity Topic Name: <%s>", cartesian_velocity_topic.c_str());
	ROS_DEBUG("Got Tool Position Topic Name: <%s>", tool_position_topic.c_str());
	ROS_DEBUG("Got Set Finger Position Topic Name: <%s>", set_finger_position_topic.c_str());
	ROS_DEBUG("Got Finger Position Topic Name: <%s>", finger_position_topic.c_str());
	ROS_DEBUG("Got Joint State Topic Name: <%s>", joint_state_topic.c_str());
	ROS_DEBUG("Got Set Joint Angle Topic Name: <%s>", set_joint_angle_topic.c_str());

	ROS_INFO("Starting Up Jaco Arm Controller...");

	previous_state = 0;

	/* Set up Services */
	stop_service = nh.advertiseService("stop", &JacoArm::StopSRV, this);
	start_service = nh.advertiseService("start", &JacoArm::StartSRV, this);
	homing_service = nh.advertiseService("home_arm", &JacoArm::HomeArmSRV, this);

	/* Set Default Configuration */

	// API->RestoreFactoryDefault(); // uncomment comment ONLY if you want to lose your settings on each launch.

	ClientConfigurations configuration;
	arm.GetConfig(configuration);
	arm.PrintConfig(configuration);

	last_update_time = ros::Time::now();
	update_time = ros::Duration(5.0);

	/* Storing arm in home position */

	/* Set up Publishers */
	JointAngles_pub = nh.advertise<jaco_driver::JointAngles>(joint_angles_topic, 2);
	JointState_pub = nh.advertise<sensor_msgs::JointState>(joint_state_topic, 2);
	ToolPosition_pub = nh.advertise<geometry_msgs::PoseStamped>(tool_position_topic, 2);
	FingerPosition_pub = nh.advertise<jaco_driver::FingerPosition>(finger_position_topic, 2);

	/* Set up Subscribers*/
	JointVelocity_sub = nh.subscribe(joint_velocity_topic, 1, &JacoArm::VelocityMSG, this);
	CartesianVelocity_sub = nh.subscribe(cartesian_velocity_topic, 1, &JacoArm::CartesianVelocityMSG, this);
	SetFingerPosition_sub = nh.subscribe(set_finger_position_topic, 1, &JacoArm::SetFingerPositionMSG, this);
	SetJoint_sub = nh.subscribe(set_joint_angle_topic, 1, &JacoArm::SetJointAnglesMSG, this);

	status_timer = nh.createTimer(ros::Duration(0.05), &JacoArm::StatusTimer, this);

	joint_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::JointVelTimer, this);
	joint_vel_timer.stop();
	joint_vel_timer_flag = false;
	cartesian_vel_timer = nh.createTimer(ros::Duration(0.01), &JacoArm::CartesianVelTimer, this);
	cartesian_vel_timer.stop();
	cartesian_vel_timer_flag = false;

	ROS_INFO("The Arm is ready to use.");

	TrajectoryPoint Jaco_Velocity;

	memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure

	arm.SetCartesianVelocities(Jaco_Velocity.Position.CartesianPosition);
}

JacoArm::~JacoArm()
{
}

bool JacoArm::HomeArmSRV(jaco_driver::HomeArm::Request &req, jaco_driver::HomeArm::Response &res)
{
	arm.HomeArm();
	res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";

	return true;
}

/*!
 * \brief Receives ROS command messages and relays them to SetFingers.
 */
void JacoArm::SetFingerPositionMSG(const jaco_driver::FingerPositionConstPtr& finger_pos)
{
	if (!arm.Stopped())
	{
		FingersPosition Finger_Position;
		memset(&Finger_Position, 0, sizeof(Finger_Position)); //zero structure

		Finger_Position.Finger1 = finger_pos->Finger_1;
		Finger_Position.Finger2 = finger_pos->Finger_2;
		Finger_Position.Finger3 = finger_pos->Finger_3;

		arm.SetFingers(Finger_Position);
	}
}

/*!
 * \brief Receives ROS command messages and relays them to SetAngles.
 */
void JacoArm::SetJointAnglesMSG(const jaco_driver::JointAnglesConstPtr& msg)
{
	if (!arm.Stopped())
	{
		jaco_driver::JointAngles angles(*msg);
		JacoAngles position(angles);
		arm.SetAngles(position);
	}
}

void JacoArm::VelocityMSG(const jaco_driver::JointVelocityConstPtr& joint_vel)
{
	if (!arm.Stopped())
	{
		joint_velocities.Actuator1 = joint_vel->Velocity_J1;
		joint_velocities.Actuator2 = joint_vel->Velocity_J2;
		joint_velocities.Actuator3 = joint_vel->Velocity_J3;
		joint_velocities.Actuator4 = joint_vel->Velocity_J4;
		joint_velocities.Actuator5 = joint_vel->Velocity_J5;
		joint_velocities.Actuator6 = joint_vel->Velocity_J6;
		last_joint_update = ros::Time().now();

		if (joint_vel_timer_flag == false)
		{
			joint_vel_timer.start();
			joint_vel_timer_flag = true;
		}
	}
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::StopSRV(jaco_driver::Stop::Request &req, jaco_driver::Stop::Response &res)
{
	arm.Stop();
	res.stop_result = "JACO ARM HAS BEEN STOPPED";
	ROS_DEBUG("JACO ARM STOP REQUEST");

	return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::StartSRV(jaco_driver::Start::Request &req, jaco_driver::Start::Response &res)
{
	arm.Start();
	res.start_result = "JACO ARM CONTROL HAS BEEN ENABLED";
	ROS_DEBUG("JACO ARM START REQUEST");

	return true;
}

void JacoArm::CartesianVelocityMSG(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
	if (!arm.Stopped())
	{
		cartesian_velocities.X = cartesian_vel->twist.linear.x;
		cartesian_velocities.Y = cartesian_vel->twist.linear.y;
		cartesian_velocities.Z = cartesian_vel->twist.linear.z;
		cartesian_velocities.ThetaX = cartesian_vel->twist.angular.x;
		cartesian_velocities.ThetaY = cartesian_vel->twist.angular.y;
		cartesian_velocities.ThetaZ = cartesian_vel->twist.angular.z;

		last_cartesian_update = ros::Time().now();

		if (cartesian_vel_timer_flag == false)
		{
			cartesian_vel_timer.start();
			cartesian_vel_timer_flag = true;
		}
	}
}

void JacoArm::CartesianVelTimer(const ros::TimerEvent&)
{
	arm.SetCartesianVelocities(cartesian_velocities);

	if ((ros::Time().now().toSec() - last_cartesian_update.toSec()) > 1)
	{
		cartesian_vel_timer.stop();
		cartesian_vel_timer_flag = false;
	}
}

void JacoArm::JointVelTimer(const ros::TimerEvent&)
{
	arm.SetVelocities(joint_velocities);

	if ((ros::Time().now().toSec() - last_joint_update.toSec()) > 1)
	{
		joint_vel_timer.stop();
		joint_vel_timer_flag = false;
	}
}

/*!
 * \brief Contains coordinates for an alternate "Home" position
 *
 * GoHome() function must be enabled in the initialization routine for this to
 * work.
 */
void JacoArm::GoHome(void)
{
/*
	AngularInfo joint_home;

	joint_home.Actuator1 = 176.0;
	joint_home.Actuator2 = 111.0;
	joint_home.Actuator3 = 107.0;
	joint_home.Actuator4 = 459.0;
	joint_home.Actuator5 = 102.0;
	joint_home.Actuator6 = 106.0;

	SetAngles(joint_home, 10); //send joints to home position

	API->SetCartesianControl();
*/
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * JointState will eventually also publish the velocity and effort for each
 * joint, when this data is made available by the C++ API.  Currenty velocity
 * and effort are reported as being zero (0.0) for all joints.
 */
void JacoArm::BroadCastAngles(void)
{
	// Populate an array of joint names.  arm_0_joint is the base, arm_5_joint is the wrist.
	const char* nameArgs[] = {"arm_0_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint"};
	std::vector<std::string> JointName(nameArgs, nameArgs+6);

	jaco_driver::JointAngles current_angles;

	sensor_msgs::JointState joint_state;
	joint_state.name = JointName;

	// Define array sizes for the joint_state topic
	joint_state.position.resize(6);
	joint_state.velocity.resize(6);
	joint_state.effort.resize(6);

	//Query arm for current joint angles
	JacoAngles arm_angles;
	arm.GetAngles(arm_angles);
	jaco_driver::JointAngles ros_angles = arm_angles.Angles();

	// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
	joint_state.position[0] = ros_angles.Angle_J1;
	joint_state.position[1] = ros_angles.Angle_J2;
	joint_state.position[2] = ros_angles.Angle_J3;
	joint_state.position[3] = ros_angles.Angle_J4;
	joint_state.position[4] = ros_angles.Angle_J5;
	joint_state.position[5] = ros_angles.Angle_J6;

	//Publish the joint state messages
	ros_angles.Angle_J1 = arm_angles.Actuator1;
	ros_angles.Angle_J2 = arm_angles.Actuator2;
	ros_angles.Angle_J3 = arm_angles.Actuator3;
	ros_angles.Angle_J4 = arm_angles.Actuator4;
	ros_angles.Angle_J5 = arm_angles.Actuator5;
	ros_angles.Angle_J6 = arm_angles.Actuator6;
	JointAngles_pub.publish(ros_angles); // Publishes the raw joint angles in a custom message.

	JointState_pub.publish(joint_state);     // Publishes the transformed angles in a standard sensor_msgs format.
}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::BroadCastPosition(void)
{
	JacoPose pose;
	geometry_msgs::PoseStamped current_position;

	arm.GetPosition(pose);
	current_position.pose = pose.Pose();

	ToolPosition_pub.publish(current_position);
}

void JacoArm::BroadCastFingerPosition(void)
{

/*
Publishes the current finger positions.
*/

	CartesianPosition Jaco_Position;
	jaco_driver::FingerPosition finger_position;

	memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
	arm.GetFingers(Jaco_Position.Fingers);

	finger_position.Finger_1 = Jaco_Position.Fingers.Finger1;
	finger_position.Finger_2 = Jaco_Position.Fingers.Finger2;
	finger_position.Finger_3 = Jaco_Position.Fingers.Finger3;

	FingerPosition_pub.publish(finger_position);
}

void JacoArm::StatusTimer(const ros::TimerEvent&)
{
	BroadCastAngles();
	BroadCastPosition();
	BroadCastFingerPosition();
}

}
<|endoftext|>


<commit_before>#include "ros/ros.h"
#include "arm_controller/arm_controller.h"
#include "controllers/armMove.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <getopt.h>
#include "arm_controller/arduino-serial-lib.h"

Arm_Controller::Arm_Controller() {}
void Arm_Controller::run()
{}

void Arm_Controller::init()
{}

char *formatMessage(int base, int shoulder, int shoulder1, int elbow, int elbow1,
                    int wrist, int wrot, int grip);

bool Arm_Controller::armMove(controllers::armMove::Request &req,
          controllers::armMove::Response &res)
{
  int fd;
  int base, shoulder, shoulder1, elbow, elbow1, wrist, wrot, grip;
  base = req.base;
  shoulder = req.shoulder;
  shoulder1=  req.shoulder1;
  elbow = req.elbow;
  elbow1 = req.elbow1;
  wrist = req.wrist;
  wrot = req.wrot;
  grip = req.grip;
  char *portname = (char*)malloc(sizeof(char) * 40);
  strcpy(portname, "/dev/tty.usbserial-A602ZALM");
  printf("copy success\n");
  fd = serialport_init(portname, 9600);
  printf("open success!\n");
  char returnval[256];
  char* teststr = formatMessage(base, shoulder, shoulder1, elbow, elbow1, wrist, wrot, grip);
  //strcpy(teststr, "Start:35,55,55,42,42,66,12,100:End");
  printf("copy success\n");
  serialport_write(fd, teststr);
  printf("write success!\n");
  serialport_read_until(fd, returnval, ':', 256, 10000);
  printf("read success!\n");
  printf("%s\n", returnval);
  free(portname);
  res.success = true;
  return true;
}

char *formatMessage(int base, int shoulder, int shoulder1, int elbow, int elbow1,
                    int wrist, int wrot, int grip) {
  char *teststr = (char *)malloc(sizeof(char) * 256);
  sprintf(teststr, "Start:%d,%d,%d,%d,%d,%d,%d,%d:End", base, shoulder,
          shoulder1, elbow, elbow1, wrist, wrot, grip);
  return teststr;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "arm_controller");
  Arm_Controller armC;
  ros::ServiceServer tester = armC.n.advertiseService("plan2ArmMove", &Arm_Controller::armMove, &armC);
  ros::spin();

  return 0;

}
<commit_msg>fire in the hole<commit_after>#include "ros/ros.h"
#include "arm_controller/arm_controller.h"
#include "controllers/armMove.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <getopt.h>
#include "arm_controller/arduino-serial-lib.h"

Arm_Controller::Arm_Controller() {}
void Arm_Controller::run()
{}

void Arm_Controller::init()
{}

char *formatMessage(int base, int shoulder, int shoulder1, int elbow, int elbow1,
                    int wrist, int wrot, int grip);

bool Arm_Controller::armMove(controllers::armMove::Request &req,
          controllers::armMove::Response &res)
{
  //TODO Change generalize to array of angles for each join
  int fd;
  int base, shoulder, shoulder1, elbow, elbow1, wrist, wrot, grip;
  base = req.base;
  shoulder = req.shoulder;
  shoulder1=  req.shoulder1;
  elbow = req.elbow;
  elbow1 = req.elbow1;
  wrist = req.wrist;
  wrot = req.wrot;
  grip = req.grip;
  char *portname = (char*)malloc(sizeof(char) * 40);
  strcpy(portname, "/dev/tty.usbserial-A602ZALM");
  printf("copy success\n");
  fd = serialport_init(portname, 9600);
  printf("open success!\n");
  char returnval[256];
  char* teststr = formatMessage(base, shoulder, shoulder1, elbow, elbow1, wrist, wrot, grip);
  //strcpy(teststr, "Start:35,55,55,42,42,66,12,100:End");
  printf("copy success\n");
  serialport_write(fd, teststr);
  printf("write success!\n");
  serialport_read_until(fd, returnval, ':', 256, 10000);
  printf("read success!\n");
  printf("%s\n", returnval);
  free(portname);
  res.success = true;
  return true;
}

char *formatMessage(int base, int shoulder, int shoulder1, int elbow, int elbow1,
                    int wrist, int wrot, int grip) {
  char *teststr = (char *)malloc(sizeof(char) * 256);
  sprintf(teststr, "Start:%d,%d,%d,%d,%d,%d,%d,%d:End", base, shoulder,
          shoulder1, elbow, elbow1, wrist, wrot, grip);
  return teststr;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "arm_controller");
  Arm_Controller armC;
  ros::ServiceServer tester = armC.n.advertiseService("plan2ArmMove", &Arm_Controller::armMove, &armC);
  ros::spin();

  return 0;

}
<|endoftext|>


<commit_before>//  (C) Copyright John Maddock 2000.
//  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)

//  See http://www.boost.org/libs/static_assert for documentation.

/*
 Revision history:
   02 August 2000
      Initial version.
*/

#ifndef BOOST_STATIC_ASSERT_HPP
#define BOOST_STATIC_ASSERT_HPP

#include "boost/config.hpp"
#include "boost/detail/workaround.hpp"

#ifdef __BORLANDC__
//
// workaround for buggy integral-constant expression support:
#define BOOST_BUGGY_INTEGRAL_CONSTANT_EXPRESSIONS
#endif

#if defined(__GNUC__) && (__GNUC__ == 3) && ((__GNUC_MINOR__ == 3) || (__GNUC_MINOR__ == 4))
// gcc 3.3 and 3.4 don't produce good error messages with the default version:
#  define BOOST_SA_GCC_WORKAROUND
#endif

//
// If the compiler issues warnings about old C style casts,
// then enable this:
//
#if defined(__GNUC__) && ((__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ >= 4)))
#  define BOOST_STATIC_ASSERT_BOOL_CAST( x ) ((x) == 0 ? false : true)
#else
#  define BOOST_STATIC_ASSERT_BOOL_CAST(x) (bool)(x)
#endif

#ifdef BOOST_HAS_STATIC_ASSERT
#  define BOOST_STATIC_ASSERT( B ) static_assert(B, #B)
#else

namespace boost{

// HP aCC cannot deal with missing names for template value parameters
template <bool x> struct STATIC_ASSERTION_FAILURE;

template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; };

// HP aCC cannot deal with missing names for template value parameters
template<int x> struct static_assert_test{};

}

//
// Implicit instantiation requires that all member declarations be
// instantiated, but that the definitions are *not* instantiated.
//
// It's not particularly clear how this applies to enum's or typedefs;
// both are described as declarations [7.1.3] and [7.2] in the standard,
// however some compilers use "delayed evaluation" of one or more of
// these when implicitly instantiating templates.  We use typedef declarations
// by default, but try defining BOOST_USE_ENUM_STATIC_ASSERT if the enum
// version gets better results from your compiler...
//
// Implementation:
// Both of these versions rely on sizeof(incomplete_type) generating an error
// message containing the name of the incomplete type.  We use
// "STATIC_ASSERTION_FAILURE" as the type name here to generate
// an eye catching error message.  The result of the sizeof expression is either
// used as an enum initialiser, or as a template argument depending which version
// is in use...
// Note that the argument to the assert is explicitly cast to bool using old-
// style casts: too many compilers currently have problems with static_cast
// when used inside integral constant expressions.
//
#if !defined(BOOST_BUGGY_INTEGRAL_CONSTANT_EXPRESSIONS)

#if defined(BOOST_MSVC) && (BOOST_MSVC < 1300)
// __LINE__ macro broken when -ZI is used see Q199057
// fortunately MSVC ignores duplicate typedef's.
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< (bool)( B ) >)\
      > boost_static_assert_typedef_
#elif defined(BOOST_MSVC)
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST ( B ) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __COUNTER__)
#elif defined(BOOST_INTEL_CXX_VERSION) || defined(BOOST_SA_GCC_WORKAROUND)
// agurt 15/sep/02: a special care is needed to force Intel C++ issue an error
// instead of warning in case of failure
# define BOOST_STATIC_ASSERT( B ) \
    typedef char BOOST_JOIN(boost_static_assert_typedef_, __LINE__) \
        [ ::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >::value ]
#elif defined(__sgi)
// special version for SGI MIPSpro compiler
#define BOOST_STATIC_ASSERT( B ) \
   BOOST_STATIC_CONSTANT(bool, \
     BOOST_JOIN(boost_static_assert_test_, __LINE__) = ( B )); \
   typedef ::boost::static_assert_test<\
     sizeof(::boost::STATIC_ASSERTION_FAILURE< \
       BOOST_JOIN(boost_static_assert_test_, __LINE__) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __LINE__)
#elif BOOST_WORKAROUND(__MWERKS__, <= 0x3003)
// special version for CodeWarrior <= 8.x
#define BOOST_STATIC_ASSERT( B ) \
   BOOST_STATIC_CONSTANT(int, \
     BOOST_JOIN(boost_static_assert_test_, __LINE__) = \
       sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >) )
#else
// generic version
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __LINE__)
#endif

#else
// alternative enum based implementation:
#define BOOST_STATIC_ASSERT( B ) \
   enum { BOOST_JOIN(boost_static_assert_enum_, __LINE__) \
      = sizeof(::boost::STATIC_ASSERTION_FAILURE< (bool)( B ) >) }
#endif
#endif // ndef BOOST_HAS_STATIC_ASSERT

#endif // BOOST_STATIC_ASSERT_HPP

<commit_msg>Supress boost STATIC_ASSERT warnings when using gcc > 4.8<commit_after>//  (C) Copyright John Maddock 2000.
//  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)

//  See http://www.boost.org/libs/static_assert for documentation.

/*
 Revision history:
   02 August 2000
      Initial version.
*/

#ifndef BOOST_STATIC_ASSERT_HPP
#define BOOST_STATIC_ASSERT_HPP

#include "boost/config.hpp"
#include "boost/detail/workaround.hpp"

#ifdef __BORLANDC__
//
// workaround for buggy integral-constant expression support:
#define BOOST_BUGGY_INTEGRAL_CONSTANT_EXPRESSIONS
#endif

#if defined(__GNUC__) && (__GNUC__ == 3) && ((__GNUC_MINOR__ == 3) || (__GNUC_MINOR__ == 4))
// gcc 3.3 and 3.4 don't produce good error messages with the default version:
#  define BOOST_SA_GCC_WORKAROUND
#endif

//
// If the compiler issues warnings about old C style casts,
// then enable this:
//
#if defined(__GNUC__) && ((__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ >= 4)))
#  define BOOST_STATIC_ASSERT_BOOL_CAST( x ) ((x) == 0 ? false : true)
#else
#  define BOOST_STATIC_ASSERT_BOOL_CAST(x) (bool)(x)
#endif

#ifdef BOOST_HAS_STATIC_ASSERT
#  define BOOST_STATIC_ASSERT( B ) static_assert(B, #B)
#else

namespace boost{

// HP aCC cannot deal with missing names for template value parameters
template <bool x> struct STATIC_ASSERTION_FAILURE;

template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; };

// HP aCC cannot deal with missing names for template value parameters
template<int x> struct static_assert_test{};

}

//
// Implicit instantiation requires that all member declarations be
// instantiated, but that the definitions are *not* instantiated.
//
// It's not particularly clear how this applies to enum's or typedefs;
// both are described as declarations [7.1.3] and [7.2] in the standard,
// however some compilers use "delayed evaluation" of one or more of
// these when implicitly instantiating templates.  We use typedef declarations
// by default, but try defining BOOST_USE_ENUM_STATIC_ASSERT if the enum
// version gets better results from your compiler...
//
// Implementation:
// Both of these versions rely on sizeof(incomplete_type) generating an error
// message containing the name of the incomplete type.  We use
// "STATIC_ASSERTION_FAILURE" as the type name here to generate
// an eye catching error message.  The result of the sizeof expression is either
// used as an enum initialiser, or as a template argument depending which version
// is in use...
// Note that the argument to the assert is explicitly cast to bool using old-
// style casts: too many compilers currently have problems with static_cast
// when used inside integral constant expressions.
//
#if !defined(BOOST_BUGGY_INTEGRAL_CONSTANT_EXPRESSIONS)

#if defined(BOOST_MSVC) && (BOOST_MSVC < 1300)
// __LINE__ macro broken when -ZI is used see Q199057
// fortunately MSVC ignores duplicate typedef's.
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< (bool)( B ) >)\
      > boost_static_assert_typedef_
#elif defined(BOOST_MSVC)
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST ( B ) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __COUNTER__)
#elif defined(BOOST_INTEL_CXX_VERSION) || defined(BOOST_SA_GCC_WORKAROUND)
// agurt 15/sep/02: a special care is needed to force Intel C++ issue an error
// instead of warning in case of failure
# define BOOST_STATIC_ASSERT( B ) \
    typedef char BOOST_JOIN(boost_static_assert_typedef_, __LINE__) \
        [ ::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >::value ]
#elif defined(__sgi)
// special version for SGI MIPSpro compiler
#define BOOST_STATIC_ASSERT( B ) \
   BOOST_STATIC_CONSTANT(bool, \
     BOOST_JOIN(boost_static_assert_test_, __LINE__) = ( B )); \
   typedef ::boost::static_assert_test<\
     sizeof(::boost::STATIC_ASSERTION_FAILURE< \
       BOOST_JOIN(boost_static_assert_test_, __LINE__) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __LINE__)
#elif BOOST_WORKAROUND(__MWERKS__, <= 0x3003)
// special version for CodeWarrior <= 8.x
#define BOOST_STATIC_ASSERT( B ) \
   BOOST_STATIC_CONSTANT(int, \
     BOOST_JOIN(boost_static_assert_test_, __LINE__) = \
       sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >) )
#else
// generic version
#define BOOST_STATIC_ASSERT( B ) \
   typedef ::boost::static_assert_test<\
      sizeof(::boost::STATIC_ASSERTION_FAILURE< BOOST_STATIC_ASSERT_BOOL_CAST( B ) >)>\
         BOOST_JOIN(boost_static_assert_typedef_, __LINE__) __attribute__((unused))
#endif

#else
// alternative enum based implementation:
#define BOOST_STATIC_ASSERT( B ) \
   enum { BOOST_JOIN(boost_static_assert_enum_, __LINE__) \
      = sizeof(::boost::STATIC_ASSERTION_FAILURE< (bool)( B ) >) }
#endif
#endif // ndef BOOST_HAS_STATIC_ASSERT

#endif // BOOST_STATIC_ASSERT_HPP

<|endoftext|>


<commit_before>#include <catch.hpp>
#include <rapidcheck-catch.h>

#include "rapidcheck/detail/RandomEngine.h"

using namespace rc;
using namespace rc::detail;

#define ENOUGH 10000

TEST_CASE("RandomEngine") {
    SECTION("nextAtom") {
        prop("same seeds yields same sequences of numbers",
             [] (RandomEngine::Seed seed) {
                 RandomEngine r1(seed);
                 RandomEngine r2(seed);
                 for (int i = 0; i < ENOUGH; i++)
                     RC_ASSERT(r1.nextAtom() == r2.nextAtom());
             });

        prop("a copy of a randomEngine yields the same sequences of numbers as"
             " the original",
             [] (RandomEngine::Seed seed) {
                 RandomEngine r1(seed);
                 RandomEngine r2(r1);
                 for (int i = 0; i < ENOUGH; i++)
                     RC_ASSERT(r1.nextAtom() == r2.nextAtom());
             });

        prop("different seeds yields different sequences of numbers",
             [] {
                 auto s1 = *gen::arbitrary<RandomEngine::Seed>();
                 auto s2 = *gen::distinctFrom(s1);

                 RandomEngine r1(s1);
                 RandomEngine r2(s2);
                 std::vector<RandomEngine::Atom> atoms1;
                 std::vector<RandomEngine::Atom> atoms2;
                 for (int i = 0; i < ENOUGH; i++) {
                     atoms1.push_back(r1.nextAtom());
                     atoms2.push_back(r2.nextAtom());
                 }

                 RC_ASSERT(atoms1 != atoms2);
             });
    }
}
<commit_msg>Move RandomEngineTests to new framework<commit_after>#include <catch.hpp>
#include <rapidcheck-catch.h>

#include "rapidcheck/detail/RandomEngine.h"

using namespace rc;
using namespace rc::detail;

#define ENOUGH 10000

TEST_CASE("RandomEngine") {
    SECTION("nextAtom") {
        newprop(
            "same seeds yields same sequences of numbers",
            [] (RandomEngine::Seed seed) {
                RandomEngine r1(seed);
                RandomEngine r2(seed);
                for (int i = 0; i < ENOUGH; i++)
                    RC_ASSERT(r1.nextAtom() == r2.nextAtom());
            });

        newprop(
            "a copy of a randomEngine yields the same sequences of numbers as"
            " the original",
            [] (RandomEngine::Seed seed) {
                RandomEngine r1(seed);
                RandomEngine r2(r1);
                for (int i = 0; i < ENOUGH; i++)
                    RC_ASSERT(r1.nextAtom() == r2.nextAtom());
            });

        newprop(
            "different seeds yields different sequences of numbers",
            [] {
                auto s1 = *newgen::arbitrary<RandomEngine::Seed>();
                auto s2 = *newgen::distinctFrom(s1);

                RandomEngine r1(s1);
                RandomEngine r2(s2);
                std::vector<RandomEngine::Atom> atoms1;
                std::vector<RandomEngine::Atom> atoms2;
                for (int i = 0; i < ENOUGH; i++) {
                    atoms1.push_back(r1.nextAtom());
                    atoms2.push_back(r2.nextAtom());
                }

                RC_ASSERT(atoms1 != atoms2);
            });
    }
}
<|endoftext|>


<commit_before>/* -*- Mode: C++; tab-width: 4; c-basic-offset: 4; indent-tabs-mode: nil -*- */
/*
 *     Copyright 2015 Couchbase, Inc
 *
 *   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.
 */

#include "config.h"

#include <atomic>
#if defined(HAVE_MEMALIGN)
#include <malloc.h>
#endif

#include "daemon/alloc_hooks.h"

std::atomic_size_t alloc_size;

// Test pointer in global scope to prevent compiler optimizing malloc/free away
// via DCE.
char* p;

extern "C" {
    static void NewHook(const void* ptr, size_t) {
        if (ptr != NULL) {
            void* p = const_cast<void*>(ptr);
            alloc_size += AllocHooks::get_allocation_size(p);
        }
    }

    static void DeleteHook(const void* ptr) {
        if (ptr != NULL) {
            void* p = const_cast<void*>(ptr);
            alloc_size -= AllocHooks::get_allocation_size(p);
        }
    }

    static void TestThread(void* arg) {
        alloc_size = 0;

        // Test new & delete //////////////////////////////////////////////////
        p = new char();
        cb_assert(alloc_size > 0);
        delete p;
        cb_assert(alloc_size == 0);

        // Test new[] & delete[] //////////////////////////////////////////////
        p = new char[100];
        cb_assert(alloc_size >= 100);
        delete []p;
        cb_assert(alloc_size == 0);

        // Test malloc() / free() /////////////////////////////////////////////
        p = static_cast<char*>(malloc(sizeof(char) * 10));
        cb_assert(alloc_size >= 10);
        free(p);
        cb_assert(alloc_size == 0);

        // Test realloc() /////////////////////////////////////////////////////
        p = static_cast<char*>(malloc(1));
        cb_assert(alloc_size >= 1);

        // Allocator may round up allocation sizes; so it's hard to
        // accurately predict how much alloc_size will increase. Hence
        // we just increase by a "large" amount and check at least half that
        // increment.
        size_t prev_size = alloc_size;
        p = static_cast<char*>(realloc(p, sizeof(char) * 100));
        cb_assert(alloc_size >= (prev_size + 50));

        prev_size = alloc_size;
        p = static_cast<char*>(realloc(p, 1));
        cb_assert(alloc_size < prev_size);

        prev_size = alloc_size;
        char* q = static_cast<char*>(realloc(NULL, 10));
        cb_assert(alloc_size >= prev_size + 10);

        free(p);
        free(q);
        cb_assert(alloc_size == 0);

        // Test calloc() //////////////////////////////////////////////////////
        p = static_cast<char*>(calloc(sizeof(char), 20));
        cb_assert(alloc_size >= 20);
        free(p);
        cb_assert(alloc_size == 0);

        // Test indirect use of malloc() via strdup() /////////////////////////
        p = strdup("random string");
        cb_assert(alloc_size >= sizeof("random string"));
        free(p);
        cb_assert(alloc_size == 0);

#if defined(HAVE_MEMALIGN)
        // Test memalign //////////////////////////////////////////////////////
        p = static_cast<char*>(memalign(16, 64));
        cb_assert(alloc_size >= 64);
        free(p);
        cb_assert(alloc_size == 0);

        // Test posix_memalign ////////////////////////////////////////////////
        void* ptr;
        cb_assert(posix_memalign(&ptr, 16, 64) == 0);
        cb_assert(alloc_size >= 64);
        free(ptr);
        cb_assert(alloc_size == 0);
#endif
    }
}

int main(void) {
    AllocHooks::initialize();

   AllocHooks::add_new_hook(NewHook);
   AllocHooks::add_delete_hook(DeleteHook);

   cb_thread_t tid;
   cb_assert(cb_create_thread(&tid, TestThread, 0, 0) == 0);
   cb_assert(cb_join_thread(tid) == 0);

   AllocHooks::remove_new_hook(NewHook);
   AllocHooks::remove_delete_hook(DeleteHook);

   return 0;
}
<commit_msg>MB-20586: Fix build break on OS X due to missing strdup header<commit_after>/* -*- Mode: C++; tab-width: 4; c-basic-offset: 4; indent-tabs-mode: nil -*- */
/*
 *     Copyright 2015 Couchbase, Inc
 *
 *   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.
 */

#include "config.h"

#include <atomic>
#include <cstring>
#if defined(HAVE_MEMALIGN)
#include <malloc.h>
#endif

#include "daemon/alloc_hooks.h"

std::atomic_size_t alloc_size;

// Test pointer in global scope to prevent compiler optimizing malloc/free away
// via DCE.
char* p;

extern "C" {
    static void NewHook(const void* ptr, size_t) {
        if (ptr != NULL) {
            void* p = const_cast<void*>(ptr);
            alloc_size += AllocHooks::get_allocation_size(p);
        }
    }

    static void DeleteHook(const void* ptr) {
        if (ptr != NULL) {
            void* p = const_cast<void*>(ptr);
            alloc_size -= AllocHooks::get_allocation_size(p);
        }
    }

    static void TestThread(void* arg) {
        alloc_size = 0;

        // Test new & delete //////////////////////////////////////////////////
        p = new char();
        cb_assert(alloc_size > 0);
        delete p;
        cb_assert(alloc_size == 0);

        // Test new[] & delete[] //////////////////////////////////////////////
        p = new char[100];
        cb_assert(alloc_size >= 100);
        delete []p;
        cb_assert(alloc_size == 0);

        // Test malloc() / free() /////////////////////////////////////////////
        p = static_cast<char*>(malloc(sizeof(char) * 10));
        cb_assert(alloc_size >= 10);
        free(p);
        cb_assert(alloc_size == 0);

        // Test realloc() /////////////////////////////////////////////////////
        p = static_cast<char*>(malloc(1));
        cb_assert(alloc_size >= 1);

        // Allocator may round up allocation sizes; so it's hard to
        // accurately predict how much alloc_size will increase. Hence
        // we just increase by a "large" amount and check at least half that
        // increment.
        size_t prev_size = alloc_size;
        p = static_cast<char*>(realloc(p, sizeof(char) * 100));
        cb_assert(alloc_size >= (prev_size + 50));

        prev_size = alloc_size;
        p = static_cast<char*>(realloc(p, 1));
        cb_assert(alloc_size < prev_size);

        prev_size = alloc_size;
        char* q = static_cast<char*>(realloc(NULL, 10));
        cb_assert(alloc_size >= prev_size + 10);

        free(p);
        free(q);
        cb_assert(alloc_size == 0);

        // Test calloc() //////////////////////////////////////////////////////
        p = static_cast<char*>(calloc(sizeof(char), 20));
        cb_assert(alloc_size >= 20);
        free(p);
        cb_assert(alloc_size == 0);

        // Test indirect use of malloc() via strdup() /////////////////////////
        p = strdup("random string");
        cb_assert(alloc_size >= sizeof("random string"));
        free(p);
        cb_assert(alloc_size == 0);

#if defined(HAVE_MEMALIGN)
        // Test memalign //////////////////////////////////////////////////////
        p = static_cast<char*>(memalign(16, 64));
        cb_assert(alloc_size >= 64);
        free(p);
        cb_assert(alloc_size == 0);

        // Test posix_memalign ////////////////////////////////////////////////
        void* ptr;
        cb_assert(posix_memalign(&ptr, 16, 64) == 0);
        cb_assert(alloc_size >= 64);
        free(ptr);
        cb_assert(alloc_size == 0);
#endif
    }
}

int main(void) {
    AllocHooks::initialize();

   AllocHooks::add_new_hook(NewHook);
   AllocHooks::add_delete_hook(DeleteHook);

   cb_thread_t tid;
   cb_assert(cb_create_thread(&tid, TestThread, 0, 0) == 0);
   cb_assert(cb_join_thread(tid) == 0);

   AllocHooks::remove_new_hook(NewHook);
   AllocHooks::remove_delete_hook(DeleteHook);

   return 0;
}
<|endoftext|>