<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: AccessibleComponentBase.cxx,v $
 *
 *  $Revision: 1.16 $
 *
 *  last change: $Author: hr $ $Date: 2007-06-27 16:35:55 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    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., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_svx.hxx"

#include <svx/AccessibleComponentBase.hxx>

#ifndef _COM_SUN_STAR_ACCESSIBILITY_ACCESSIBLE_ROLE_HPP_
#include <com/sun/star/accessibility/AccessibleRole.hpp>
#endif
#ifndef _COM_SUN_STAR_ACCESSIBILITY_XACCESSIBLE_SELECTION_HPP_
#include <com/sun/star/accessibility/XAccessibleSelection.hpp>
#endif
#ifndef _COM_SUN_STAR_CONTAINER_XCHILD_HPP_
#include <com/sun/star/container/XChild.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif
#ifndef _COM_SUN_STAR_DRAWING_XSHAPES_HPP_
#include <com/sun/star/drawing/XShapes.hpp>
#endif
#ifndef _COM_SUN_STAR_DRAWING_XSHAPEDESCRIPTOR_HPP_
#include <com/sun/star/drawing/XShapeDescriptor.hpp>
#endif
#ifndef _COM_SUN_STAR_LANG_INDEXOUTOFBOUNDSEXCEPTION_HPP_
#include <com/sun/star/lang/IndexOutOfBoundsException.hpp>
#endif
#ifndef _TOOLS_COLOR_HXX
#include <tools/color.hxx>
#endif

using namespace ::rtl;
using namespace ::com::sun::star;
using namespace ::com::sun::star::accessibility;

namespace accessibility {

//=====  internal  ============================================================

AccessibleComponentBase::AccessibleComponentBase (void)
{
}

AccessibleComponentBase::~AccessibleComponentBase (void)
{
}

//=====  XAccessibleComponent  ================================================

sal_Bool SAL_CALL AccessibleComponentBase::containsPoint (
        const ::com::sun::star::awt::Point& aPoint)
        throw (::com::sun::star::uno::RuntimeException)
{
    awt::Size aSize (getSize());
    return (aPoint.X >= 0)
        && (aPoint.X < aSize.Width)
        && (aPoint.Y >= 0)
        && (aPoint.Y < aSize.Height);
}

uno::Reference<XAccessible > SAL_CALL
    AccessibleComponentBase::getAccessibleAtPoint (
        const awt::Point& /*aPoint*/)
    throw (uno::RuntimeException)
{
    return uno::Reference<XAccessible>();
}

awt::Rectangle SAL_CALL AccessibleComponentBase::getBounds (void)
    throw (uno::RuntimeException)
{
    return awt::Rectangle();
}

awt::Point SAL_CALL AccessibleComponentBase::getLocation (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    awt::Rectangle aBBox (getBounds());
    return awt::Point (aBBox.X, aBBox.Y);
}

awt::Point SAL_CALL AccessibleComponentBase::getLocationOnScreen (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return awt::Point();
}

::com::sun::star::awt::Size SAL_CALL AccessibleComponentBase::getSize (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    awt::Rectangle aBBox (getBounds());
    return awt::Size (aBBox.Width, aBBox.Height);
}

void SAL_CALL AccessibleComponentBase::addFocusListener (
    const ::com::sun::star::uno::Reference<
    ::com::sun::star::awt::XFocusListener >& /*xListener*/)
    throw (::com::sun::star::uno::RuntimeException)
{
    // Ignored
}

void SAL_CALL AccessibleComponentBase::removeFocusListener (const ::com::sun::star::uno::Reference<
    ::com::sun::star::awt::XFocusListener >& /*xListener*/ )
    throw (::com::sun::star::uno::RuntimeException)
{
    // Ignored
}

void SAL_CALL AccessibleComponentBase::grabFocus (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    uno::Reference<XAccessibleContext> xContext (this, uno::UNO_QUERY);
    uno::Reference<XAccessibleSelection> xSelection (
        xContext->getAccessibleParent(), uno::UNO_QUERY);
    if (xSelection.is())
    {
        // Do a single selection on this object.
        xSelection->clearAccessibleSelection();
        xSelection->selectAccessibleChild (xContext->getAccessibleIndexInParent());
    }
}

sal_Int32 SAL_CALL AccessibleComponentBase::getForeground (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return Color(COL_BLACK).GetColor();
}

sal_Int32 SAL_CALL AccessibleComponentBase::getBackground (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return Color(COL_WHITE).GetColor();
}

//=====  XAccessibleExtendedComponent  ========================================

::com::sun::star::uno::Reference< ::com::sun::star::awt::XFont > SAL_CALL
        AccessibleComponentBase::getFont (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return uno::Reference<awt::XFont>();
}

::rtl::OUString SAL_CALL AccessibleComponentBase::getTitledBorderText (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return ::rtl::OUString::createFromAscii ("");
}

::rtl::OUString SAL_CALL AccessibleComponentBase::getToolTipText (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return ::rtl::OUString::createFromAscii ("");
}

//=====  XTypeProvider  ===================================================

uno::Sequence<uno::Type> SAL_CALL
    AccessibleComponentBase::getTypes (void)
    throw (uno::RuntimeException)
{
    // Get list of types from the context base implementation...
    uno::Sequence<uno::Type> aTypeList (2);
    // ...and add the additional type for the component.
    const uno::Type aComponentType =
         ::getCppuType((const uno::Reference<XAccessibleComponent>*)0);
    const uno::Type aExtendedComponentType =
        ::getCppuType((const uno::Reference<XAccessibleExtendedComponent>*)0);
    aTypeList[0] = aComponentType;
    aTypeList[1] = aExtendedComponentType;

    return aTypeList;
}

} // end of namespace accessibility
<commit_msg>INTEGRATION: CWS changefileheader (1.16.368); FILE MERGED 2008/04/01 15:50:06 thb 1.16.368.3: #i85898# Stripping all external header guards 2008/04/01 12:47:57 thb 1.16.368.2: #i85898# Stripping all external header guards 2008/03/31 14:19:11 rt 1.16.368.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: AccessibleComponentBase.cxx,v $
 * $Revision: 1.17 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_svx.hxx"

#include <svx/AccessibleComponentBase.hxx>

#ifndef _COM_SUN_STAR_ACCESSIBILITY_ACCESSIBLE_ROLE_HPP_
#include <com/sun/star/accessibility/AccessibleRole.hpp>
#endif
#ifndef _COM_SUN_STAR_ACCESSIBILITY_XACCESSIBLE_SELECTION_HPP_
#include <com/sun/star/accessibility/XAccessibleSelection.hpp>
#endif
#include <com/sun/star/container/XChild.hpp>
#include <com/sun/star/beans/XPropertySet.hpp>
#include <com/sun/star/drawing/XShapes.hpp>
#include <com/sun/star/drawing/XShapeDescriptor.hpp>
#include <com/sun/star/lang/IndexOutOfBoundsException.hpp>
#include <tools/color.hxx>

using namespace ::rtl;
using namespace ::com::sun::star;
using namespace ::com::sun::star::accessibility;

namespace accessibility {

//=====  internal  ============================================================

AccessibleComponentBase::AccessibleComponentBase (void)
{
}

AccessibleComponentBase::~AccessibleComponentBase (void)
{
}

//=====  XAccessibleComponent  ================================================

sal_Bool SAL_CALL AccessibleComponentBase::containsPoint (
        const ::com::sun::star::awt::Point& aPoint)
        throw (::com::sun::star::uno::RuntimeException)
{
    awt::Size aSize (getSize());
    return (aPoint.X >= 0)
        && (aPoint.X < aSize.Width)
        && (aPoint.Y >= 0)
        && (aPoint.Y < aSize.Height);
}

uno::Reference<XAccessible > SAL_CALL
    AccessibleComponentBase::getAccessibleAtPoint (
        const awt::Point& /*aPoint*/)
    throw (uno::RuntimeException)
{
    return uno::Reference<XAccessible>();
}

awt::Rectangle SAL_CALL AccessibleComponentBase::getBounds (void)
    throw (uno::RuntimeException)
{
    return awt::Rectangle();
}

awt::Point SAL_CALL AccessibleComponentBase::getLocation (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    awt::Rectangle aBBox (getBounds());
    return awt::Point (aBBox.X, aBBox.Y);
}

awt::Point SAL_CALL AccessibleComponentBase::getLocationOnScreen (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return awt::Point();
}

::com::sun::star::awt::Size SAL_CALL AccessibleComponentBase::getSize (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    awt::Rectangle aBBox (getBounds());
    return awt::Size (aBBox.Width, aBBox.Height);
}

void SAL_CALL AccessibleComponentBase::addFocusListener (
    const ::com::sun::star::uno::Reference<
    ::com::sun::star::awt::XFocusListener >& /*xListener*/)
    throw (::com::sun::star::uno::RuntimeException)
{
    // Ignored
}

void SAL_CALL AccessibleComponentBase::removeFocusListener (const ::com::sun::star::uno::Reference<
    ::com::sun::star::awt::XFocusListener >& /*xListener*/ )
    throw (::com::sun::star::uno::RuntimeException)
{
    // Ignored
}

void SAL_CALL AccessibleComponentBase::grabFocus (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    uno::Reference<XAccessibleContext> xContext (this, uno::UNO_QUERY);
    uno::Reference<XAccessibleSelection> xSelection (
        xContext->getAccessibleParent(), uno::UNO_QUERY);
    if (xSelection.is())
    {
        // Do a single selection on this object.
        xSelection->clearAccessibleSelection();
        xSelection->selectAccessibleChild (xContext->getAccessibleIndexInParent());
    }
}

sal_Int32 SAL_CALL AccessibleComponentBase::getForeground (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return Color(COL_BLACK).GetColor();
}

sal_Int32 SAL_CALL AccessibleComponentBase::getBackground (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return Color(COL_WHITE).GetColor();
}

//=====  XAccessibleExtendedComponent  ========================================

::com::sun::star::uno::Reference< ::com::sun::star::awt::XFont > SAL_CALL
        AccessibleComponentBase::getFont (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return uno::Reference<awt::XFont>();
}

::rtl::OUString SAL_CALL AccessibleComponentBase::getTitledBorderText (void)
        throw (::com::sun::star::uno::RuntimeException)
{
    return ::rtl::OUString::createFromAscii ("");
}

::rtl::OUString SAL_CALL AccessibleComponentBase::getToolTipText (void)
    throw (::com::sun::star::uno::RuntimeException)
{
    return ::rtl::OUString::createFromAscii ("");
}

//=====  XTypeProvider  ===================================================

uno::Sequence<uno::Type> SAL_CALL
    AccessibleComponentBase::getTypes (void)
    throw (uno::RuntimeException)
{
    // Get list of types from the context base implementation...
    uno::Sequence<uno::Type> aTypeList (2);
    // ...and add the additional type for the component.
    const uno::Type aComponentType =
         ::getCppuType((const uno::Reference<XAccessibleComponent>*)0);
    const uno::Type aExtendedComponentType =
        ::getCppuType((const uno::Reference<XAccessibleExtendedComponent>*)0);
    aTypeList[0] = aComponentType;
    aTypeList[1] = aExtendedComponentType;

    return aTypeList;
}

} // end of namespace accessibility
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/system_wrappers/interface/data_log.h"

#include <cstdio>

#include "gtest/gtest.h"

using ::webrtc::DataLog;

const char* kDataLogFileName = "table_1.txt";

void PerformLogging(std::string table_name) {
  // Simulate normal DataTable logging behavior using this table name.
  ASSERT_EQ(0, DataLog::AddTable(table_name));
  ASSERT_EQ(0, DataLog::AddColumn(table_name, "test", 1));
  for (int i = 0; i < 10; ++i) {
    // TODO(kjellander): Check InsertCell result when the DataLog dummy is
    // fixed.
    DataLog::InsertCell(table_name, "test", static_cast<double>(i));
    ASSERT_EQ(0, DataLog::NextRow(table_name));
  }
}

// Simple test to verify DataLog is still working when the GYP variable
// enable_data_logging==0 (the default case).
TEST(TestDataLogDisabled, VerifyLoggingWorks) {
  ASSERT_EQ(0, DataLog::CreateLog());
  // Generate a table_name name and assure it's an empty string
  // (dummy behavior).
  std::string table_name = DataLog::Combine("table", 1);
  ASSERT_EQ("", table_name);
  PerformLogging(table_name);
  DataLog::ReturnLog();
}

TEST(TestDataLogDisabled, EnsureNoFileIsWritten) {
  // Remove any previous data files on disk:
  std::remove(kDataLogFileName);
  ASSERT_EQ(0, DataLog::CreateLog());
  // Don't use the table name we would get from Combine on a disabled DataLog.
  // Use "table_1" instead (which is what an enabled DataLog would give us).
  PerformLogging("table_1");
  DataLog::ReturnLog();
  // Verify no data log file have been written:
  ASSERT_EQ(NULL, fopen(kDataLogFileName, "r"));
}
<commit_msg>Tool found: pass by value when pass by reference is better in system wrapper unit test.<commit_after>/*
 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/system_wrappers/interface/data_log.h"

#include <cstdio>

#include "gtest/gtest.h"

using ::webrtc::DataLog;

const char* kDataLogFileName = "table_1.txt";

void PerformLogging(const std::string& table_name) {
  // Simulate normal DataTable logging behavior using this table name.
  ASSERT_EQ(0, DataLog::AddTable(table_name));
  ASSERT_EQ(0, DataLog::AddColumn(table_name, "test", 1));
  for (int i = 0; i < 10; ++i) {
    // TODO(kjellander): Check InsertCell result when the DataLog dummy is
    // fixed.
    DataLog::InsertCell(table_name, "test", static_cast<double>(i));
    ASSERT_EQ(0, DataLog::NextRow(table_name));
  }
}

// Simple test to verify DataLog is still working when the GYP variable
// enable_data_logging==0 (the default case).
TEST(TestDataLogDisabled, VerifyLoggingWorks) {
  ASSERT_EQ(0, DataLog::CreateLog());
  // Generate a table_name name and assure it's an empty string
  // (dummy behavior).
  std::string table_name = DataLog::Combine("table", 1);
  ASSERT_EQ("", table_name);
  PerformLogging(table_name);
  DataLog::ReturnLog();
}

TEST(TestDataLogDisabled, EnsureNoFileIsWritten) {
  // Remove any previous data files on disk:
  std::remove(kDataLogFileName);
  ASSERT_EQ(0, DataLog::CreateLog());
  // Don't use the table name we would get from Combine on a disabled DataLog.
  // Use "table_1" instead (which is what an enabled DataLog would give us).
  PerformLogging("table_1");
  DataLog::ReturnLog();
  // Verify no data log file have been written:
  ASSERT_EQ(NULL, fopen(kDataLogFileName, "r"));
}
<|endoftext|>


<commit_before>#include "controller_node.h"
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>
#include <iostream>

Controller::Controller() :
    speedX_(1.05f), speedY_(1.05f), speedZ_(1.05f), speedYaw_(1.01f), goal_reached_(true),
    search_for_plane_(false), stick_to_plane_(false), sticking_distance_(0.5f), correction_speed_(0.5f) {

    std::cout << "Controller node started..." << std::endl;

    // get Ros parameters and set variables
    nh_.param("speedForward", speedX_, speedX_);
    nh_.param("speedRight", speedY_, speedY_);
    nh_.param("speedUp", speedZ_, speedZ_);
    nh_.param("speedYaw", speedYaw_, speedYaw_);

    // set subscriber and publisher
    sub_joy_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Controller::callback, this);
    sub_mocap_pose_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("vrpn_client/estimated_transform", 10, &Controller::setMocapPose, this);
    sub_plane_tf_ = nh_.subscribe<geometry_msgs::TransformStamped>("plane", 10, &Controller::processPlaneMsg, this);
    pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("command/pose", 1);
    pub_stickToSurface_ = nh_.advertise<std_msgs::Bool>("stickToSurface", 10);

    std::cout << "speeds: " << speedX_ << " " << speedY_ << " " << speedZ_ << " " << speedYaw_ << std::endl;

    // identity rotation matrix as quaternion
    tf::Quaternion q;
    q.setRPY(0,0,0);

    // init mocap_tf
    mavToWorld_.setOrigin( tf::Vector3(0,0,0) );
    mavToWorld_.setRotation(q);

    // init transform
    transform_.setOrigin( tf::Vector3(0,0,0) );
    transform_.setRotation(q);

    // TBD if we should be able to change this via launch file
    // init hover transform
    hover_goal_tf_.setOrigin( tf::Vector3(0,0,1) );
    hover_goal_tf_.setRotation(q);

    // init plane_tf
    plane_tf_.setOrigin(tf::Vector3(2,0,0));
    tf::Quaternion plane_q;
    plane_q.setRPY(0,0,-M_PI/4.f);
    plane_tf_.setRotation(plane_q);

    // init snap_goal_tf
    snap_goal_tf_.setOrigin(tf::Vector3(0,0,0));
    snap_goal_tf_.setRotation(q);

    // set sensor offset tf by getting the relative offset
//    tf::TransformListener listener;
//    try {
//        listener.lookupTransform("euroc_hex/ground_truth", "euroc_hex/vi_sensor/ground_truth", ros::Time(0), sensor_offset_tf_);
//    } catch (tf::TransformException ex) {
//        ROS_ERROR("%s",ex.what());
//    }

    // hack
    tf::Transform mavToWorld;
    mavToWorld.setOrigin(tf::Vector3(0,0,0.117));
    mavToWorld.setRotation(tf::Quaternion(0,0,0,1));
    tf::Transform sensorToWorld;
    sensorToWorld.setOrigin(tf::Vector3(0.133,0,0.0605));
    sensorToWorld.setRotation(tf::Quaternion(0,0.17365,0,0.98481));
    sensorToMav_ = mavToWorld.inverse() * sensorToWorld;
}

void Controller::setMocapPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
    mavToWorld_.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z) );
    mavToWorld_.setRotation( tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w) );

    if(!goal_reached_) {
        tf::Transform diff = mavToWorld_.inverse() * hover_goal_tf_;
        if(diff.getOrigin().length() < 0.05f) {
            goal_reached_ = true;
        }
        else {
            transform_ = diff;
        }
    }

    // world transform
    tf::Transform curr_transform;
    /// stick to plane
    if(stick_to_plane_) {
        testPlanes();
        tf::Vector3 diff = snap_goal_tf_.getOrigin() - mavToWorld_.getOrigin();
        curr_transform.setOrigin(mavToWorld_.getOrigin() + correction_speed_*diff);
        curr_transform.setRotation(snap_goal_tf_.getRotation());
    } else {
        curr_transform = mavToWorld_ * transform_;
    }

    // convert tf into pose and publish the pose
    tf::Vector3 desired_pos = curr_transform.getOrigin();
    tf::Quaternion desired_rot = curr_transform.getRotation();

    geometry_msgs::PoseStamped pose;
    // set header
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "world";
    // set pose
    pose.pose.position.x = desired_pos.x();
    pose.pose.position.y = desired_pos.y();
    pose.pose.position.z = desired_pos.z();
    pose.pose.orientation.x = desired_rot.x();
    pose.pose.orientation.y = desired_rot.y();
    pose.pose.orientation.z = desired_rot.z();
    pose.pose.orientation.w = desired_rot.w();
    pub_pose_.publish(pose);

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(curr_transform, ros::Time::now(), "world", "controller") );
}

void Controller::callback(const sensor_msgs::Joy::ConstPtr& joy) {
    /// translation from controller axis
    float jx = speedX_ * joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS];
    float jy = speedY_ * joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
    float jz = speedZ_ * joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS];
    // yaw from axis
    float jr = speedYaw_ * joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];

    // save only the latest relative transform in global variable transform_
    transform_.setOrigin( tf::Vector3(jx,jy,jz) );
    tf::Quaternion q;
    q.setRPY(0,0,jr);
    transform_.setRotation(q);

    /// buttons
    // tell PcMeshBuilder to search for a plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1]) {
        search_for_plane_ = true;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_1]) {
        search_for_plane_ = false;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    // enable or disable sticking to the plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2]) {
        stick_to_plane_ = true;
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_2]) {
        stick_to_plane_ = false;
    }
    // sticking distance
    if(joy->buttons[PS3_BUTTON_CROSS_UP] && sticking_distance_ > 0.05f) {
        sticking_distance_ -= 0.005f;
    }
    if(joy->buttons[PS3_BUTTON_CROSS_DOWN]) {
        sticking_distance_ += 0.005f;
    }
}

void Controller::takeoffAndHover() {
    std_srvs::Empty::Request request;
    std_srvs::Empty::Response response;
    ros::service::call("euroc2/takeoff", request, response);

    tf::Vector3 desired_pos = tf::Vector3( mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), 1.0f);
    tf::Quaternion desired_rot = mavToWorld_.getRotation();

    hover_goal_tf_.setOrigin(desired_pos);
    hover_goal_tf_.setRotation(desired_rot);
}

// TODO add constant offset transform for camera
// TODO improve handedness correction
void Controller::processPlaneMsg(const geometry_msgs::TransformStamped::ConstPtr& msg) {
    // realtive plane transform to mav
    //planeToCam = plane_tf
    plane_tf_.setOrigin( tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z) );
    plane_tf_.setRotation( tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w) );

    //plane_tf_.setOrigin( tf::Vector3(0,0,0) ); //TODO rm debug
    //plane_tf_.setRotation( tf::Quaternion::getIdentity() );

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "plane_Cam_World") );
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Cam") );

    // plane forward is z should be x, hack to fix it for testing
    //CamToSensor
    tf::Quaternion correction_rot;
//    correction_rot.setRPY(M_PI/2.0,-M_PI/2.0,0);
    correction_rot.setRPY(M_PI/2.0,-M_PI/2.0,0);
    plane_tf_.setOrigin(tf::Matrix3x3(correction_rot)*plane_tf_.getOrigin());
    plane_tf_.setRotation(correction_rot*plane_tf_.getRotation());
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Sensor") );

    // correct the relative camera offset
    // planeToMav = plane_tf CORRECT
    plane_tf_ = sensorToMav_*plane_tf_;
    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/ground_truth", "plane_Mav") );

    // transform into global coordinates
    // planeToWorld = plane_tf_ CORRECT
    plane_tf_ = mavToWorld_*plane_tf_;
    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "plane_Global") );
}

// TODO remove hacks enhance methods
void Controller::testPlanes() {
    //// mav
    // predicted mav tf
    tf::Transform mav_tf = mavToWorld_ * transform_;

    // Eigen conversions
    Eigen::Vector3f mav_pos = Eigen::Vector3f( mav_tf.getOrigin().x(),
                                               mav_tf.getOrigin().y(),
                                               mav_tf.getOrigin().z());

    //// plane
    Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
    tf::Quaternion plane_q = plane_tf_.getRotation();
    Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "normal") );

    //// calculations
    Eigen::Vector3f normal = plane_rot*forward;
    normal.normalize();
//    normal = -normal;
    // determine if mav is in front or behind plane normal, take current pos to avoid switching of sides by wrong predictions in mav_tf
    Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());

    int facing = normal.dot(curr_pos-plane_pos) >= 0 ? 1 : -1;
    // calculate projected position of the mav onto the plane
    Eigen::Vector3f proj_pos = mav_pos + (facing*sticking_distance_ - normal.dot(mav_pos-plane_pos))*normal;

    // another hack
//    tf::Quaternion correction_rot;
//    correction_rot.setRPY(0,0,M_PI);

    // set snapping goal tf
    snap_goal_tf_.setOrigin( tf::Vector3(proj_pos.x(), proj_pos.y(), proj_pos.z()) );
//    snap_goal_tf_.setRotation(correction_rot*plane_tf_.getRotation());
    snap_goal_tf_.setRotation(plane_tf_.getRotation());
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "te_joy_controller");
    Controller rc;
    ros::spin();
}
<commit_msg>Found a strange bug with rotations. WIP<commit_after>#include "controller_node.h"
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>
#include <iostream>

Controller::Controller() :
    speedX_(1.05f), speedY_(1.05f), speedZ_(1.05f), speedYaw_(1.01f), goal_reached_(true),
    search_for_plane_(false), stick_to_plane_(false), sticking_distance_(0.5f), correction_speed_(0.5f) {

    std::cout << "Controller node started..." << std::endl;

    // get Ros parameters and set variables
    nh_.param("speedForward", speedX_, speedX_);
    nh_.param("speedRight", speedY_, speedY_);
    nh_.param("speedUp", speedZ_, speedZ_);
    nh_.param("speedYaw", speedYaw_, speedYaw_);

    // set subscriber and publisher
    sub_joy_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Controller::callback, this);
    sub_mocap_pose_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("vrpn_client/estimated_transform", 10, &Controller::setMocapPose, this);
    sub_plane_tf_ = nh_.subscribe<geometry_msgs::TransformStamped>("plane", 10, &Controller::processPlaneMsg, this);
    pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("command/pose", 1);
    pub_stickToSurface_ = nh_.advertise<std_msgs::Bool>("stickToSurface", 10);

    std::cout << "speeds: " << speedX_ << " " << speedY_ << " " << speedZ_ << " " << speedYaw_ << std::endl;

    // identity rotation matrix as quaternion
    tf::Quaternion q;
    q.setRPY(0,0,0);

    // init mocap_tf
    mavToWorld_.setOrigin( tf::Vector3(0,0,0) );
    mavToWorld_.setRotation(q);

    // init transform
    transform_.setOrigin( tf::Vector3(0,0,0) );
    transform_.setRotation(q);

    // TBD if we should be able to change this via launch file
    // init hover transform
    hover_goal_tf_.setOrigin( tf::Vector3(0,0,1) );
    hover_goal_tf_.setRotation(q);

    // init plane_tf
    plane_tf_.setOrigin(tf::Vector3(2,0,0));
    tf::Quaternion plane_q;
    plane_q.setRPY(0,0,-M_PI/4.f);
    plane_tf_.setRotation(plane_q);

    // init snap_goal_tf
    snap_goal_tf_.setOrigin(tf::Vector3(0,0,0));
    snap_goal_tf_.setRotation(q);

    // set sensor offset tf by getting the relative offset
//    tf::TransformListener listener;
//    try {
//        listener.lookupTransform("euroc_hex/ground_truth", "euroc_hex/vi_sensor/ground_truth", ros::Time(0), sensor_offset_tf_);
//    } catch (tf::TransformException ex) {
//        ROS_ERROR("%s",ex.what());
//    }

    // hack
    tf::Transform mavToWorld;
    mavToWorld.setOrigin(tf::Vector3(0,0,0.117));
    mavToWorld.setRotation(tf::Quaternion(0,0,0,1));
    tf::Transform sensorToWorld;
    sensorToWorld.setOrigin(tf::Vector3(0.133,0,0.0605));
    sensorToWorld.setRotation(tf::Quaternion(0,0.17365,0,0.98481));
    sensorToMav_ = mavToWorld.inverse() * sensorToWorld;
}

void Controller::setMocapPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
    mavToWorld_.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z) );
    mavToWorld_.setRotation( tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w) );

    if(!goal_reached_) {
        tf::Transform diff = mavToWorld_.inverse() * hover_goal_tf_;
        if(diff.getOrigin().length() < 0.05f) {
            goal_reached_ = true;
        }
        else {
            transform_ = diff;
        }
    }

    // world transform
    tf::Transform curr_transform;
    /// stick to plane
    if(stick_to_plane_) {
        testPlanes();
        tf::Vector3 diff = snap_goal_tf_.getOrigin() - mavToWorld_.getOrigin();
        curr_transform.setOrigin(mavToWorld_.getOrigin() + correction_speed_*diff);
        curr_transform.setRotation(snap_goal_tf_.getRotation());
    } else {
        curr_transform = mavToWorld_ * transform_;
    }

    // convert tf into pose and publish the pose
    tf::Vector3 desired_pos = curr_transform.getOrigin();
    tf::Quaternion desired_rot = curr_transform.getRotation();

    geometry_msgs::PoseStamped pose;
    // set header
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "world";
    // set pose
    pose.pose.position.x = desired_pos.x();
    pose.pose.position.y = desired_pos.y();
    pose.pose.position.z = desired_pos.z();
    pose.pose.orientation.x = desired_rot.x();
    pose.pose.orientation.y = desired_rot.y();
    pose.pose.orientation.z = desired_rot.z();
    pose.pose.orientation.w = desired_rot.w();
    pub_pose_.publish(pose);

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(curr_transform, ros::Time::now(), "world", "controller") );
}

void Controller::callback(const sensor_msgs::Joy::ConstPtr& joy) {
    /// translation from controller axis
    float jx = speedX_ * joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS];
    float jy = speedY_ * joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
    float jz = speedZ_ * joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS];
    // yaw from axis
    float jr = speedYaw_ * joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];

    // save only the latest relative transform in global variable transform_
    transform_.setOrigin( tf::Vector3(jx,jy,jz) );
    tf::Quaternion q;
    q.setRPY(0,0,jr);
    transform_.setRotation(q);

    /// buttons
    // tell PcMeshBuilder to search for a plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1]) {
        search_for_plane_ = true;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_1]) {
        search_for_plane_ = false;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    // enable or disable sticking to the plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2]) {
        stick_to_plane_ = true;
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_2]) {
        stick_to_plane_ = false;
    }
    // sticking distance
    if(joy->buttons[PS3_BUTTON_CROSS_UP] && sticking_distance_ > 0.05f) {
        sticking_distance_ -= 0.005f;
    }
    if(joy->buttons[PS3_BUTTON_CROSS_DOWN]) {
        sticking_distance_ += 0.005f;
    }
}

void Controller::takeoffAndHover() {
    std_srvs::Empty::Request request;
    std_srvs::Empty::Response response;
    ros::service::call("euroc2/takeoff", request, response);

    tf::Vector3 desired_pos = tf::Vector3( mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), 1.0f);
    tf::Quaternion desired_rot = mavToWorld_.getRotation();

    hover_goal_tf_.setOrigin(desired_pos);
    hover_goal_tf_.setRotation(desired_rot);
}

// TODO add constant offset transform for camera
// TODO improve handedness correction
void Controller::processPlaneMsg(const geometry_msgs::TransformStamped::ConstPtr& msg) {
    // realtive plane transform to mav
    //planeToCam = plane_tf
    plane_tf_.setOrigin( tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z) );
    plane_tf_.setRotation( tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w) );

    //plane_tf_.setOrigin( tf::Vector3(0,0,0) ); //TODO rm debug
    //plane_tf_.setRotation( tf::Quaternion::getIdentity() );

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "plane_Cam_World") );
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Cam") );

    // plane forward is z should be x, hack to fix it for testing
    //CamToSensor
//    tf::Matrix3x3 tmp_rot = tf::Matrix3x3(0,-1,0,0,0,-1,1,0,0);
    tf::Quaternion q1; q1.setRPY(0,M_PI/2.0,0);
    tf::Quaternion q2; q2.setRPY(-M_PI/2.0,0,0);
    tf::Quaternion correction_rot = q2*q1;
    plane_tf_.setOrigin( tf::Matrix3x3(correction_rot)*plane_tf_.getOrigin() );
    plane_tf_.setRotation( plane_tf_.getRotation() );
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Sensor") );

    /// testing ///
////    tf::Quaternion q1; q1.setRPY(0,-M_PI/2.0,0);
////    tf::Quaternion q2; q2.setRPY(M_PI/2.0,0,0);
////    tf::Quaternion q3; q3.setRPY(0,0,M_PI);
//    tf::Quaternion q1; q1.setRPY(0,M_PI/2.0,0);
//    tf::Quaternion q2; q2.setRPY(-M_PI/2.0,0,0);
//    tf::Quaternion q3; q3.setRPY(0,0,0);
//    correction_rot = q3*q2*q1;
//    tf::Transform test0;
//    test0.setOrigin(tf::Vector3(-0.5,-0.5,1));
//    test0.setRotation(tf::Quaternion::getIdentity());
//    br_tf_.sendTransform( tf::StampedTransform(test0, ros::Time::now(), "world", "test0") );
//    tf::Transform test1;
//    test1.setOrigin( tf::Matrix3x3(correction_rot)*test0.getOrigin() );
//    test1.setRotation( test0.getRotation() );
//    br_tf_.sendTransform( tf::StampedTransform(test1, ros::Time::now(), "world", "test1") );
    /// end of testing section ///

    // correct the relative camera offset
    // planeToMav = plane_tf CORRECT
    plane_tf_ = sensorToMav_.inverse()*plane_tf_;

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "plane_Mav_World") );
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/ground_truth", "plane_Mav") );

    // transform into global coordinates
    // planeToWorld = plane_tf_ CORRECT
    plane_tf_ = mavToWorld_*plane_tf_;
    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "plane_Global") );
}

// TODO remove hacks enhance methods
void Controller::testPlanes() {
    //// mav
    // predicted mav tf
    tf::Transform mav_tf = mavToWorld_ * transform_;

    // Eigen conversions
    Eigen::Vector3f mav_pos = Eigen::Vector3f( mav_tf.getOrigin().x(),
                                               mav_tf.getOrigin().y(),
                                               mav_tf.getOrigin().z());

    //// plane
    Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
    tf::Quaternion plane_q = plane_tf_.getRotation();
    Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "normal") );

    //// calculations
    Eigen::Vector3f normal = plane_rot*forward;
    normal.normalize();
//    normal = -normal;
    // determine if mav is in front or behind plane normal, take current pos to avoid switching of sides by wrong predictions in mav_tf
    Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());

    int facing = normal.dot(curr_pos-plane_pos) >= 0 ? 1 : -1;
    // calculate projected position of the mav onto the plane
    Eigen::Vector3f proj_pos = mav_pos + (facing*sticking_distance_ - normal.dot(mav_pos-plane_pos))*normal;

    // another hack
//    tf::Quaternion correction_rot;
//    correction_rot.setRPY(0,0,M_PI);

    // set snapping goal tf
    snap_goal_tf_.setOrigin( tf::Vector3(proj_pos.x(), proj_pos.y(), proj_pos.z()) );
//    snap_goal_tf_.setRotation(correction_rot*plane_tf_.getRotation());
    snap_goal_tf_.setRotation(plane_tf_.getRotation());
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "te_joy_controller");
    Controller rc;
    ros::spin();
}
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p10/procedures/hwp/pm/p10_pm.H $             */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p10_pm.H
/// @brief Common header for Power Manangement procedures
///

// *HWP Backup HWP Owner: Greg Still <stillgs@us.ibm.com>
// *HWP FW Owner        : Amit Tendolkar <amit.tendolkar@in.ibm.com>
// *HWP Team            : PM
// *HWP Level           : 2
// *HWP Consumed by     : HS

#ifndef _PM_H_
#define _PM_H_

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Macro Defintions
//------------------------------------------------------------------------------

extern const char* PM_FLOW_MODE_NAME[];

#define PM_FLOW_MODE_NAME   \
    {                           \
        "PM_HALT",              \
        "PM_START",             \
        "PM_SETUP",             \
        "PM_SETUP_PIB",         \
        "PM_SETUP_ALL",         \
        "PM_RESET_SOFT",        \
        "PM_INIT_SOFT",         \
        "PM_INIT_SPECIAL"       \
    }

#define PM_MODE_NAME_VAR PM_FLOW_MODE_NAME[] = PM_FLOW_MODE_NAME

#define PM_MODE_NAME(_mi_mode)(         \
                                        PM_FLOW_MODE_NAME[_mi_mode-1]    \
                              )

// Incase the attribute does not a have a value previously assigned,
// assign the user given default value
#define GETATTR_DEFAULT(_i_attr, _i_attr_name, _i_target, _i_value, _i_defVal){\
        FAPI_ATTR_GET(_i_attr, _i_target, _i_value); \
        if (!_i_value) \
        { \
            FAPI_DBG("Attribute %s set to default = 0x%x",_i_attr_name,_i_defVal); \
            _i_value = _i_defVal;\
        } \
        FAPI_INF("Value read from attribute %s = 0x%x", _i_attr_name,_i_value);\
    }

//------------------------------------------------------------------------------
// Constant definitions
//------------------------------------------------------------------------------
namespace pm
{

enum PM_FLOW_MODE
{
    PM_HALT         = 0x1,
    PM_START        = 0x2,
    PM_SETUP        = 0x3,
    PM_SETUP_PIB    = 0x4,
    PM_SETUP_ALL    = 0x5,
    PM_RESET_SOFT   = 0x6,
    PM_INIT_SOFT    = 0x7,
    PM_INIT_SPECIAL = 0x8,
};

} // end of namespace pm

//------------------------------------------------------------------------------
// Functions
//------------------------------------------------------------------------------

// Byte-reverse a 32-bit integer if on a little-endian machine

uint32_t
revle32(uint32_t i_x)
{
    uint32_t rx;

#ifndef _BIG_ENDIAN
    uint8_t* pix = (uint8_t*)(&i_x);
    uint8_t* prx = (uint8_t*)(&rx);

    prx[0] = pix[3];
    prx[1] = pix[2];
    prx[2] = pix[1];
    prx[3] = pix[0];
#else
    rx = i_x;
#endif

    return rx;
}

// Byte-reverse a 64-bit integer if on a little-endian machine

uint64_t
revle64(const uint64_t i_x)
{
    uint64_t rx;

#ifndef _BIG_ENDIAN
    uint8_t* pix = (uint8_t*)(&i_x);
    uint8_t* prx = (uint8_t*)(&rx);

    prx[0] = pix[7];
    prx[1] = pix[6];
    prx[2] = pix[5];
    prx[3] = pix[4];
    prx[4] = pix[3];
    prx[5] = pix[2];
    prx[6] = pix[1];
    prx[7] = pix[0];
#else
    rx = i_x;
#endif

    return rx;
}

#endif // _PM_H_
<commit_msg>PM: P10 OCB Hardware Procedures (with new OCI headers)<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p10/procedures/hwp/pm/p10_pm.H $             */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p10_pm.H
/// @brief Common header for Power Manangement procedures
///

// *HWP Backup HWP Owner: Greg Still <stillgs@us.ibm.com>
// *HWP FW Owner        : Amit Tendolkar <amit.tendolkar@in.ibm.com>
// *HWP Team            : PM
// *HWP Level           : 2
// *HWP Consumed by     : HS

#ifndef _PM_H_
#define _PM_H_

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Macro Defintions
//------------------------------------------------------------------------------

extern const char* PM_FLOW_MODE_NAME[];

#define PM_FLOW_MODE_NAME   \
    {                           \
        "PM_HALT",              \
        "PM_START",             \
        "PM_SETUP",             \
        "PM_SETUP_PIB",         \
        "PM_SETUP_ALL",         \
        "PM_RESET_SOFT",        \
        "PM_INIT_SOFT",         \
        "PM_INIT_SPECIAL"       \
    }

#define PM_MODE_NAME_VAR PM_FLOW_MODE_NAME[] = PM_FLOW_MODE_NAME

#define PM_MODE_NAME(_mi_mode)(         \
                                        PM_FLOW_MODE_NAME[_mi_mode-1]    \
                              )

// Incase the attribute does not a have a value previously assigned,
// assign the user given default value
#define GETATTR_DEFAULT(_i_attr, _i_attr_name, _i_target, _i_value, _i_defVal){\
        FAPI_ATTR_GET(_i_attr, _i_target, _i_value); \
        if (!_i_value) \
        { \
            FAPI_DBG("Attribute %s set to default = 0x%x",_i_attr_name,_i_defVal); \
            _i_value = _i_defVal;\
        } \
        FAPI_INF("Value read from attribute %s = 0x%x", _i_attr_name,_i_value);\
    }

//------------------------------------------------------------------------------
// Constant definitions
//------------------------------------------------------------------------------
namespace pm
{

enum PM_FLOW_MODE
{
    PM_HALT         = 0x1,
    PM_START        = 0x2,
    PM_SETUP        = 0x3,
    PM_SETUP_PIB    = 0x4,
    PM_SETUP_ALL    = 0x5,
    PM_RESET_SOFT   = 0x6,
    PM_INIT_SOFT    = 0x7,
    PM_INIT_SPECIAL = 0x8,
};

} // end of namespace pm

#endif // _PM_H_
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/hwpf/fapi2/include/error_info_defs.H $             */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2011,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file error_info_defs.H
/// @brief Defines to support the Error Information class
///

#ifndef FAPI2_ERRORINFO_DEFS_H_
#define FAPI2_ERRORINFO_DEFS_H_

#include <stdint.h>
#include <target.H>

#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
    #include <variable_buffer.H>
    #include <utility>
#endif
namespace fapi2
{

///
/// @brief Type to hold the ffdc data to be sent to hostboot
///
/// Note: Typical data sent seems to be register/addresss info
///       rather than use extra space converting stuff just
///       send a uint64 always
///
struct sbeFfdc_t
{
    uint32_t  size;
    uint64_t  data;
} __attribute__((packed));

// 10 entries limits the size of SbeFfdcData_t to 128 bytes
enum
{
    MAX_SBE_FFDC_ENTRIES = 10
};

// Data type for SBE ffdc buffer sent through fifo
typedef struct
{
    uint32_t  fapiRc;      // Return code from failure
    uint32_t  ffdcLength;  // length of Fapi FFDC data (in bytes)
    struct sbeFfdc_t ffdcData[MAX_SBE_FFDC_ENTRIES]; // fapi FFDC data
} SbeFfdcData_t; // 128 bytes

///
/// @brief Type to hold the ffdc element in the ffdc class
/// Needed so that the size can be squirled away before the
/// macro is called.
///
struct ffdc_struct
{
    const void* ptr;
    size_t size;
};

class ffdc_t
{
    public:
        ffdc_t(void)
        {}

        void operator=(const ffdc_t& i )
        {
            iv_value.ptr  = i.ptr();
            iv_value.size = i.size();
        }

        operator const void* () const
        {
            return iv_value.ptr;
        }
        operator uint8_t() const
        {
            return *(reinterpret_cast<const uint8_t*>(iv_value.ptr));
        }

        size_t size(void) const
        {
            return iv_value.size;
        }
        size_t& size(void)
        {
            return iv_value.size;
        }

        const void* ptr(void) const
        {
            return iv_value.ptr;
        }
        const void*& ptr(void)
        {
            return iv_value.ptr;
        }

    private:
        struct ffdc_struct iv_value = {}; //init to zero
};

///
/// @brief Enumeration of ErrorInfo FFDC sizes that are used to indicate a
///        special type that cannot simply be memcopied
enum ErrorInfoFfdcSize
{
    EI_FFDC_SIZE_BUF    = 0xffff,   // fapi2::buffer<T>
    EI_FFDC_SIZE_TARGET = 0xfffe,   // fapi2::Target
    EI_FFDC_SIZE_VBUF   = 0xfffd,   // fapi2::variable_buffer
    EI_FFDC_MAX_SIZE    = 0x1000,   // Limit regular FFDC capture to 4kb
};

///
/// @brief Enumeration of error log severity.
///
enum errlSeverity_t
{
    FAPI2_ERRL_SEV_UNDEFINED     = 0x00, /// Used internally by ffdc mechanism
    FAPI2_ERRL_SEV_RECOVERED     = 0x10, /// Not seen by customer
    FAPI2_ERRL_SEV_PREDICTIVE    = 0x20, /// Error recovered but customer will see
    FAPI2_ERRL_SEV_UNRECOVERABLE = 0x40  /// Unrecoverable, general
};

///
/// @brief Enumeration of ErrorInfo types
///
enum ErrorInfoType
{
    EI_TYPE_FFDC              = 0,
    EI_TYPE_HW_CALLOUT        = 1,
    EI_TYPE_PROCEDURE_CALLOUT = 2,
    EI_TYPE_BUS_CALLOUT       = 3,
    EI_TYPE_CDG               = 4, // Target Callout/Deconfig/GARD
    EI_TYPE_CHILDREN_CDG      = 5, // Children Callout/Deconfig/GARD
    EI_TYPE_COLLECT_TRACE     = 6,
    EI_LAST_TYPE              = EI_TYPE_COLLECT_TRACE + 1,
};

#ifndef MINIMUM_FFDC
///
/// @enum HwCallout
///
/// This enumeration defines the possible Hardware Callouts that are not
/// represented by fapi2::Targets
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform callout value
/// so do not reorder without consulting all platforms
///
namespace HwCallouts
{
enum HwCallout
{
    // Where indicated, a HW Callout in FAPI Error XML must include a
    // reference target that is used to identify the HW. e.g. for
    // TOD_CLOCK, the proc chip that the clock is attached to must be
    // specified
    TOD_CLOCK           = 0, // Include proc-chip ref (or child chiplet)
    MEM_REF_CLOCK       = 1, // Include membuf-chip ref (or child chiplet)
    PROC_REF_CLOCK      = 2, // Include proc-chip ref (or child chiplet)
    PCI_REF_CLOCK       = 3, // Include proc-chip ref (or child chiplet)
    FLASH_CONTROLLER_PART      = 4,
    PNOR_PART                  = 5,
    SBE_SEEPROM_PART           = 6,
    VPD_PART                   = 7,
    LPC_SLAVE_PART             = 8,
    GPIO_EXPANDER_PART         = 9,
    SPIVID_SLAVE_PART          = 10,
};
}

///
/// @enum ProcedureCallout
///
/// This enumeration defines the possible Procedure Callouts
/// These instruct the customer/customer-engineer what to do
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform callout value
/// so do not reorder without consulting all platforms
///
namespace ProcedureCallouts
{
enum ProcedureCallout
{
    CODE                  = 0, // Code problem
    LVL_SUPPORT           = 1, // Call next level of support
    MEMORY_PLUGGING_ERROR = 2, // DIMM Plugging error
    BUS_CALLOUT           = 3, // Bus Called Out
};
}

///
/// @enum CalloutPriority
///
/// This enumeration defines the possible Procedure and Target callout priorities
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform priority value
/// so do not reorder without consulting all platforms
///
namespace CalloutPriorities
{
enum CalloutPriority
{
    LOW = 0,
    MEDIUM = 1,
    HIGH   = 2,
    NONE   = 3,
};
}

///
/// @enum CollectTrace
///
/// This enumeration defines the possible firmware traces to collect
///
namespace CollectTraces
{
const uint32_t TRACE_SIZE = 256; // limit collected trace size
enum CollectTrace
{
    FSI              = 1,
    SCOM             = 2,
    SCAN             = 3,
    MBOX             = 4,
};
}

// NOTE - this assumes no buffer_t or variable_buffers are passed
//        data is converted to a uint64_t when placed into the sbe ffdc
//        buffer
inline fapi2::ffdc_t getFfdcData( sbeFfdc_t& i_sbeFfdc )
{
    fapi2::ffdc_t temp;

    temp.size() = static_cast<size_t>(i_sbeFfdc.size);

    if(temp.size() == EI_FFDC_SIZE_TARGET)
    {
#ifdef FAPI2_ENABLE_PLATFORM_GET_TARGET
        uint64_t targetData = i_sbeFfdc.data;
        fapi2::TargetType type = static_cast<fapi2::TargetType>(targetData >> 32);
        uint8_t instance = static_cast<uint8_t>(targetData & 0xFFFFFFFF);
        // call hostboot to get the fapi2 target reference
        temp.ptr() = static_cast<void*>(getTarget<TARGET_TYPE_ALL>(type, instance));
#endif
    }
    else
    {
        // adjust the pointer based on the data size.
        temp.ptr() = static_cast<void*>(reinterpret_cast<uint8_t*>(&i_sbeFfdc.data) +
                                        (sizeof(uint64_t) - i_sbeFfdc.size));
    }

    return temp;
}
#endif
///
/// @brief Get FFDC Size
///
/// This is called by the FAPI_SET_HWP_ERROR macro to find out the size of
/// FFDC data. If the data is of a special type that is handled differently
/// than types that are simply memcopied then it is handled by a template
/// specialization.
/// If this function template is instantiated with a pointer, the compile
/// will fail.
///
/// @return uint16_t. Size of the FFDC data
///
template<typename T>
inline uint16_t getErrorInfoFfdcSize(const T&)
{
    static_assert(sizeof(T) <= EI_FFDC_MAX_SIZE,
                  "FFDC too large to capture");
    return sizeof(T);
}
#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
///
/// @brief Compile error if caller tries to get the FFDC size of a pointer
///
template<typename T>
inline uint16_t getErrorInfoFfdcSize(const T*)
{
    static_assert(std::is_pointer<T>::value,
                  "pointer passed to getErrorInfoFfdcSize");
    return 0;
}
#endif
///
/// @brief Get FFDC Size specialization for fapi2::Target
///
template<fapi2::TargetType T, typename V>
inline uint16_t getErrorInfoFfdcSize(const fapi2::Target<T, V>&)
{
    return EI_FFDC_SIZE_TARGET;
}

#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
///
/// @brief Get FFDC Size specialization for variable buffers
///
template<>
inline uint16_t getErrorInfoFfdcSize(const fapi2::variable_buffer& i_thing)
{
    // Limit a variable buffer to 4kb bytes, and we can memcpy the storage.
    return std::min(static_cast<uint32_t>(EI_FFDC_MAX_SIZE),
                    i_thing.getLength<uint8_t>());
}
#endif

///
/// @brief Get FFDC Size specialization for ffdc_t
///
template<>
inline uint16_t getErrorInfoFfdcSize(const fapi2::ffdc_t& i_ffdc)
{
    return static_cast<uint16_t>(i_ffdc.size());
}

};

#endif // FAPI2_ERRORINFO_DEFS_H_
<commit_msg>Optimized PPE FFDC collection framework<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/hwpf/fapi2/include/error_info_defs.H $             */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2011,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file error_info_defs.H
/// @brief Defines to support the Error Information class
///

#ifndef FAPI2_ERRORINFO_DEFS_H_
#define FAPI2_ERRORINFO_DEFS_H_

#include <stdint.h>
#include <target.H>

#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
    #include <variable_buffer.H>
    #include <utility>
#endif
namespace fapi2
{

///
/// @brief Type to hold the ffdc data to be sent to hostboot
///
/// Note: Typical data sent seems to be register/addresss info
///       rather than use extra space converting stuff just
///       send a uint64 always
///
struct sbeFfdc_t
{
    uint32_t  size;
    uint64_t  data;
} __attribute__((packed));

// 20 entries limits the size of SbeFfdcData_t under 256 bytes
enum
{
    MAX_SBE_FFDC_ENTRIES = 20
};

// Data type for SBE ffdc buffer sent through fifo
typedef struct
{
    uint32_t  fapiRc;      // Return code from failure
    uint32_t  ffdcLength;  // length of Fapi FFDC data (in bytes)
    struct sbeFfdc_t ffdcData[MAX_SBE_FFDC_ENTRIES]; // fapi FFDC data
} SbeFfdcData_t; // ~248 bytes

///
/// @brief Type to hold the ffdc element in the ffdc class
/// Needed so that the size can be squirled away before the
/// macro is called.
///
struct ffdc_struct
{
    const void* ptr;
    size_t size;
};

class ffdc_t
{
    public:
        ffdc_t(void)
        {}

        void operator=(const ffdc_t& i )
        {
            iv_value.ptr  = i.ptr();
            iv_value.size = i.size();
        }

        operator const void* () const
        {
            return iv_value.ptr;
        }
        operator uint8_t() const
        {
            return *(reinterpret_cast<const uint8_t*>(iv_value.ptr));
        }

        size_t size(void) const
        {
            return iv_value.size;
        }
        size_t& size(void)
        {
            return iv_value.size;
        }

        const void* ptr(void) const
        {
            return iv_value.ptr;
        }
        const void*& ptr(void)
        {
            return iv_value.ptr;
        }

    private:
        struct ffdc_struct iv_value = {}; //init to zero
};

///
/// @brief Enumeration of ErrorInfo FFDC sizes that are used to indicate a
///        special type that cannot simply be memcopied
enum ErrorInfoFfdcSize
{
    EI_FFDC_SIZE_BUF    = 0xffff,   // fapi2::buffer<T>
    EI_FFDC_SIZE_TARGET = 0xfffe,   // fapi2::Target
    EI_FFDC_SIZE_VBUF   = 0xfffd,   // fapi2::variable_buffer
    EI_FFDC_MAX_SIZE    = 0x1000,   // Limit regular FFDC capture to 4kb
};

///
/// @brief Enumeration of error log severity.
///
enum errlSeverity_t
{
    FAPI2_ERRL_SEV_UNDEFINED     = 0x00, /// Used internally by ffdc mechanism
    FAPI2_ERRL_SEV_RECOVERED     = 0x10, /// Not seen by customer
    FAPI2_ERRL_SEV_PREDICTIVE    = 0x20, /// Error recovered but customer will see
    FAPI2_ERRL_SEV_UNRECOVERABLE = 0x40  /// Unrecoverable, general
};

///
/// @brief Enumeration of ErrorInfo types
///
enum ErrorInfoType
{
    EI_TYPE_FFDC              = 0,
    EI_TYPE_HW_CALLOUT        = 1,
    EI_TYPE_PROCEDURE_CALLOUT = 2,
    EI_TYPE_BUS_CALLOUT       = 3,
    EI_TYPE_CDG               = 4, // Target Callout/Deconfig/GARD
    EI_TYPE_CHILDREN_CDG      = 5, // Children Callout/Deconfig/GARD
    EI_TYPE_COLLECT_TRACE     = 6,
    EI_LAST_TYPE              = EI_TYPE_COLLECT_TRACE + 1,
};

#ifndef MINIMUM_FFDC
///
/// @enum HwCallout
///
/// This enumeration defines the possible Hardware Callouts that are not
/// represented by fapi2::Targets
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform callout value
/// so do not reorder without consulting all platforms
///
namespace HwCallouts
{
enum HwCallout
{
    // Where indicated, a HW Callout in FAPI Error XML must include a
    // reference target that is used to identify the HW. e.g. for
    // TOD_CLOCK, the proc chip that the clock is attached to must be
    // specified
    TOD_CLOCK           = 0, // Include proc-chip ref (or child chiplet)
    MEM_REF_CLOCK       = 1, // Include membuf-chip ref (or child chiplet)
    PROC_REF_CLOCK      = 2, // Include proc-chip ref (or child chiplet)
    PCI_REF_CLOCK       = 3, // Include proc-chip ref (or child chiplet)
    FLASH_CONTROLLER_PART      = 4,
    PNOR_PART                  = 5,
    SBE_SEEPROM_PART           = 6,
    VPD_PART                   = 7,
    LPC_SLAVE_PART             = 8,
    GPIO_EXPANDER_PART         = 9,
    SPIVID_SLAVE_PART          = 10,
};
}

///
/// @enum ProcedureCallout
///
/// This enumeration defines the possible Procedure Callouts
/// These instruct the customer/customer-engineer what to do
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform callout value
/// so do not reorder without consulting all platforms
///
namespace ProcedureCallouts
{
enum ProcedureCallout
{
    CODE                  = 0, // Code problem
    LVL_SUPPORT           = 1, // Call next level of support
    MEMORY_PLUGGING_ERROR = 2, // DIMM Plugging error
    BUS_CALLOUT           = 3, // Bus Called Out
};
}

///
/// @enum CalloutPriority
///
/// This enumeration defines the possible Procedure and Target callout priorities
///
/// Note that platform code may depend on the enum values starting at 0 and
/// incrementing in order to efficiently convert to a platform priority value
/// so do not reorder without consulting all platforms
///
namespace CalloutPriorities
{
enum CalloutPriority
{
    LOW = 0,
    MEDIUM = 1,
    HIGH   = 2,
    NONE   = 3,
};
}

///
/// @enum CollectTrace
///
/// This enumeration defines the possible firmware traces to collect
///
namespace CollectTraces
{
const uint32_t TRACE_SIZE = 256; // limit collected trace size
enum CollectTrace
{
    FSI              = 1,
    SCOM             = 2,
    SCAN             = 3,
    MBOX             = 4,
};
}

// NOTE - this assumes no buffer_t or variable_buffers are passed
//        data is converted to a uint64_t when placed into the sbe ffdc
//        buffer
inline fapi2::ffdc_t getFfdcData( sbeFfdc_t& i_sbeFfdc )
{
    fapi2::ffdc_t temp;

    temp.size() = static_cast<size_t>(i_sbeFfdc.size);

    if(temp.size() == EI_FFDC_SIZE_TARGET)
    {
#ifdef FAPI2_ENABLE_PLATFORM_GET_TARGET
        uint64_t targetData = i_sbeFfdc.data;
        fapi2::TargetType type = static_cast<fapi2::TargetType>(targetData >> 32);
        uint8_t instance = static_cast<uint8_t>(targetData & 0xFFFFFFFF);
        // call hostboot to get the fapi2 target reference
        temp.ptr() = static_cast<void*>(getTarget<TARGET_TYPE_ALL>(type, instance));
#endif
    }
    else
    {
        // adjust the pointer based on the data size.
        temp.ptr() = static_cast<void*>(reinterpret_cast<uint8_t*>(&i_sbeFfdc.data) +
                                        (sizeof(uint64_t) - i_sbeFfdc.size));
    }

    return temp;
}
#endif
///
/// @brief Get FFDC Size
///
/// This is called by the FAPI_SET_HWP_ERROR macro to find out the size of
/// FFDC data. If the data is of a special type that is handled differently
/// than types that are simply memcopied then it is handled by a template
/// specialization.
/// If this function template is instantiated with a pointer, the compile
/// will fail.
///
/// @return uint16_t. Size of the FFDC data
///
template<typename T>
inline uint16_t getErrorInfoFfdcSize(const T&)
{
    static_assert(sizeof(T) <= EI_FFDC_MAX_SIZE,
                  "FFDC too large to capture");
    return sizeof(T);
}
#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
///
/// @brief Compile error if caller tries to get the FFDC size of a pointer
///
template<typename T>
inline uint16_t getErrorInfoFfdcSize(const T*)
{
    static_assert(std::is_pointer<T>::value,
                  "pointer passed to getErrorInfoFfdcSize");
    return 0;
}
#endif
///
/// @brief Get FFDC Size specialization for fapi2::Target
///
template<fapi2::TargetType T, typename V>
inline uint16_t getErrorInfoFfdcSize(const fapi2::Target<T, V>&)
{
    return EI_FFDC_SIZE_TARGET;
}

#if !defined(MINIMUM_FFDC) && !defined(FAPI2_NO_FFDC)
///
/// @brief Get FFDC Size specialization for variable buffers
///
template<>
inline uint16_t getErrorInfoFfdcSize(const fapi2::variable_buffer& i_thing)
{
    // Limit a variable buffer to 4kb bytes, and we can memcpy the storage.
    return std::min(static_cast<uint32_t>(EI_FFDC_MAX_SIZE),
                    i_thing.getLength<uint8_t>());
}
#endif

///
/// @brief Get FFDC Size specialization for ffdc_t
///
template<>
inline uint16_t getErrorInfoFfdcSize(const fapi2::ffdc_t& i_ffdc)
{
    return static_cast<uint16_t>(i_ffdc.size());
}

};

#endif // FAPI2_ERRORINFO_DEFS_H_
<|endoftext|>


<commit_before>#include "bayesian/graph.hpp"
#include "bayesian/sampling.hpp"

namespace bn {

matrix_type sampling::operator()(
    graph_t const& graph,
    vertex_type const& node,
    std::vector<std::pair<vertex_type, int>> const& condition
    )
{
    // パターン作る(generate_pattern)
    auto const generated_patterns = generate_pattern(graph, 100/*tmp*/, condition);

    // 数え上げを行う
    bn::matrix_type result(node->selectable_num, 1, 0.0);
	for(auto const& pattern : generated_patterns)
	{
		result[pattern.at(node)][0] += 1.0;
	}

	// 全要素をパターン数で割る
	for(std::size_t i = 0; i < node->selectable_num; ++i)
	{
		result[i][0] /= generated_patterns.size();
	}

    return result;
}

sampling::pattern_list sampling::generate_pattern(
    graph_t const& graph,
    int const num,
    std::vector<std::pair<vertex_type, int>> const& condition
    )
{
    // 条件探査用関数
    auto const is_condition = [&condition](std::unordered_map<vertex_type, int> const& pattern) -> bool
        {
            for(auto const& forcus_cond : condition)
            {
                // ノードを探す．見つからなければfalse
                auto position = pattern.find(forcus_cond.first);
                if(position == pattern.cend()) return false;

                // 条件と一致しなければfalse
                if(position->second != forcus_cond.second) return false;
            }
            return true;
        };

    // 最初の要素をseed_node_listから削除したうえで再帰に移譲(当然ノードがなければ失敗)
    auto const seed_node_list = graph.vertex_list();
    if(seed_node_list.empty()) return {};

    // num回ループで生成する
    pattern_list result;
    while(result.size() < static_cast<std::size_t>(num))
    {
        auto node_list = seed_node_list;
		std::unordered_map<vertex_type, int> pattern;

        while(!node_list.empty())
        {
            auto const first_node = node_list.back();
            node_list.pop_back();

            choice_pattern(graph, first_node, node_list, pattern);
        }

		if(is_condition(pattern))
		{
			result.push_back(std::move(pattern));
		}
    }

    return result;
}

void sampling::choice_pattern(
    graph_t const& graph,
    vertex_type const& current,
    std::vector<vertex_type>& remain_node,
    std::unordered_map<vertex_type, int>& pattern
    )
{
    // 上位ノードが決定していることを確認する．
    // 決定していなかった場合は再帰的処理
    std::unordered_map<vertex_type, int> parent_condition;
    for(auto const& edge : graph.in_edges(current))
    {
        auto const parent = graph.source(edge);
        auto const position = std::find(remain_node.begin(), remain_node.end(), parent);
        if(position != remain_node.end())
        {
            // 抜き出して消す
            auto const next_current = *position;
            remain_node.erase(position);

            // 再帰
            choice_pattern(graph, next_current, remain_node, pattern);
        }

        parent_condition[parent] = pattern[parent];
    }

    // CPTから必要データを取得
    auto const optional_cpt_data = current->cpt[parent_condition];
    std::vector<double> condition_probability;
    if(optional_cpt_data.first)
    {
        condition_probability = optional_cpt_data.second;
    }
    else
    {
        throw std::runtime_error("Invalid data from CPT(cannot search data)");
    }

    // メルセンヌ・ツイスタで自身のノード値を生成
    auto const generated_value = probability_generator_();
    double total = 0.0;
    for(int i = 0; current->selectable_num; ++i)
    {
        auto const old_total = total;
        total += condition_probability.at(i);
        if(old_total <= generated_value && generated_value < total)
        {
            pattern[current] = i;
            break;
        }
    }

    return;
}

sampling::probability_generator::probability_generator()
    : distribution_(0.0, 1.0)
{
    std::random_device rand_dev;
    std::vector<std::uint_least32_t> vec(10);
    std::generate(vec.begin(), vec.end(), std::ref(rand_dev));
    std::seed_seq seed(vec.begin(), vec.end());
    engine_.reset(new std::mt19937(seed));
}

double sampling::probability_generator::operator() ()
{
    return distribution_(*engine_);
}

} // namespace bn

<commit_msg>update: edit sample num.<commit_after>#include "bayesian/graph.hpp"
#include "bayesian/sampling.hpp"

namespace bn {

matrix_type sampling::operator()(
    graph_t const& graph,
    vertex_type const& node,
    std::vector<std::pair<vertex_type, int>> const& condition
    )
{
    // パターン作る(generate_pattern)
    auto const generated_patterns = generate_pattern(graph, 10000/*tmp*/, condition);

    // 数え上げを行う
    bn::matrix_type result(node->selectable_num, 1, 0.0);
	for(auto const& pattern : generated_patterns)
	{
		result[pattern.at(node)][0] += 1.0;
	}

	// 全要素をパターン数で割る
	for(std::size_t i = 0; i < node->selectable_num; ++i)
	{
		result[i][0] /= generated_patterns.size();
	}

    return result;
}

sampling::pattern_list sampling::generate_pattern(
    graph_t const& graph,
    int const num,
    std::vector<std::pair<vertex_type, int>> const& condition
    )
{
    // 条件探査用関数
    auto const is_condition = [&condition](std::unordered_map<vertex_type, int> const& pattern) -> bool
        {
            for(auto const& forcus_cond : condition)
            {
                // ノードを探す．見つからなければfalse
                auto position = pattern.find(forcus_cond.first);
                if(position == pattern.cend()) return false;

                // 条件と一致しなければfalse
                if(position->second != forcus_cond.second) return false;
            }
            return true;
        };

    // 最初の要素をseed_node_listから削除したうえで再帰に移譲(当然ノードがなければ失敗)
    auto const seed_node_list = graph.vertex_list();
    if(seed_node_list.empty()) return {};

    // num回ループで生成する
    pattern_list result;
    while(result.size() < static_cast<std::size_t>(num))
    {
        auto node_list = seed_node_list;
		std::unordered_map<vertex_type, int> pattern;

        while(!node_list.empty())
        {
            auto const first_node = node_list.back();
            node_list.pop_back();

            choice_pattern(graph, first_node, node_list, pattern);
        }

		if(is_condition(pattern))
		{
			result.push_back(std::move(pattern));
		}
    }

    return result;
}

void sampling::choice_pattern(
    graph_t const& graph,
    vertex_type const& current,
    std::vector<vertex_type>& remain_node,
    std::unordered_map<vertex_type, int>& pattern
    )
{
    // 上位ノードが決定していることを確認する．
    // 決定していなかった場合は再帰的処理
    std::unordered_map<vertex_type, int> parent_condition;
    for(auto const& edge : graph.in_edges(current))
    {
        auto const parent = graph.source(edge);
        auto const position = std::find(remain_node.begin(), remain_node.end(), parent);
        if(position != remain_node.end())
        {
            // 抜き出して消す
            auto const next_current = *position;
            remain_node.erase(position);

            // 再帰
            choice_pattern(graph, next_current, remain_node, pattern);
        }

        parent_condition[parent] = pattern[parent];
    }

    // CPTから必要データを取得
    auto const optional_cpt_data = current->cpt[parent_condition];
    std::vector<double> condition_probability;
    if(optional_cpt_data.first)
    {
        condition_probability = optional_cpt_data.second;
    }
    else
    {
        throw std::runtime_error("Invalid data from CPT(cannot search data)");
    }

    // メルセンヌ・ツイスタで自身のノード値を生成
    auto const generated_value = probability_generator_();
    double total = 0.0;
    for(int i = 0; current->selectable_num; ++i)
    {
        auto const old_total = total;
        total += condition_probability.at(i);
        if(old_total <= generated_value && generated_value < total)
        {
            pattern[current] = i;
            break;
        }
    }

    return;
}

sampling::probability_generator::probability_generator()
    : distribution_(0.0, 1.0)
{
    std::random_device rand_dev;
    std::vector<std::uint_least32_t> vec(10);
    std::generate(vec.begin(), vec.end(), std::ref(rand_dev));
    std::seed_seq seed(vec.begin(), vec.end());
    engine_.reset(new std::mt19937(seed));
}

double sampling::probability_generator::operator() ()
{
    return distribution_(*engine_);
}

} // namespace bn

<|endoftext|>