<commit_before>#include "sceneGridHandler.h"
#include "nodeElement.h"
#include "../view/editorViewScene.h"

namespace {
// TODO: find a way to remove it
// magic constants
const int widthLineX = 15000;
const int widthLineY = 11000;
}

SceneGridHandler::SceneGridHandler(NodeElement *node)
		: mNode(node), mGuidesPixmap(NULL)
{
	mGuidesPen = QPen(QColor(0, 0, 0, 42), 1, Qt::DashLine);
	mShowAlignment = SettingsManager::value("ShowAlignment", true).toBool();
	mSwitchGrid = SettingsManager::value("ActivateGrid", true).toBool();
	mSwitchAlignment = SettingsManager::value("ActivateAlignment", true).toBool();
}

SceneGridHandler::~SceneGridHandler()
{
	delete mGuidesPixmap;
}

void SceneGridHandler::delUnusedLines()
{
	EditorViewScene *evScene = dynamic_cast<EditorViewScene *>(mNode->scene());
	evScene->deleteFromForeground(mGuidesPixmap);
	delete mGuidesPixmap;
	mGuidesPixmap = NULL;
	mLines.clear();
}

void SceneGridHandler::drawLineY(qreal pointY, QRectF const &sceneRect)
{
	pointY -= sceneRect.y();
	QLineF const newLine(0, pointY, sceneRect.width(), pointY);

	// checking whether the scene already has this line or not.
	// if not (lineIsFound is false), then adding it
	foreach (QLineF const &line, mLines) {
		if (qAbs(line.y1() - newLine.y1()) < indistinguishabilitySpace
				&& line.y2() == line.y1())
		{
			return;
		}
	}

	mLines.push_back(newLine);
}

void SceneGridHandler::drawLineX(qreal pointX, QRectF const &sceneRect)
{
	pointX -= sceneRect.x();
	QLineF const newLine(pointX, 0, pointX, sceneRect.height());

	// checking whether the scene already has this line or not.
	// if not (lineIsFound is false), then adding it
	foreach (QLineF const &line, mLines) {
		if (qAbs(line.x1() - newLine.x1()) < indistinguishabilitySpace
				&& line.x2() == line.x1())
		{
			return;
		}
	}

	mLines.push_back(newLine);
}

// checking whether we should align with the vertical line or not
bool SceneGridHandler::makeJumpX(qreal deltaX, qreal pointX)
{
	if (mSwitchAlignment && deltaX <= radiusJump) {
		mNode->setX(pointX - mNode->contentsRect().x());
		return true;
	}
	return false;
}

// checking whether we should align with the horizontal line or not
bool SceneGridHandler::makeJumpY(qreal deltaY, qreal pointY)
{
	if (mSwitchAlignment && deltaY <= radiusJump) {
		mNode->setY(pointY - mNode->contentsRect().y());
		return true;
	}
	return false;
}

// build a vertical line: draw it and check for alignment
void SceneGridHandler::buildLineX(qreal deltaX
		, qreal pointX, qreal correctionX, qreal &myX1, qreal &myX2, QRectF const &sceneRect)
{
	if (deltaX > radius) {
		return;
	}

	if (mShowAlignment) {
		drawLineX(pointX, sceneRect);
	}
	if (makeJumpX(deltaX, pointX - correctionX)) {
		myX1 = recalculateX1();
		myX2 = recalculateX2(myX1);
	}
}

// build a horizontal line: draw it and check for alignment
void SceneGridHandler::buildLineY(qreal deltaY
		, qreal pointY, qreal correctionY, qreal &myY1, qreal &myY2, QRectF const &sceneRect)
{
	if (deltaY > radius) {
		return;
	}

	if (mShowAlignment) {
		drawLineY(pointY, sceneRect);
	}
	if (makeJumpY(deltaY, pointY - correctionY)) {
		myY1 = recalculateY1();
		myY2 = recalculateY2(myY1);
	}
}

qreal SceneGridHandler::recalculateX1() const
{
	return mNode->scenePos().x() + mNode->boundingRect().x();
}

qreal SceneGridHandler::recalculateX2(qreal myX1) const
{
	return myX1 + mNode->boundingRect().width();
}

qreal SceneGridHandler::recalculateY1() const
{
	return mNode->scenePos().y() + mNode->boundingRect().y();
}

qreal SceneGridHandler::recalculateY2(qreal myY1) const
{
	return myY1 + mNode->boundingRect().height();
}

// move element vertically according to the grid
void SceneGridHandler::makeGridMovingX(qreal myX, int coef, int indexGrid)
{
	int coefSign = coef != 0 ? coef / qAbs(coef) : 0;

	int distToCeilTop = qAbs(qAbs(myX) - qAbs(coef) * indexGrid);
	int distToCeilBottom = qAbs(qAbs(myX) - (qAbs(coef) + 1) * indexGrid);

	if (distToCeilTop * 2 <= indexGrid) {
		mNode->setX(coef * indexGrid);
		mNode->adjustLinks();
	} else if (distToCeilBottom * 2 < indexGrid) {
		// TODO: understand why we sum coef and coefSign. Comments are welcome
		mNode->setX((coef + coefSign) * indexGrid);
		mNode->adjustLinks();
	}
}

// move element horizontally according to the grid
void SceneGridHandler::makeGridMovingY(qreal myY, int coef, int indexGrid)
{
	int coefSign = coef != 0 ? coef / qAbs(coef) : 0;

	int distToCeilTop = qAbs(qAbs(myY) - qAbs(coef) * indexGrid);
	int distToCeilBottom = qAbs(qAbs(myY) - (qAbs(coef) + 1) * indexGrid);

	if (distToCeilTop * 2 <= indexGrid) {
		mNode->setY(coef * indexGrid);
		mNode->adjustLinks();
	} else if (distToCeilBottom < indexGrid) {
		// TODO: understand why we sum coef and coefSign. Comments are welcome
		mNode->setY((coef + coefSign) * indexGrid);
		mNode->adjustLinks();
	}
}

void SceneGridHandler::setShowAlignmentMode(bool mode)
{
	mShowAlignment = mode;
}

void SceneGridHandler::setGridMode(bool mode)
{
	mSwitchGrid = mode;
}

void SceneGridHandler::setAlignmentMode(bool mode)
{
	mSwitchAlignment = mode;
}

QList<QGraphicsItem *> SceneGridHandler::getAdjancedNodes() const
{
	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();

	// verical
	QList<QGraphicsItem *> listX = mNode->scene()->items(nodeScenePos.x(), 0
			, contentsRect.width(), widthLineY
			, Qt::IntersectsItemBoundingRect);

	// horizontal
	QList<QGraphicsItem *> listY = mNode->scene()->items(0, nodeScenePos.y()
			, widthLineX, contentsRect.height()
			, Qt::IntersectsItemBoundingRect);

	return listX + listY;
}

void SceneGridHandler::alignToGrid()
{
	if (!mSwitchGrid) {
		return;
	}
	int const indexGrid = SettingsManager::value("IndexGrid", 50).toInt();

	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();

	qreal myX1 = nodeScenePos.x() + contentsRect.x();
	qreal myY1 = nodeScenePos.y() + contentsRect.y();

	int coefX = static_cast<int>(myX1) / indexGrid;
	int coefY = static_cast<int>(myY1) / indexGrid;

	makeGridMovingX(myX1, coefX, indexGrid);
	makeGridMovingY(myY1, coefY, indexGrid);

	myX1 = nodeScenePos.x() + contentsRect.x();
	myY1 = nodeScenePos.y() + contentsRect.y();
}

void SceneGridHandler::drawGuides()
{
	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();
	QRectF const sceneRect = mNode->scene()->sceneRect();

	delUnusedLines();

	QList<QGraphicsItem *> list = getAdjancedNodes();

	qreal myX1 = nodeScenePos.x() + contentsRect.x();
	qreal myY1 = nodeScenePos.y() + contentsRect.y();
	qreal myX2 = myX1 + contentsRect.width();
	qreal myY2 = myY1 + contentsRect.height();

	foreach (QGraphicsItem *graphicsItem, list) {
		NodeElement *item = dynamic_cast<NodeElement *>(graphicsItem);
		if (item == NULL || item->parentItem() != NULL || item == mNode) {
			continue;
		}

		QPointF const point = item->scenePos();
		QRectF const contents = item->contentsRect();

		qreal const pointX1 = point.x() + contents.x() - spacing;
		qreal const pointY1  = point.y() + contents.y() - spacing;
		qreal const pointX2 = pointX1  + contents.width() + 2 * spacing;
		qreal const pointY2  = pointY1 + contents.height() + 2 * spacing;

		if (pointX1 != myX1 || pointY1 != myY1) {
			qreal const deltaY1 = qAbs(pointY1 - myY1);
			qreal const deltaY2 = qAbs(pointY2 - myY2);
			qreal const deltaX1 = qAbs(pointX1 - myX1);
			qreal const deltaX2 = qAbs(pointX2 - myX2);

			buildLineY(deltaY1, pointY1, 0, myY1, myY2, sceneRect);
			buildLineY(deltaY2, pointY2, contentsRect.height(), myY1, myY2, sceneRect);

			buildLineX(deltaX1, pointX1, 0, myX1, myX2, sceneRect);
			buildLineX(deltaX2, pointX2, contentsRect.width(), myX1, myX2, sceneRect);

			buildLineY(qAbs(pointY1 - myY2), pointY1, contentsRect.height(), myY1, myY2, sceneRect);
			buildLineX(qAbs(pointX1 - myX2), pointX1, contentsRect.width(), myX1, myX2, sceneRect);

			buildLineY(qAbs(pointY2 - myY1), pointY2, 0, myY1, myY2, sceneRect);
			buildLineX(qAbs(pointX2 - myX1), pointX2, 0, myX1, myX2, sceneRect);
		}
	}

	if (mLines.size()) {
		EditorViewScene *evScene = dynamic_cast<EditorViewScene *>(mNode->scene());

		mGuidesPixmap = new QPixmap(sceneRect.width(), sceneRect.height());
		mGuidesPixmap->fill(Qt::transparent);

		QPainter painter(mGuidesPixmap);
		painter.setPen(mGuidesPen);
		painter.drawLines(mLines);
		evScene->putOnForeground(mGuidesPixmap);
	}
}

void SceneGridHandler::mouseMoveEvent(QGraphicsSceneMouseEvent *event)
{
	Q_UNUSED(event)
	NodeElement *parItem = dynamic_cast<NodeElement*>(mNode->parentItem());
	if (parItem != NULL) {
		return;
	}

	alignToGrid();
	drawGuides();
	mNode->adjustLinks();
}
<commit_msg>modified sceneGridHandler.cpp<commit_after>#include "sceneGridHandler.h"
#include "nodeElement.h"
#include "../view/editorViewScene.h"

namespace {
// TODO: find a way to remove it
// magic constants
const int widthLineX = 15000;
const int widthLineY = 11000;
}

SceneGridHandler::SceneGridHandler(NodeElement *node)
		: mNode(node), mGuidesPixmap(NULL)
{
	mGuidesPen = QPen(QColor(0, 0, 0, 42), 1, Qt::DashLine);
	mShowAlignment = SettingsManager::value("ShowAlignment", true).toBool();
	mSwitchGrid = SettingsManager::value("ActivateGrid", true).toBool();
	mSwitchAlignment = SettingsManager::value("ActivateAlignment", true).toBool();
}

SceneGridHandler::~SceneGridHandler()
{
	delete mGuidesPixmap;
}

void SceneGridHandler::delUnusedLines()
{
	EditorViewScene *evScene = dynamic_cast<EditorViewScene *>(mNode->scene());
	evScene->deleteFromForeground(mGuidesPixmap);
	delete mGuidesPixmap;
	mGuidesPixmap = NULL;
	mLines.clear();
}

void SceneGridHandler::drawLineY(qreal pointY, QRectF const &sceneRect)
{
	pointY -= sceneRect.y();
	QLineF const newLine(0, pointY, sceneRect.width(), pointY);

	// checking whether the scene already has this line or not.
	// if not (lineIsFound is false), then adding it
	foreach (QLineF const &line, mLines) {
		if (qAbs(line.y1() - newLine.y1()) < indistinguishabilitySpace
				&& line.y2() == line.y1())
		{
			return;
		}
	}

	mLines.push_back(newLine);
}

void SceneGridHandler::drawLineX(qreal pointX, QRectF const &sceneRect)
{
	pointX -= sceneRect.x();
	QLineF const newLine(pointX, 0, pointX, sceneRect.height());

	// checking whether the scene already has this line or not.
	// if not (lineIsFound is false), then adding it
	foreach (QLineF const &line, mLines) {
		if (qAbs(line.x1() - newLine.x1()) < indistinguishabilitySpace
				&& line.x2() == line.x1())
		{
			return;
		}
	}

	mLines.push_back(newLine);
}

// checking whether we should align with the vertical line or not
bool SceneGridHandler::makeJumpX(qreal deltaX, qreal pointX)
{
	if (mSwitchAlignment && deltaX <= radiusJump) {
		mNode->setX(pointX - mNode->contentsRect().x());
		return true;
	}
	return false;
}

// checking whether we should align with the horizontal line or not
bool SceneGridHandler::makeJumpY(qreal deltaY, qreal pointY)
{
	if (mSwitchAlignment && deltaY <= radiusJump) {
		mNode->setY(pointY - mNode->contentsRect().y());
		return true;
	}
	return false;
}

// build a vertical line: draw it and check for alignment
void SceneGridHandler::buildLineX(qreal deltaX
		, qreal pointX, qreal correctionX, qreal &myX1, qreal &myX2, QRectF const &sceneRect)
{
	if (deltaX > radius) {
		return;
	}

	if (mShowAlignment) {
		drawLineX(pointX, sceneRect);
	}
	if (makeJumpX(deltaX, pointX - correctionX)) {
		myX1 = recalculateX1();
		myX2 = recalculateX2(myX1);
	}
}

// build a horizontal line: draw it and check for alignment
void SceneGridHandler::buildLineY(qreal deltaY
		, qreal pointY, qreal correctionY, qreal &myY1, qreal &myY2, QRectF const &sceneRect)
{
	if (deltaY > radius) {
		return;
	}

	if (mShowAlignment) {
		drawLineY(pointY, sceneRect);
	}
	if (makeJumpY(deltaY, pointY - correctionY)) {
		myY1 = recalculateY1();
		myY2 = recalculateY2(myY1);
	}
}

qreal SceneGridHandler::recalculateX1() const
{
	return mNode->scenePos().x() + mNode->boundingRect().x();
}

qreal SceneGridHandler::recalculateX2(qreal myX1) const
{
	return myX1 + mNode->boundingRect().width();
}

qreal SceneGridHandler::recalculateY1() const
{
	return mNode->scenePos().y() + mNode->boundingRect().y();
}

qreal SceneGridHandler::recalculateY2(qreal myY1) const
{
	return myY1 + mNode->boundingRect().height();
}

qreal SceneGridHandler::alignedCoordinate(qreal coord, int coef, int indexGrid)
{
	int const coefSign = coef != 0 ? coef / qAbs(coef) : 0;

	if (qAbs(qAbs(coord) - qAbs(coef) * indexGrid) <= indexGrid / 2) {
		return coef * indexGrid;
	} else if (qAbs(qAbs(coord) - (qAbs(coef) + 1) * indexGrid) < indexGrid / 2) {
		return (coef + coefSign) * indexGrid;
	}
	return coord;
}

void SceneGridHandler::makeGridMovingX(qreal myX, int coef, int indexGrid)
{
	mNode->setX(alignedCoordinate(myX, coef, indexGrid));
	mNode->adjustLinks();
}

void SceneGridHandler::makeGridMovingY(qreal myY, int coef, int indexGrid)
{
	mNode->setY(alignedCoordinate(myY, coef, indexGrid));
	mNode->adjustLinks();
}

qreal SceneGridHandler::makeGridAlignment(qreal coord)
{
	int const indexGrid = SettingsManager::value("IndexGrid", 50).toInt();
	int const coef = static_cast<int>(coord) / indexGrid;
	return alignedCoordinate(coord, coef, indexGrid);
}

void SceneGridHandler::setShowAlignmentMode(bool mode)
{
	mShowAlignment = mode;
}

void SceneGridHandler::setGridMode(bool mode)
{
	mSwitchGrid = mode;
}

void SceneGridHandler::setAlignmentMode(bool mode)
{
	mSwitchAlignment = mode;
}

QList<QGraphicsItem *> SceneGridHandler::getAdjancedNodes() const
{
	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();

	// verical
	QList<QGraphicsItem *> listX = mNode->scene()->items(nodeScenePos.x(), 0
			, contentsRect.width(), widthLineY
			, Qt::IntersectsItemBoundingRect);

	// horizontal
	QList<QGraphicsItem *> listY = mNode->scene()->items(0, nodeScenePos.y()
			, widthLineX, contentsRect.height()
			, Qt::IntersectsItemBoundingRect);

	return listX + listY;
}

void SceneGridHandler::alignToGrid()
{
	if (!mSwitchGrid) {
		return;
	}
	int const indexGrid = SettingsManager::value("IndexGrid", 50).toInt();

	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();

	qreal myX1 = nodeScenePos.x() + contentsRect.x();
	qreal myY1 = nodeScenePos.y() + contentsRect.y();

	int coefX = static_cast<int>(myX1) / indexGrid;
	int coefY = static_cast<int>(myY1) / indexGrid;

	makeGridMovingX(myX1, coefX, indexGrid);
	makeGridMovingY(myY1, coefY, indexGrid);

	myX1 = nodeScenePos.x() + contentsRect.x();
	myY1 = nodeScenePos.y() + contentsRect.y();
}

void SceneGridHandler::drawGuides()
{
	QPointF const nodeScenePos = mNode->scenePos();
	QRectF const contentsRect = mNode->contentsRect();
	QRectF const sceneRect = mNode->scene()->sceneRect();

	delUnusedLines();

	QList<QGraphicsItem *> list = getAdjancedNodes();

	qreal myX1 = nodeScenePos.x() + contentsRect.x();
	qreal myY1 = nodeScenePos.y() + contentsRect.y();
	qreal myX2 = myX1 + contentsRect.width();
	qreal myY2 = myY1 + contentsRect.height();

	foreach (QGraphicsItem *graphicsItem, list) {
		NodeElement *item = dynamic_cast<NodeElement *>(graphicsItem);
		if (item == NULL || item->parentItem() != NULL || item == mNode) {
			continue;
		}

		QPointF const point = item->scenePos();
		QRectF const contents = item->contentsRect();

		qreal const pointX1 = point.x() + contents.x() - spacing;
		qreal const pointY1  = point.y() + contents.y() - spacing;
		qreal const pointX2 = pointX1  + contents.width() + 2 * spacing;
		qreal const pointY2  = pointY1 + contents.height() + 2 * spacing;

		if (pointX1 != myX1 || pointY1 != myY1) {
			qreal const deltaY1 = qAbs(pointY1 - myY1);
			qreal const deltaY2 = qAbs(pointY2 - myY2);
			qreal const deltaX1 = qAbs(pointX1 - myX1);
			qreal const deltaX2 = qAbs(pointX2 - myX2);

			buildLineY(deltaY1, pointY1, 0, myY1, myY2, sceneRect);
			buildLineY(deltaY2, pointY2, contentsRect.height(), myY1, myY2, sceneRect);

			buildLineX(deltaX1, pointX1, 0, myX1, myX2, sceneRect);
			buildLineX(deltaX2, pointX2, contentsRect.width(), myX1, myX2, sceneRect);

			buildLineY(qAbs(pointY1 - myY2), pointY1, contentsRect.height(), myY1, myY2, sceneRect);
			buildLineX(qAbs(pointX1 - myX2), pointX1, contentsRect.width(), myX1, myX2, sceneRect);

			buildLineY(qAbs(pointY2 - myY1), pointY2, 0, myY1, myY2, sceneRect);
			buildLineX(qAbs(pointX2 - myX1), pointX2, 0, myX1, myX2, sceneRect);
		}
	}

	if (mLines.size()) {
		EditorViewScene *evScene = dynamic_cast<EditorViewScene *>(mNode->scene());

		mGuidesPixmap = new QPixmap(sceneRect.width(), sceneRect.height());
		mGuidesPixmap->fill(Qt::transparent);

		QPainter painter(mGuidesPixmap);
		painter.setPen(mGuidesPen);
		painter.drawLines(mLines);
		evScene->putOnForeground(mGuidesPixmap);
	}
}

void SceneGridHandler::mouseMoveEvent(QGraphicsSceneMouseEvent *event)
{
	Q_UNUSED(event)
	NodeElement *parItem = dynamic_cast<NodeElement*>(mNode->parentItem());
	if (parItem != NULL) {
		return;
	}

	alignToGrid();
	drawGuides();
	mNode->adjustLinks();
}
<|endoftext|>


<commit_before>// Copyright 2015 Open Source Robotics Foundation, 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.

#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_

#include <string>

#include <rmw/rmw.h>

#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>

#include <rcl_interfaces/GetParameters.h>
#include <rcl_interfaces/GetParameterTypes.h>
#include <rcl_interfaces/Parameter.h>
#include <rcl_interfaces/ParameterDescriptor.h>
#include <rcl_interfaces/ParameterType.h>
#include <rcl_interfaces/SetParameters.h>
#include <rcl_interfaces/SetParametersAtomically.h>
#include <rcl_interfaces/ListParameters.h>
#include <rcl_interfaces/DescribeParameters.h>

namespace rclcpp
{

namespace parameter_service
{

class ParameterService
{

public:
  RCLCPP_MAKE_SHARED_DEFINITIONS(ParameterService);

  ParameterService(const rclcpp::node::Node::SharedPtr & node)
  : node_(node)
  {
    get_parameters_service_ = node_->create_service<rcl_interfaces::GetParameters>(
      "get_parameters_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::GetParameters::Request> request,
        std::shared_ptr<rcl_interfaces::GetParameters::Response> response)
        {
          auto values = node->get_parameters(request->names);
          std::transform(values.cbegin(), values.cend(), std::back_inserter(response->values),
          [](const rclcpp::parameter::ParameterVariant & pvariant) {
            return pvariant.
            get_parameter_value();
          });
        }
      );

    get_parameter_types_service_ = node_->create_service<rcl_interfaces::GetParameterTypes>(
      "get_parameter_types_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::GetParameterTypes::Request> request,
        std::shared_ptr<rcl_interfaces::GetParameterTypes::Response> response)
        {
          auto types = node->get_parameter_types(request->parameter_names);
          std::transform(types.cbegin(), types.cend(),
          std::back_inserter(response->parameter_types), [](const uint8_t & type) {
            return static_cast<rclcpp::parameter::ParameterType>(type);
          });
        }
      );

    set_parameters_service_ = node_->create_service<rcl_interfaces::SetParameters>(
      "set_parameters_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::SetParameters::Request> request,
        std::shared_ptr<rcl_interfaces::SetParameters::Response> response)
        {
          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
          std::transform(request->parameters.cbegin(), request->parameters.cend(),
          std::back_inserter(pvariants),
          [](const rcl_interfaces::Parameter & p) {
            return rclcpp::parameter::ParameterVariant::
            from_parameter(p);
          });
          auto results = node->set_parameters(pvariants);
          response->results = results;
        }
      );

    set_parameters_atomically_service_ =
      node_->create_service<rcl_interfaces::SetParametersAtomically>(
      "set_parameters_atomically_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::SetParametersAtomically::Request> request,
        std::shared_ptr<rcl_interfaces::SetParametersAtomically::Response> response)
        {
          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
          std::transform(request->parameters.cbegin(), request->parameters.cend(),
          std::back_inserter(pvariants),
          [](const rcl_interfaces::Parameter & p) {
            return rclcpp::parameter::ParameterVariant::
            from_parameter(p);
          });
          auto result = node->set_parameters_atomically(pvariants);
          response->result = result;
        }
      );

    describe_parameters_service_ = node_->create_service<rcl_interfaces::DescribeParameters>(
      "describe_parameters_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::DescribeParameters::Request> request,
        std::shared_ptr<rcl_interfaces::DescribeParameters::Response> response)
        {
          auto descriptors = node->describe_parameters(request->names);
          response->descriptors = descriptors;
        }
      );

    list_parameters_service_ = node_->create_service<rcl_interfaces::ListParameters>(
      "describe_parameters_" + node_->get_name(), [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::ListParameters::Request> request,
        std::shared_ptr<rcl_interfaces::ListParameters::Response> response)
        {
          auto result = node->list_parameters(request->parameter_prefixes, request->depth);
          response->result = result;
        }
      );

  }

private:
  const rclcpp::node::Node::SharedPtr node_;
  rclcpp::service::Service<rcl_interfaces::GetParameters>::SharedPtr get_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::GetParameterTypes>::SharedPtr
    get_parameter_types_service_;
  rclcpp::service::Service<rcl_interfaces::SetParameters>::SharedPtr set_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::SetParametersAtomically>::SharedPtr
    set_parameters_atomically_service_;
  rclcpp::service::Service<rcl_interfaces::DescribeParameters>::SharedPtr
    describe_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::ListParameters>::SharedPtr list_parameters_service_;
};

} /* namespace parameter_service */

} /* namespace rclcpp */

#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
<commit_msg>Prepend parameter methods with the node name<commit_after>// Copyright 2015 Open Source Robotics Foundation, 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.

#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_

#include <string>

#include <rmw/rmw.h>

#include <rclcpp/executors.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/parameter.hpp>

#include <rcl_interfaces/GetParameters.h>
#include <rcl_interfaces/GetParameterTypes.h>
#include <rcl_interfaces/Parameter.h>
#include <rcl_interfaces/ParameterDescriptor.h>
#include <rcl_interfaces/ParameterType.h>
#include <rcl_interfaces/SetParameters.h>
#include <rcl_interfaces/SetParametersAtomically.h>
#include <rcl_interfaces/ListParameters.h>
#include <rcl_interfaces/DescribeParameters.h>

namespace rclcpp
{

namespace parameter_service
{

class ParameterService
{

public:
  RCLCPP_MAKE_SHARED_DEFINITIONS(ParameterService);

  ParameterService(const rclcpp::node::Node::SharedPtr & node)
  : node_(node)
  {
    get_parameters_service_ = node_->create_service<rcl_interfaces::GetParameters>(
      node_->get_name() + "__get_parameters", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::GetParameters::Request> request,
        std::shared_ptr<rcl_interfaces::GetParameters::Response> response)
        {
          auto values = node->get_parameters(request->names);
          std::transform(values.cbegin(), values.cend(), std::back_inserter(response->values),
          [](const rclcpp::parameter::ParameterVariant & pvariant) {
            return pvariant.
            get_parameter_value();
          });
        }
      );

    get_parameter_types_service_ = node_->create_service<rcl_interfaces::GetParameterTypes>(
      node_->get_name() + "__get_parameter_types", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::GetParameterTypes::Request> request,
        std::shared_ptr<rcl_interfaces::GetParameterTypes::Response> response)
        {
          auto types = node->get_parameter_types(request->parameter_names);
          std::transform(types.cbegin(), types.cend(),
          std::back_inserter(response->parameter_types), [](const uint8_t & type) {
            return static_cast<rclcpp::parameter::ParameterType>(type);
          });
        }
      );

    set_parameters_service_ = node_->create_service<rcl_interfaces::SetParameters>(
      node_->get_name() + "__set_parameters", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::SetParameters::Request> request,
        std::shared_ptr<rcl_interfaces::SetParameters::Response> response)
        {
          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
          std::transform(request->parameters.cbegin(), request->parameters.cend(),
          std::back_inserter(pvariants),
          [](const rcl_interfaces::Parameter & p) {
            return rclcpp::parameter::ParameterVariant::
            from_parameter(p);
          });
          auto results = node->set_parameters(pvariants);
          response->results = results;
        }
      );

    set_parameters_atomically_service_ =
      node_->create_service<rcl_interfaces::SetParametersAtomically>(
      node_->get_name() + "__set_parameters_atomically", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::SetParametersAtomically::Request> request,
        std::shared_ptr<rcl_interfaces::SetParametersAtomically::Response> response)
        {
          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
          std::transform(request->parameters.cbegin(), request->parameters.cend(),
          std::back_inserter(pvariants),
          [](const rcl_interfaces::Parameter & p) {
            return rclcpp::parameter::ParameterVariant::
            from_parameter(p);
          });
          auto result = node->set_parameters_atomically(pvariants);
          response->result = result;
        }
      );

    describe_parameters_service_ = node_->create_service<rcl_interfaces::DescribeParameters>(
      node_->get_name() + "__describe_parameters", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::DescribeParameters::Request> request,
        std::shared_ptr<rcl_interfaces::DescribeParameters::Response> response)
        {
          auto descriptors = node->describe_parameters(request->names);
          response->descriptors = descriptors;
        }
      );

    list_parameters_service_ = node_->create_service<rcl_interfaces::ListParameters>(
      node_->get_name() + "__describe_parameters", [&node](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<rcl_interfaces::ListParameters::Request> request,
        std::shared_ptr<rcl_interfaces::ListParameters::Response> response)
        {
          auto result = node->list_parameters(request->parameter_prefixes, request->depth);
          response->result = result;
        }
      );

  }

private:
  const rclcpp::node::Node::SharedPtr node_;
  rclcpp::service::Service<rcl_interfaces::GetParameters>::SharedPtr get_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::GetParameterTypes>::SharedPtr
    get_parameter_types_service_;
  rclcpp::service::Service<rcl_interfaces::SetParameters>::SharedPtr set_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::SetParametersAtomically>::SharedPtr
    set_parameters_atomically_service_;
  rclcpp::service::Service<rcl_interfaces::DescribeParameters>::SharedPtr
    describe_parameters_service_;
  rclcpp::service::Service<rcl_interfaces::ListParameters>::SharedPtr list_parameters_service_;
};

} /* namespace parameter_service */

} /* namespace rclcpp */

#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
<|endoftext|>


<commit_before>// Copyright 2021 Google LLC
//
// 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
//
//     https://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 "protocol_driver_mercury.h"

#include <mercury.h>
#include <mercury_bulk.h>
#include <mercury_macros.h>
#include <mercury_proc_string.h>

#include "absl/base/const_init.h"
#include "absl/strings/str_replace.h"
#include "absl/synchronization/mutex.h"
#include "distbench_utils.h"
#include "glog/logging.h"

namespace {
ABSL_CONST_INIT absl::Mutex mercury_init_mutex(absl::kConstInit);
}  // namespace

namespace distbench {

ProtocolDriverMercury::ProtocolDriverMercury() {}

absl::Status ProtocolDriverMercury::Initialize(
    const ProtocolDriverOptions& pd_opts, int* port) {
  std::string netdev_name = pd_opts.netdev_name();
  auto maybe_ip = IpAddressForDevice(netdev_name, 4);
  if (!maybe_ip.ok()) return maybe_ip.status();
  server_ip_address_ = maybe_ip.value();
  server_socket_address_ = SocketAddressForIp(server_ip_address_, *port);

  // TODO: Choosen interface does not seem respected
  std::string info_string =
      GetNamedSettingString(pd_opts.server_settings(), "hg_init_info_string",
                            "ofi+tcp://__SERVER_IP__");
  info_string = absl::StrReplaceAll(
      info_string, {{"__SERVER_IP__", server_socket_address_}});
  {
    absl::MutexLock l(&mercury_init_mutex);
    hg_class_ = HG_Init(info_string.c_str(), /*listen=*/true);
  }
  if (hg_class_ == nullptr) {
    return absl::UnknownError("HG_Init: failed");
  }

  hg_context_ = HG_Context_create(hg_class_);
  if (hg_context_ == nullptr) {
    return absl::UnknownError("HG_Context_create: failed");
  }

  mercury_generic_rpc_id_ = HG_Register_name(
      hg_class_, "mercury_generic_rpc", StaticRpcServerSerialize,
      StaticRpcServerSerialize, StaticRpcServerCallback);

  hg_return_t hg_ret;
  hg_ret = HG_Register_data(hg_class_, mercury_generic_rpc_id_, /*data=*/this,
                            /*free_callback=*/NULL);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Register_data: failed");
  }

  hg_addr_t addr;
  char buf[256] = {'\0'};
  hg_size_t buf_size = 256;

  hg_ret = HG_Addr_self(hg_class_, &addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_self: failed");
  }

  hg_ret = HG_Addr_to_string(hg_class_, buf, &buf_size, addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_to_string: failed");
  }

  hg_ret = HG_Addr_free(hg_class_, addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_free: failed");
  }

  server_socket_address_ = std::string(buf);

  PrintMercuryVersion();
  LOG(INFO) << "Mercury Traffic server listening on " << server_socket_address_;
  progress_thread_ = RunRegisteredThread(
      "MercuryProgress", [=]() { this->RpcCompletionThread(); });
  return absl::OkStatus();
}

void ProtocolDriverMercury::SetHandler(
    std::function<std::function<void()>(ServerRpcState* state)> handler) {
  handler_ = handler;
}

void ProtocolDriverMercury::SetNumPeers(int num_peers) {
  remote_addresses_.resize(num_peers);
}

ProtocolDriverMercury::~ProtocolDriverMercury() {
  ShutdownServer();
  ShutdownClient();
  {
    absl::MutexLock l(&mercury_init_mutex);
    if (hg_class_ != nullptr) {
      HG_Deregister(hg_class_, mercury_generic_rpc_id_);
    }
    if (hg_context_ != nullptr) {
      HG_Context_destroy(hg_context_);
    }
    if (hg_class_ != nullptr) {
      HG_Finalize(hg_class_);
    }
  }
}

absl::StatusOr<std::string> ProtocolDriverMercury::HandlePreConnect(
    std::string_view remote_connection_info, int peer) {
  return server_socket_address_;
}

absl::Status ProtocolDriverMercury::HandleConnect(
    std::string remote_connection_info, int peer) {
  CHECK_GE(peer, 0);
  CHECK_LT((size_t)peer, remote_addresses_.size());

  LOG(INFO) << "Connect to peer " << peer << " at " << remote_connection_info;

  hg_return_t hg_ret;
  hg_ret = HG_Addr_lookup2(hg_class_, remote_connection_info.c_str(),
                           &remote_addresses_[peer]);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_lookup: failed");
  }

  return absl::OkStatus();
}

std::vector<TransportStat> ProtocolDriverMercury::GetTransportStats() {
  return {};
}

void ProtocolDriverMercury::PrintMercuryVersion() {
  unsigned major;
  unsigned minor;
  unsigned patch;
  absl::MutexLock l(&mercury_init_mutex);

  static bool already_done = false;
  if (already_done) return;
  already_done = true;
  HG_Version_get(&major, &minor, &patch);
  LOG(INFO) << "Mercury version " << major << "." << minor << "." << patch;
}

namespace {
struct PendingRpc {
  GenericRequest request;
  GenericResponse response;
  std::function<void(void)> done_callback;
  ClientRpcState* state;
  ProtocolDriverMercury* this_pd;
  mercury_generic_rpc_string_t encoded_request;
  hg_handle_t hg_handle;
};
}  // anonymous namespace

void ProtocolDriverMercury::InitiateRpc(
    int peer_index, ClientRpcState* state,
    std::function<void(void)> done_callback) {
  CHECK_GE(peer_index, 0);
  CHECK_LT((size_t)peer_index, remote_addresses_.size());

  ++pending_rpcs_;

  hg_handle_t target_handle;
  hg_return_t hg_ret;
  hg_ret = HG_Create(hg_context_, remote_addresses_[peer_index],
                     mercury_generic_rpc_id_, &target_handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Create: failed";
    return;
  }

  PendingRpc* new_rpc = new PendingRpc();
  new_rpc->done_callback = done_callback;
  new_rpc->state = state;
  new_rpc->request = std::move(state->request);
  new_rpc->this_pd = this;
  new_rpc->hg_handle = target_handle;

  new_rpc->request.SerializeToString(&new_rpc->encoded_request.string);

  hg_ret = HG_Forward(target_handle, StaticClientCallback, new_rpc,
                      &new_rpc->encoded_request);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << " HG_Forward: failed";
  }
}

void ProtocolDriverMercury::ChurnConnection(int peer) {}

void ProtocolDriverMercury::ShutdownServer() {}

void ProtocolDriverMercury::ShutdownClient() {
  while (pending_rpcs_) {
  }
  if (!shutdown_.HasBeenNotified()) {
    shutdown_.Notify();
    if (progress_thread_.joinable()) {
      progress_thread_.join();
    }
  }
  for (hg_addr_t addr : remote_addresses_) {
    HG_Addr_free(hg_class_, addr);
  }
  remote_addresses_.resize(0);
}

void ProtocolDriverMercury::RpcCompletionThread() {
  while (!shutdown_.HasBeenNotified()) {
    hg_return_t hg_ret;
    unsigned int actual_count = 0;

    // Process callbacks based on the network event received
    hg_ret = HG_Trigger(hg_context_, /*timeout=*/0, /*max_count=1*/ 1,
                        &actual_count);
    if (hg_ret != HG_SUCCESS && hg_ret != HG_TIMEOUT) {
      LOG(ERROR) << "HG_Trigger: failed with " << hg_ret
                 << " actual_count:" << actual_count;
    }

    // Process network events
    hg_ret = HG_Progress(hg_context_, /*timeout_ms=*/1);
    if (hg_ret != HG_SUCCESS && hg_ret != HG_TIMEOUT) {
      LOG(ERROR) << "HG_Progress: failed with " << hg_ret
                 << " actual_count:" << actual_count;
    }
  }
}

hg_return_t ProtocolDriverMercury::StaticClientCallback(
    const struct hg_cb_info* callback_info) {
  struct PendingRpc* rpc = (struct PendingRpc*)callback_info->arg;
  return rpc->this_pd->RpcClientCallback(callback_info);
}

// This function performs both serialization and deserialization (specified
// by proc).
hg_return_t ProtocolDriverMercury::StaticRpcServerSerialize(hg_proc_t proc,
                                                            void* data) {
  hg_return_t hg_ret;
  mercury_generic_rpc_string_t* struct_data =
      (mercury_generic_rpc_string_t*)data;
  size_t slen = struct_data->string.size();
  hg_ret = hg_proc_hg_int64_t(proc, &slen);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "Serialization error (size)";
  }
  hg_ret = hg_proc_hg_int64_t(proc, &slen);
  struct_data->string.resize(slen);
  hg_ret = hg_proc_bytes(proc, &struct_data->string[0], slen);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "Serialization error (data)";
  }
  return hg_ret;
}

hg_return_t ProtocolDriverMercury::StaticRpcServerCallback(hg_handle_t handle) {
  const struct hg_info* info = HG_Get_info(handle);
  ProtocolDriverMercury* this_pd =
      (ProtocolDriverMercury*)HG_Registered_data(info->hg_class, info->id);
  return this_pd->RpcServerCallback(handle);
}

hg_return_t ProtocolDriverMercury::RpcServerCallback(hg_handle_t handle) {
  mercury_generic_rpc_string_t input;
  hg_return_t hg_ret;

  hg_ret = HG_Get_input(handle, &input);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Get_input: failed";
  }

  ServerRpcState* rpc_state = new ServerRpcState();
  rpc_state->have_dedicated_thread = false;

  distbench::GenericRequest* request = new distbench::GenericRequest();
  bool success = request->ParseFromString(input.string);
  if (!success) {
    LOG(ERROR) << "Unable to decode payload !";
  }
  hg_ret = HG_Free_input(handle, &input);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Free_input: failed";
  }
  hg_ret = HG_Destroy(handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Destroy: failed";
  }

  rpc_state->request = request;
  rpc_state->SetSendResponseFunction([rpc_state, handle]() {
    mercury_generic_rpc_string_t* result = new mercury_generic_rpc_string_t();
    rpc_state->response.SerializeToString(&result->string);
    hg_return_t hg_ret;
    hg_ret =
        HG_Respond(handle, StaticRpcServerDoneCallback, /*arg=*/result, result);
    if (hg_ret != HG_SUCCESS) {
      LOG(ERROR) << "HG_Respond: failed";
    }
  });
  rpc_state->SetFreeStateFunction([rpc_state]() {
    delete rpc_state->request;
    delete rpc_state;
  });

  auto fct_action_list_thread = handler_(rpc_state);
  if (fct_action_list_thread) {
    RunRegisteredThread("DedicatedActionListThread", fct_action_list_thread)
        .detach();
  }
  return HG_SUCCESS;
}

hg_return_t ProtocolDriverMercury::StaticRpcServerDoneCallback(
    const struct hg_cb_info* hg_cb_info) {
  if (hg_cb_info->ret != HG_SUCCESS) {
    LOG(ERROR) << "StaticRpcServerDoneCallback with non success ret="
               << hg_cb_info->ret;
  }
  mercury_generic_rpc_string_t* result =
      (mercury_generic_rpc_string_t*)hg_cb_info->arg;
  delete result;
  return HG_SUCCESS;
}

hg_return_t ProtocolDriverMercury::RpcClientCallback(
    const struct hg_cb_info* callback_info) {
  struct PendingRpc* rpc = (struct PendingRpc*)callback_info->arg;
  rpc->state->success = (callback_info->ret == 0);
  mercury_generic_rpc_string_t result;
  hg_return_t hg_ret = HG_Get_output(rpc->hg_handle, &result);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Get_output: failed";
    return hg_ret;
  }

  bool success = rpc->response.ParseFromString(result.string);
  if (!success) {
    LOG(ERROR) << "Unable to decode payload";
  } else {
    rpc->state->request = std::move(rpc->request);
    rpc->state->response = std::move(rpc->response);
  }
  rpc->state->success = success;
  rpc->done_callback();

  hg_ret = HG_Free_output(rpc->hg_handle, &result);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Free_output: failed";
  }

  hg_ret = HG_Destroy(rpc->hg_handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Destroy: failed";
  }

  delete rpc;
  --pending_rpcs_;
  return HG_SUCCESS;
}

}  // namespace distbench
<commit_msg>reduce logspam<commit_after>// Copyright 2021 Google LLC
//
// 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
//
//     https://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 "protocol_driver_mercury.h"

#include <mercury.h>
#include <mercury_bulk.h>
#include <mercury_macros.h>
#include <mercury_proc_string.h>

#include "absl/base/const_init.h"
#include "absl/strings/str_replace.h"
#include "absl/synchronization/mutex.h"
#include "distbench_utils.h"
#include "glog/logging.h"

namespace {
ABSL_CONST_INIT absl::Mutex mercury_init_mutex(absl::kConstInit);
}  // namespace

namespace distbench {

ProtocolDriverMercury::ProtocolDriverMercury() {}

absl::Status ProtocolDriverMercury::Initialize(
    const ProtocolDriverOptions& pd_opts, int* port) {
  std::string netdev_name = pd_opts.netdev_name();
  auto maybe_ip = IpAddressForDevice(netdev_name, 4);
  if (!maybe_ip.ok()) return maybe_ip.status();
  server_ip_address_ = maybe_ip.value();
  server_socket_address_ = SocketAddressForIp(server_ip_address_, *port);

  // TODO: Choosen interface does not seem respected
  std::string info_string =
      GetNamedSettingString(pd_opts.server_settings(), "hg_init_info_string",
                            "ofi+tcp://__SERVER_IP__");
  info_string = absl::StrReplaceAll(
      info_string, {{"__SERVER_IP__", server_socket_address_}});
  {
    absl::MutexLock l(&mercury_init_mutex);
    hg_class_ = HG_Init(info_string.c_str(), /*listen=*/true);
  }
  if (hg_class_ == nullptr) {
    return absl::UnknownError("HG_Init: failed");
  }

  hg_context_ = HG_Context_create(hg_class_);
  if (hg_context_ == nullptr) {
    return absl::UnknownError("HG_Context_create: failed");
  }

  mercury_generic_rpc_id_ = HG_Register_name(
      hg_class_, "mercury_generic_rpc", StaticRpcServerSerialize,
      StaticRpcServerSerialize, StaticRpcServerCallback);

  hg_return_t hg_ret;
  hg_ret = HG_Register_data(hg_class_, mercury_generic_rpc_id_, /*data=*/this,
                            /*free_callback=*/NULL);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Register_data: failed");
  }

  hg_addr_t addr;
  char buf[256] = {'\0'};
  hg_size_t buf_size = 256;

  hg_ret = HG_Addr_self(hg_class_, &addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_self: failed");
  }

  hg_ret = HG_Addr_to_string(hg_class_, buf, &buf_size, addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_to_string: failed");
  }

  hg_ret = HG_Addr_free(hg_class_, addr);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_free: failed");
  }

  server_socket_address_ = std::string(buf);

  PrintMercuryVersion();
  VLOG(1) << "Mercury Traffic server listening on " << server_socket_address_;
  progress_thread_ = RunRegisteredThread(
      "MercuryProgress", [=]() { this->RpcCompletionThread(); });
  return absl::OkStatus();
}

void ProtocolDriverMercury::SetHandler(
    std::function<std::function<void()>(ServerRpcState* state)> handler) {
  handler_ = handler;
}

void ProtocolDriverMercury::SetNumPeers(int num_peers) {
  remote_addresses_.resize(num_peers);
}

ProtocolDriverMercury::~ProtocolDriverMercury() {
  ShutdownServer();
  ShutdownClient();
  {
    absl::MutexLock l(&mercury_init_mutex);
    if (hg_class_ != nullptr) {
      HG_Deregister(hg_class_, mercury_generic_rpc_id_);
    }
    if (hg_context_ != nullptr) {
      HG_Context_destroy(hg_context_);
    }
    if (hg_class_ != nullptr) {
      HG_Finalize(hg_class_);
    }
  }
}

absl::StatusOr<std::string> ProtocolDriverMercury::HandlePreConnect(
    std::string_view remote_connection_info, int peer) {
  return server_socket_address_;
}

absl::Status ProtocolDriverMercury::HandleConnect(
    std::string remote_connection_info, int peer) {
  CHECK_GE(peer, 0);
  CHECK_LT((size_t)peer, remote_addresses_.size());

  VLOG(1) << "Connect to peer " << peer << " at " << remote_connection_info;

  hg_return_t hg_ret;
  hg_ret = HG_Addr_lookup2(hg_class_, remote_connection_info.c_str(),
                           &remote_addresses_[peer]);
  if (hg_ret != HG_SUCCESS) {
    return absl::UnknownError("HG_Addr_lookup: failed");
  }

  return absl::OkStatus();
}

std::vector<TransportStat> ProtocolDriverMercury::GetTransportStats() {
  return {};
}

void ProtocolDriverMercury::PrintMercuryVersion() {
  unsigned major;
  unsigned minor;
  unsigned patch;
  absl::MutexLock l(&mercury_init_mutex);

  static bool already_done = false;
  if (already_done) return;
  already_done = true;
  HG_Version_get(&major, &minor, &patch);
  VLOG(1) << "Mercury version " << major << "." << minor << "." << patch;
}

namespace {
struct PendingRpc {
  GenericRequest request;
  GenericResponse response;
  std::function<void(void)> done_callback;
  ClientRpcState* state;
  ProtocolDriverMercury* this_pd;
  mercury_generic_rpc_string_t encoded_request;
  hg_handle_t hg_handle;
};
}  // anonymous namespace

void ProtocolDriverMercury::InitiateRpc(
    int peer_index, ClientRpcState* state,
    std::function<void(void)> done_callback) {
  CHECK_GE(peer_index, 0);
  CHECK_LT((size_t)peer_index, remote_addresses_.size());

  ++pending_rpcs_;

  hg_handle_t target_handle;
  hg_return_t hg_ret;
  hg_ret = HG_Create(hg_context_, remote_addresses_[peer_index],
                     mercury_generic_rpc_id_, &target_handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Create: failed";
    return;
  }

  PendingRpc* new_rpc = new PendingRpc();
  new_rpc->done_callback = done_callback;
  new_rpc->state = state;
  new_rpc->request = std::move(state->request);
  new_rpc->this_pd = this;
  new_rpc->hg_handle = target_handle;

  new_rpc->request.SerializeToString(&new_rpc->encoded_request.string);

  hg_ret = HG_Forward(target_handle, StaticClientCallback, new_rpc,
                      &new_rpc->encoded_request);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << " HG_Forward: failed";
  }
}

void ProtocolDriverMercury::ChurnConnection(int peer) {}

void ProtocolDriverMercury::ShutdownServer() {}

void ProtocolDriverMercury::ShutdownClient() {
  while (pending_rpcs_) {
  }
  if (!shutdown_.HasBeenNotified()) {
    shutdown_.Notify();
    if (progress_thread_.joinable()) {
      progress_thread_.join();
    }
  }
  for (hg_addr_t addr : remote_addresses_) {
    HG_Addr_free(hg_class_, addr);
  }
  remote_addresses_.resize(0);
}

void ProtocolDriverMercury::RpcCompletionThread() {
  while (!shutdown_.HasBeenNotified()) {
    hg_return_t hg_ret;
    unsigned int actual_count = 0;

    // Process callbacks based on the network event received
    hg_ret = HG_Trigger(hg_context_, /*timeout=*/0, /*max_count=1*/ 1,
                        &actual_count);
    if (hg_ret != HG_SUCCESS && hg_ret != HG_TIMEOUT) {
      LOG(ERROR) << "HG_Trigger: failed with " << hg_ret
                 << " actual_count:" << actual_count;
    }

    // Process network events
    hg_ret = HG_Progress(hg_context_, /*timeout_ms=*/1);
    if (hg_ret != HG_SUCCESS && hg_ret != HG_TIMEOUT) {
      LOG(ERROR) << "HG_Progress: failed with " << hg_ret
                 << " actual_count:" << actual_count;
    }
  }
}

hg_return_t ProtocolDriverMercury::StaticClientCallback(
    const struct hg_cb_info* callback_info) {
  struct PendingRpc* rpc = (struct PendingRpc*)callback_info->arg;
  return rpc->this_pd->RpcClientCallback(callback_info);
}

// This function performs both serialization and deserialization (specified
// by proc).
hg_return_t ProtocolDriverMercury::StaticRpcServerSerialize(hg_proc_t proc,
                                                            void* data) {
  hg_return_t hg_ret;
  mercury_generic_rpc_string_t* struct_data =
      (mercury_generic_rpc_string_t*)data;
  size_t slen = struct_data->string.size();
  hg_ret = hg_proc_hg_int64_t(proc, &slen);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "Serialization error (size)";
  }
  hg_ret = hg_proc_hg_int64_t(proc, &slen);
  struct_data->string.resize(slen);
  hg_ret = hg_proc_bytes(proc, &struct_data->string[0], slen);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "Serialization error (data)";
  }
  return hg_ret;
}

hg_return_t ProtocolDriverMercury::StaticRpcServerCallback(hg_handle_t handle) {
  const struct hg_info* info = HG_Get_info(handle);
  ProtocolDriverMercury* this_pd =
      (ProtocolDriverMercury*)HG_Registered_data(info->hg_class, info->id);
  return this_pd->RpcServerCallback(handle);
}

hg_return_t ProtocolDriverMercury::RpcServerCallback(hg_handle_t handle) {
  mercury_generic_rpc_string_t input;
  hg_return_t hg_ret;

  hg_ret = HG_Get_input(handle, &input);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Get_input: failed";
  }

  ServerRpcState* rpc_state = new ServerRpcState();
  rpc_state->have_dedicated_thread = false;

  distbench::GenericRequest* request = new distbench::GenericRequest();
  bool success = request->ParseFromString(input.string);
  if (!success) {
    LOG(ERROR) << "Unable to decode payload !";
  }
  hg_ret = HG_Free_input(handle, &input);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Free_input: failed";
  }
  hg_ret = HG_Destroy(handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Destroy: failed";
  }

  rpc_state->request = request;
  rpc_state->SetSendResponseFunction([rpc_state, handle]() {
    mercury_generic_rpc_string_t* result = new mercury_generic_rpc_string_t();
    rpc_state->response.SerializeToString(&result->string);
    hg_return_t hg_ret;
    hg_ret =
        HG_Respond(handle, StaticRpcServerDoneCallback, /*arg=*/result, result);
    if (hg_ret != HG_SUCCESS) {
      LOG(ERROR) << "HG_Respond: failed";
    }
  });
  rpc_state->SetFreeStateFunction([rpc_state]() {
    delete rpc_state->request;
    delete rpc_state;
  });

  auto fct_action_list_thread = handler_(rpc_state);
  if (fct_action_list_thread) {
    RunRegisteredThread("DedicatedActionListThread", fct_action_list_thread)
        .detach();
  }
  return HG_SUCCESS;
}

hg_return_t ProtocolDriverMercury::StaticRpcServerDoneCallback(
    const struct hg_cb_info* hg_cb_info) {
  if (hg_cb_info->ret != HG_SUCCESS) {
    LOG(ERROR) << "StaticRpcServerDoneCallback with non success ret="
               << hg_cb_info->ret;
  }
  mercury_generic_rpc_string_t* result =
      (mercury_generic_rpc_string_t*)hg_cb_info->arg;
  delete result;
  return HG_SUCCESS;
}

hg_return_t ProtocolDriverMercury::RpcClientCallback(
    const struct hg_cb_info* callback_info) {
  struct PendingRpc* rpc = (struct PendingRpc*)callback_info->arg;
  rpc->state->success = (callback_info->ret == 0);
  mercury_generic_rpc_string_t result;
  hg_return_t hg_ret = HG_Get_output(rpc->hg_handle, &result);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Get_output: failed";
    return hg_ret;
  }

  bool success = rpc->response.ParseFromString(result.string);
  if (!success) {
    LOG(ERROR) << "Unable to decode payload";
  } else {
    rpc->state->request = std::move(rpc->request);
    rpc->state->response = std::move(rpc->response);
  }
  rpc->state->success = success;
  rpc->done_callback();

  hg_ret = HG_Free_output(rpc->hg_handle, &result);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Free_output: failed";
  }

  hg_ret = HG_Destroy(rpc->hg_handle);
  if (hg_ret != HG_SUCCESS) {
    LOG(ERROR) << "HG_Destroy: failed";
  }

  delete rpc;
  --pending_rpcs_;
  return HG_SUCCESS;
}

}  // namespace distbench
<|endoftext|>


<commit_before>#include "rapid_pbd/action_executor.h"

#include <string>

#include "actionlib/client/simple_action_client.h"
#include "actionlib/server/simple_action_server.h"
#include "control_msgs/GripperCommandAction.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "ros/ros.h"

#include "rapid_pbd/action_names.h"
#include "rapid_pbd_msgs/ExecuteActionAction.h"

using actionlib::SimpleActionClient;
using actionlib::SimpleClientGoalState;
using control_msgs::FollowJointTrajectoryAction;
using rapid_pbd_msgs::Action;
using rapid_pbd_msgs::ExecuteActionGoalConstPtr;

namespace rapid {
namespace pbd {
ActionExecutor::ActionExecutor(const Action& action)
    : action_(action),
      gripper_client_(kGripperActionName, true),
      l_gripper_client_(kLeftGripperActionName, true),
      r_gripper_client_(kRightGripperActionName, true),
      arm_joint_client_(kArmJointActionName, true),
      l_arm_joint_client_(kLeftArmJointActionName, true),
      r_arm_joint_client_(kRightArmJointActionName, true) {
  IsValid(action_);
}

bool ActionExecutor::IsValid(const Action& action) {
  if (action.type == Action::ACTUATE_GRIPPER) {
    if (action.actuator_group == Action::GRIPPER ||
        action.actuator_group == Action::LEFT_GRIPPER ||
        action.actuator_group == Action::RIGHT_GRIPPER) {
    } else {
      PublishInvalidGroupError(action);
      return false;
    }
  } else if (action.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action.actuator_group == Action::ARM ||
        action.actuator_group == Action::LEFT_ARM ||
        action.actuator_group == Action::RIGHT_ARM) {
    } else {
      PublishInvalidGroupError(action);
      return false;
    }
  } else if (action.type == Action::MOVE_TO_CARTESIAN_GOAL) {
  } else if (action.type == Action::DETECT_TABLETOP_OBJECTS) {
  } else if (action.type == Action::FIND_CUSTOM_LANDMARK) {
  } else {
    ROS_ERROR("Invalid action type: \"%s\"", action.type.c_str());
    return false;
  }
  return true;
}

void ActionExecutor::Start() {
  if (!IsValid(action_)) {
    return;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    ActuateGripper();
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    MoveToJointGoal();
  }
}

bool ActionExecutor::IsDone() const {
  if (!IsValid(action_)) {
    return true;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    if (action_.actuator_group == Action::GRIPPER) {
      return gripper_client_.getState().isDone();
    } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
      return l_gripper_client_.getState().isDone();
    } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
      return r_gripper_client_.getState().isDone();
    }
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action_.actuator_group == Action::ARM) {
      return arm_joint_client_.getState().isDone();
    } else if (action_.actuator_group == Action::LEFT_ARM) {
      return l_arm_joint_client_.getState().isDone();
    } else if (action_.actuator_group == Action::RIGHT_ARM) {
      return r_arm_joint_client_.getState().isDone();
    }
  }
  return true;
}

void ActionExecutor::Cancel() {
  if (!IsValid(action_)) {
    return;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    if (action_.actuator_group == Action::GRIPPER) {
      gripper_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
      l_gripper_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
      r_gripper_client_.cancelAllGoals();
    }
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action_.actuator_group == Action::ARM) {
      arm_joint_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::LEFT_ARM) {
      l_arm_joint_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::RIGHT_ARM) {
      r_arm_joint_client_.cancelAllGoals();
    }
  }
}

void ActionExecutor::ActuateGripper() {
  control_msgs::GripperCommandGoal gripper_goal;
  gripper_goal.command = action_.gripper_command;
  SimpleActionClient<control_msgs::GripperCommandAction>* client;
  if (action_.actuator_group == Action::GRIPPER) {
    client = &gripper_client_;
  } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
    client = &l_gripper_client_;
  } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
    client = &r_gripper_client_;
  } else {
    return;
  }
  while (!client->waitForServer(ros::Duration(5.0))) {
    ROS_WARN("%s server not running! Trying again in 5 seconds.",
             action_.actuator_group.c_str());
    ros::spinOnce();
  }
  client->sendGoal(gripper_goal);
}

void ActionExecutor::MoveToJointGoal() {
  control_msgs::FollowJointTrajectoryGoal joint_goal;
  joint_goal.trajectory = action_.joint_trajectory;
  joint_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.1);
  SimpleActionClient<FollowJointTrajectoryAction>* client;
  if (action_.actuator_group == Action::ARM) {
    client = &arm_joint_client_;
  } else if (action_.actuator_group == Action::LEFT_ARM) {
    client = &l_arm_joint_client_;
  } else if (action_.actuator_group == Action::RIGHT_ARM) {
    client = &r_arm_joint_client_;
  } else {
    return;
  }
  while (!client->waitForServer(ros::Duration(5.0))) {
    ROS_WARN("%s server not running! Trying again in 5 seconds.",
             action_.actuator_group.c_str());
    ros::spinOnce();
  }
  client->sendGoal(joint_goal);
}

void ActionExecutor::PublishInvalidGroupError(const Action& action) {
  ROS_ERROR("Invalid actuator_group \"%s\" for action type \"%s\".",
            action.actuator_group.c_str(), action.type.c_str());
}
}  // namespace pbd
}  // namespace rapid
<commit_msg>Removed unused include.<commit_after>#include "rapid_pbd/action_executor.h"

#include <string>

#include "actionlib/client/simple_action_client.h"
#include "actionlib/server/simple_action_server.h"
#include "control_msgs/GripperCommandAction.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "ros/ros.h"

#include "rapid_pbd/action_names.h"

using actionlib::SimpleActionClient;
using actionlib::SimpleClientGoalState;
using control_msgs::FollowJointTrajectoryAction;
using rapid_pbd_msgs::Action;

namespace rapid {
namespace pbd {
ActionExecutor::ActionExecutor(const Action& action)
    : action_(action),
      gripper_client_(kGripperActionName, true),
      l_gripper_client_(kLeftGripperActionName, true),
      r_gripper_client_(kRightGripperActionName, true),
      arm_joint_client_(kArmJointActionName, true),
      l_arm_joint_client_(kLeftArmJointActionName, true),
      r_arm_joint_client_(kRightArmJointActionName, true) {
  IsValid(action_);
}

bool ActionExecutor::IsValid(const Action& action) {
  if (action.type == Action::ACTUATE_GRIPPER) {
    if (action.actuator_group == Action::GRIPPER ||
        action.actuator_group == Action::LEFT_GRIPPER ||
        action.actuator_group == Action::RIGHT_GRIPPER) {
    } else {
      PublishInvalidGroupError(action);
      return false;
    }
  } else if (action.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action.actuator_group == Action::ARM ||
        action.actuator_group == Action::LEFT_ARM ||
        action.actuator_group == Action::RIGHT_ARM) {
    } else {
      PublishInvalidGroupError(action);
      return false;
    }
  } else if (action.type == Action::MOVE_TO_CARTESIAN_GOAL) {
  } else if (action.type == Action::DETECT_TABLETOP_OBJECTS) {
  } else if (action.type == Action::FIND_CUSTOM_LANDMARK) {
  } else {
    ROS_ERROR("Invalid action type: \"%s\"", action.type.c_str());
    return false;
  }
  return true;
}

void ActionExecutor::Start() {
  if (!IsValid(action_)) {
    return;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    ActuateGripper();
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    MoveToJointGoal();
  }
}

bool ActionExecutor::IsDone() const {
  if (!IsValid(action_)) {
    return true;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    if (action_.actuator_group == Action::GRIPPER) {
      return gripper_client_.getState().isDone();
    } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
      return l_gripper_client_.getState().isDone();
    } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
      return r_gripper_client_.getState().isDone();
    }
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action_.actuator_group == Action::ARM) {
      return arm_joint_client_.getState().isDone();
    } else if (action_.actuator_group == Action::LEFT_ARM) {
      return l_arm_joint_client_.getState().isDone();
    } else if (action_.actuator_group == Action::RIGHT_ARM) {
      return r_arm_joint_client_.getState().isDone();
    }
  }
  return true;
}

void ActionExecutor::Cancel() {
  if (!IsValid(action_)) {
    return;
  }
  if (action_.type == Action::ACTUATE_GRIPPER) {
    if (action_.actuator_group == Action::GRIPPER) {
      gripper_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
      l_gripper_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
      r_gripper_client_.cancelAllGoals();
    }
  } else if (action_.type == Action::MOVE_TO_JOINT_GOAL) {
    if (action_.actuator_group == Action::ARM) {
      arm_joint_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::LEFT_ARM) {
      l_arm_joint_client_.cancelAllGoals();
    } else if (action_.actuator_group == Action::RIGHT_ARM) {
      r_arm_joint_client_.cancelAllGoals();
    }
  }
}

void ActionExecutor::ActuateGripper() {
  control_msgs::GripperCommandGoal gripper_goal;
  gripper_goal.command = action_.gripper_command;
  SimpleActionClient<control_msgs::GripperCommandAction>* client;
  if (action_.actuator_group == Action::GRIPPER) {
    client = &gripper_client_;
  } else if (action_.actuator_group == Action::LEFT_GRIPPER) {
    client = &l_gripper_client_;
  } else if (action_.actuator_group == Action::RIGHT_GRIPPER) {
    client = &r_gripper_client_;
  } else {
    return;
  }
  while (!client->waitForServer(ros::Duration(5.0))) {
    ROS_WARN("%s server not running! Trying again in 5 seconds.",
             action_.actuator_group.c_str());
    ros::spinOnce();
  }
  client->sendGoal(gripper_goal);
}

void ActionExecutor::MoveToJointGoal() {
  control_msgs::FollowJointTrajectoryGoal joint_goal;
  joint_goal.trajectory = action_.joint_trajectory;
  joint_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.1);
  SimpleActionClient<FollowJointTrajectoryAction>* client;
  if (action_.actuator_group == Action::ARM) {
    client = &arm_joint_client_;
  } else if (action_.actuator_group == Action::LEFT_ARM) {
    client = &l_arm_joint_client_;
  } else if (action_.actuator_group == Action::RIGHT_ARM) {
    client = &r_arm_joint_client_;
  } else {
    return;
  }
  while (!client->waitForServer(ros::Duration(5.0))) {
    ROS_WARN("%s server not running! Trying again in 5 seconds.",
             action_.actuator_group.c_str());
    ros::spinOnce();
  }
  client->sendGoal(joint_goal);
}

void ActionExecutor::PublishInvalidGroupError(const Action& action) {
  ROS_ERROR("Invalid actuator_group \"%s\" for action type \"%s\".",
            action.actuator_group.c_str(), action.type.c_str());
}
}  // namespace pbd
}  // namespace rapid
<|endoftext|>


<commit_before>
/** Copyright (C) 2016, 2017 European Spallation Source ERIC */

#include <cinttypes>
#include <common/Detector.h>
#include <common/EFUArgs.h>
#include <common/FBSerializer.h>
#include <common/NewStats.h>
#include <common/Producer.h>
#include <common/RingBuffer.h>
#include <common/Trace.h>
#include <cstring>
#include <iostream>
#include <libs/include/SPSCFifo.h>
#include <libs/include/Socket.h>
#include <libs/include/TSCTimer.h>
#include <libs/include/Timer.h>
#include <memory>
#include <sonde/ideas/Data.h>
#include <stdio.h>
#include <unistd.h>

#undef TRC_LEVEL
#define TRC_LEVEL TRC_L_DEB

using namespace std;
using namespace memory_sequential_consistent; // Lock free fifo

const char *classname = "SoNDe detector using IDEA readout";

const int TSC_MHZ = 2900; // MJC's workstation - not reliable

/** ----------------------------------------------------- */

class SONDEIDEA : public Detector {
public:
  SONDEIDEA(void *args);
  void input_thread();
  void processing_thread();

  int statsize();
  int64_t statvalue(size_t index);
  std::string &statname(size_t index);

  /** @todo figure out the right size  of the .._max_entries  */
  static const int eth_buffer_max_entries = 20000;
  static const int eth_buffer_size = 9000;
  static const int kafka_buffer_size = 124000; /**< events */

private:
  /** Shared between input_thread and processing_thread*/
  CircularFifo<unsigned int, eth_buffer_max_entries> input2proc_fifo;
  RingBuffer<eth_buffer_size> *eth_ringbuf;

  NewStats ns{
      "efu2.sonde."}; //

  struct {
    // Input Counters
    int64_t rx_packets;
    int64_t rx_bytes;
    int64_t fifo1_push_errors;
    int64_t pad[5];

    // Processing and Output counters
    int64_t rx_idle1;
    int64_t rx_events;
    int64_t rx_geometry_errors;
    int64_t tx_bytes;
  } ALIGN(64) mystats;

  EFUArgs *opts;
};

SONDEIDEA::SONDEIDEA(void *args) {
  opts = (EFUArgs *)args;

  XTRACE(INIT, ALW, "Adding stats\n");
  // clang-format off
  ns.create("input.rx_packets",                &mystats.rx_packets);
  ns.create("input.rx_bytes",                  &mystats.rx_bytes);
  ns.create("input.dropped",                   &mystats.fifo1_push_errors);
  ns.create("processing.idle",                 &mystats.rx_idle1);
  ns.create("processing.rx_events",            &mystats.rx_events);
  ns.create("processing.rx_geometry_errors",   &mystats.rx_geometry_errors);
  ns.create("output.tx_bytes",                 &mystats.tx_bytes);
  // clang-format on

  XTRACE(INIT, ALW, "Creating %d SONDE Rx ringbuffers of size %d\n",
         eth_buffer_max_entries, eth_buffer_size);
  eth_ringbuf = new RingBuffer<eth_buffer_size>(eth_buffer_max_entries);
  assert(eth_ringbuf != 0);
}

int SONDEIDEA::statsize() { return ns.size(); }

int64_t SONDEIDEA::statvalue(size_t index) { return ns.value(index); }

std::string &SONDEIDEA::statname(size_t index) { return ns.name(index); }

void SONDEIDEA::input_thread() {
  /** Connection setup */
  Socket::Endpoint local(opts->ip_addr.c_str(), opts->port);
  UDPServer nmxdata(local);
  nmxdata.buflen(opts->buflen);
  nmxdata.setbuffers(0, opts->rcvbuf);
  nmxdata.printbuffers();
  nmxdata.settimeout(0, 100000); // One tenth of a second

  int rdsize;
  Timer stop_timer;
  TSCTimer report_timer;
  for (;;) {
    unsigned int eth_index = eth_ringbuf->getindex();

    /** this is the processing step */
    if ((rdsize = nmxdata.receive(eth_ringbuf->getdatabuffer(eth_index),
                                  eth_ringbuf->getmaxbufsize())) > 0) {
      mystats.rx_packets++;
      mystats.rx_bytes += rdsize;
      eth_ringbuf->setdatalength(eth_index, rdsize);

      if (input2proc_fifo.push(eth_index) == false) {
        mystats.fifo1_push_errors++;
      } else {
        eth_ringbuf->nextbuffer();
      }
    }

    // Checking for exit
    if (report_timer.timetsc() >= opts->updint * 1000000 * TSC_MHZ) {

      if (stop_timer.timeus() >= opts->stopafter * 1000000LU) {
        std::cout << "stopping input thread, timeus " << stop_timer.timeus()
                  << std::endl;
        return;
      }
      report_timer.now();
    }
  }
}

void SONDEIDEA::processing_thread() {
  SoNDeGeometry geometry;

  IDEASData ideasdata(&geometry);
  Producer eventprod(opts->broker, "SKADI_detector");
  FBSerializer flatbuffer(kafka_buffer_size, eventprod);

  Timer stopafter_clock;
  TSCTimer global_time, report_timer;

  unsigned int data_index;

  while (1) {
    if ((input2proc_fifo.pop(data_index)) == false) {
      mystats.rx_idle1++;
      usleep(10);
    } else {
      int events = ideasdata.receive(eth_ringbuf->getdatabuffer(data_index),
        eth_ringbuf->getdatalength(data_index));

      mystats.rx_geometry_errors += ideasdata.errors;
      mystats.rx_events += ideasdata.events;

      if (events > 0) {
        for (int i = 0; i < events; i++) {
            XTRACE(PROCESS, DEB, "flatbuffer.addevent[i: %d](t: %d, pix: %d)\n", i
                    ideasdata.data[i].time,
                    ideasdata.data[i].pixel_id);
            mystats.tx_bytes += flatbuffer.addevent(ideasdata.data[i].time, ideasdata.data[i].pixel_id);
        }
      }
    }

    // Checking for exit
    if (report_timer.timetsc() >= opts->updint * 1000000 * TSC_MHZ) {
      // printf("timetsc: %" PRIu64 "\n", global_time.timetsc());

      mystats.tx_bytes += flatbuffer.produce();

      if (stopafter_clock.timeus() >= opts->stopafter * 1000000LU) {
        std::cout << "stopping processing thread, timeus " << std::endl;
        return;
      }
      report_timer.now();
    }
  }
}

/** ----------------------------------------------------- */

class SONDEIDEAFactory : DetectorFactory {
public:
  std::shared_ptr<Detector> create(void *args) {
    return std::shared_ptr<Detector>(new SONDEIDEA(args));
  }
};

SONDEIDEAFactory Factory;
<commit_msg>didnt compile, getting tired<commit_after>
/** Copyright (C) 2016, 2017 European Spallation Source ERIC */

#include <cinttypes>
#include <common/Detector.h>
#include <common/EFUArgs.h>
#include <common/FBSerializer.h>
#include <common/NewStats.h>
#include <common/Producer.h>
#include <common/RingBuffer.h>
#include <common/Trace.h>
#include <cstring>
#include <iostream>
#include <libs/include/SPSCFifo.h>
#include <libs/include/Socket.h>
#include <libs/include/TSCTimer.h>
#include <libs/include/Timer.h>
#include <memory>
#include <sonde/ideas/Data.h>
#include <stdio.h>
#include <unistd.h>

#undef TRC_LEVEL
#define TRC_LEVEL TRC_L_DEB

using namespace std;
using namespace memory_sequential_consistent; // Lock free fifo

const char *classname = "SoNDe detector using IDEA readout";

const int TSC_MHZ = 2900; // MJC's workstation - not reliable

/** ----------------------------------------------------- */

class SONDEIDEA : public Detector {
public:
  SONDEIDEA(void *args);
  void input_thread();
  void processing_thread();

  int statsize();
  int64_t statvalue(size_t index);
  std::string &statname(size_t index);

  /** @todo figure out the right size  of the .._max_entries  */
  static const int eth_buffer_max_entries = 20000;
  static const int eth_buffer_size = 9000;
  static const int kafka_buffer_size = 124000; /**< events */

private:
  /** Shared between input_thread and processing_thread*/
  CircularFifo<unsigned int, eth_buffer_max_entries> input2proc_fifo;
  RingBuffer<eth_buffer_size> *eth_ringbuf;

  NewStats ns{
      "efu2.sonde."}; //

  struct {
    // Input Counters
    int64_t rx_packets;
    int64_t rx_bytes;
    int64_t fifo1_push_errors;
    int64_t pad[5];

    // Processing and Output counters
    int64_t rx_idle1;
    int64_t rx_events;
    int64_t rx_geometry_errors;
    int64_t tx_bytes;
  } ALIGN(64) mystats;

  EFUArgs *opts;
};

SONDEIDEA::SONDEIDEA(void *args) {
  opts = (EFUArgs *)args;

  XTRACE(INIT, ALW, "Adding stats\n");
  // clang-format off
  ns.create("input.rx_packets",                &mystats.rx_packets);
  ns.create("input.rx_bytes",                  &mystats.rx_bytes);
  ns.create("input.dropped",                   &mystats.fifo1_push_errors);
  ns.create("processing.idle",                 &mystats.rx_idle1);
  ns.create("processing.rx_events",            &mystats.rx_events);
  ns.create("processing.rx_geometry_errors",   &mystats.rx_geometry_errors);
  ns.create("output.tx_bytes",                 &mystats.tx_bytes);
  // clang-format on

  XTRACE(INIT, ALW, "Creating %d SONDE Rx ringbuffers of size %d\n",
         eth_buffer_max_entries, eth_buffer_size);
  eth_ringbuf = new RingBuffer<eth_buffer_size>(eth_buffer_max_entries);
  assert(eth_ringbuf != 0);
}

int SONDEIDEA::statsize() { return ns.size(); }

int64_t SONDEIDEA::statvalue(size_t index) { return ns.value(index); }

std::string &SONDEIDEA::statname(size_t index) { return ns.name(index); }

void SONDEIDEA::input_thread() {
  /** Connection setup */
  Socket::Endpoint local(opts->ip_addr.c_str(), opts->port);
  UDPServer nmxdata(local);
  nmxdata.buflen(opts->buflen);
  nmxdata.setbuffers(0, opts->rcvbuf);
  nmxdata.printbuffers();
  nmxdata.settimeout(0, 100000); // One tenth of a second

  int rdsize;
  Timer stop_timer;
  TSCTimer report_timer;
  for (;;) {
    unsigned int eth_index = eth_ringbuf->getindex();

    /** this is the processing step */
    if ((rdsize = nmxdata.receive(eth_ringbuf->getdatabuffer(eth_index),
                                  eth_ringbuf->getmaxbufsize())) > 0) {
      mystats.rx_packets++;
      mystats.rx_bytes += rdsize;
      eth_ringbuf->setdatalength(eth_index, rdsize);

      if (input2proc_fifo.push(eth_index) == false) {
        mystats.fifo1_push_errors++;
      } else {
        eth_ringbuf->nextbuffer();
      }
    }

    // Checking for exit
    if (report_timer.timetsc() >= opts->updint * 1000000 * TSC_MHZ) {

      if (stop_timer.timeus() >= opts->stopafter * 1000000LU) {
        std::cout << "stopping input thread, timeus " << stop_timer.timeus()
                  << std::endl;
        return;
      }
      report_timer.now();
    }
  }
}

void SONDEIDEA::processing_thread() {
  SoNDeGeometry geometry;

  IDEASData ideasdata(&geometry);
  Producer eventprod(opts->broker, "SKADI_detector");
  FBSerializer flatbuffer(kafka_buffer_size, eventprod);

  Timer stopafter_clock;
  TSCTimer global_time, report_timer;

  unsigned int data_index;

  while (1) {
    if ((input2proc_fifo.pop(data_index)) == false) {
      mystats.rx_idle1++;
      usleep(10);
    } else {
      int events = ideasdata.receive(eth_ringbuf->getdatabuffer(data_index),
        eth_ringbuf->getdatalength(data_index));

      mystats.rx_geometry_errors += ideasdata.errors;
      mystats.rx_events += ideasdata.events;

      if (events > 0) {
        for (int i = 0; i < events; i++) {
            XTRACE(PROCESS, DEB, "flatbuffer.addevent[i: %d](t: %d, pix: %d)\n", i,
                    ideasdata.data[i].time,
                    ideasdata.data[i].pixel_id);
            mystats.tx_bytes += flatbuffer.addevent(ideasdata.data[i].time, ideasdata.data[i].pixel_id);
        }
      }
    }

    // Checking for exit
    if (report_timer.timetsc() >= opts->updint * 1000000 * TSC_MHZ) {
      // printf("timetsc: %" PRIu64 "\n", global_time.timetsc());

      mystats.tx_bytes += flatbuffer.produce();

      if (stopafter_clock.timeus() >= opts->stopafter * 1000000LU) {
        std::cout << "stopping processing thread, timeus " << std::endl;
        return;
      }
      report_timer.now();
    }
  }
}

/** ----------------------------------------------------- */

class SONDEIDEAFactory : DetectorFactory {
public:
  std::shared_ptr<Detector> create(void *args) {
    return std::shared_ptr<Detector>(new SONDEIDEA(args));
  }
};

SONDEIDEAFactory Factory;
<|endoftext|>


<commit_before>#include <bits/stdc++.h>

using namespace std;

void calc_sizes(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int p) {
    size[u] = 1;
    for (int v : tree[u]) {
        if (v == p || deleted[v]) continue;
        calc_sizes(tree, size, deleted, v, u);
        size[u] += size[v];
    }
}

int find_tree_centroid(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int p,
                       int vertices) {
    for (int v : tree[u]) {
        if (v == p || deleted[v]) continue;
        if (size[v] > vertices / 2) {
            return find_tree_centroid(tree, size, deleted, v, u, vertices);
        }
    }
    return u;
}

void dfs(vector<vector<int>> &tree, vector<bool> &deleted, int u, int p) {
    for (int v: tree[u]) {
        if (v == p || deleted[v])continue;
        dfs(tree, deleted, v, u);
    }
}

void decompose(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int total) {
    calc_sizes(tree, size, deleted, u, -1);
    int centroid = find_tree_centroid(tree, size, deleted, u, -1, total);
    deleted[centroid] = true;

    // process centroid vertex here
    // cout << centroid << endl;

    // for (int v: tree[centroid]) {
    //    if (deleted[v])continue;
    //    dfs(tree, deleted, v, centroid);
    // }

    for (int v : tree[centroid]) {
        if (deleted[v]) continue;
        decompose(tree, size, deleted, v, size[v]);
    }
}

// usage example
int main() {
    vector<vector<int>> tree = {{3},
                                {3},
                                {3},
                                {0, 1, 2}};

    int n = tree.size();
    vector<int> size(n);
    vector<bool> deleted(n);
    decompose(tree, size, deleted, 0, n);
}
<commit_msg>update<commit_after>#include <bits/stdc++.h>

using namespace std;

void calc_sizes(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int p) {
    size[u] = 1;
    for (int v : tree[u]) {
        if (v == p || deleted[v]) continue;
        calc_sizes(tree, size, deleted, v, u);
        size[u] += size[v];
    }
}

int find_tree_centroid(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int p,
                       int vertices) {
    for (int v : tree[u]) {
        if (v == p || deleted[v]) continue;
        if (size[v] > vertices / 2) {
            return find_tree_centroid(tree, size, deleted, v, u, vertices);
        }
    }
    return u;
}

// void dfs(vector<vector<int>> &tree, vector<bool> &deleted, int u, int p) {
//     for (int v: tree[u]) {
//         if (v == p || deleted[v])continue;
//         dfs(tree, deleted, v, u);
//     }
// }

void decompose(vector<vector<int>> &tree, vector<int> &size, vector<bool> &deleted, int u, int total) {
    calc_sizes(tree, size, deleted, u, -1);
    int centroid = find_tree_centroid(tree, size, deleted, u, -1, total);
    deleted[centroid] = true;

    // process centroid vertex here
    // cout << centroid << endl;

    // for (int v: tree[centroid]) {
    //    if (deleted[v])continue;
    //    dfs(tree, deleted, v, centroid);
    // }

    for (int v : tree[centroid]) {
        if (deleted[v]) continue;
        decompose(tree, size, deleted, v, size[v]);
    }
}

// usage example
int main() {
    vector<vector<int>> tree = {{3},
                                {3},
                                {3},
                                {0, 1, 2}};

    int n = tree.size();
    vector<int> size(n);
    vector<bool> deleted(n);
    decompose(tree, size, deleted, 0, n);
}
<|endoftext|>


<commit_before>/**
 * Render Pipeline C++
 *
 * Copyright (c) 2016-2017 Center of Human-centered Interaction for Coexistence.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or substantial
 * portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
 * LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#pragma once

#include <filename.h>

#include <boost/filesystem.hpp>

#include <render_pipeline/rpcore/config.hpp>

namespace rppanda {

/**
 * Convert Filename path to boost::filesystem path.
 *
 * If the filename path has virtual mount point, this will search the mount point
 * and resolve the mount point.
 */
RENDER_PIPELINE_DECL boost::filesystem::path convert_path(const Filename& path);

RENDER_PIPELINE_DECL Filename convert_path(const boost::filesystem::path& path);

}
<commit_msg>Change to include only required header<commit_after>/**
 * Render Pipeline C++
 *
 * Copyright (c) 2016-2017 Center of Human-centered Interaction for Coexistence.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or substantial
 * portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
 * LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#pragma once

#include <filename.h>

#include <boost/filesystem/path.hpp>

#include <render_pipeline/rpcore/config.hpp>

namespace rppanda {

/**
 * Convert Filename path to boost::filesystem path.
 *
 * If the filename path has virtual mount point, this will search the mount point
 * and resolve the mount point.
 */
RENDER_PIPELINE_DECL boost::filesystem::path convert_path(const Filename& path);

RENDER_PIPELINE_DECL Filename convert_path(const boost::filesystem::path& path);

}
<|endoftext|>